From 08a209367b8e32320276b6ef40d955d803fef0ce Mon Sep 17 00:00:00 2001 From: klaxalk Date: Thu, 30 Nov 2023 22:31:46 +0000 Subject: [PATCH] deploy: a2ccb26d82b74fee4e3d3bc6eaebcf875a031e52 --- .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 | 512 + index-sort-l.html | 512 + index.html | 512 + .../attitude_converter.h.func-sort-c.html | 124 + .../mrs_lib/attitude_converter.h.func.html | 124 + .../attitude_converter.h.gcov.frameset.html | 19 + .../mrs_lib/attitude_converter.h.gcov.html | 617 ++ .../attitude_converter.h.gcov.overview.html | 154 + .../mrs_lib/attitude_converter.h.gcov.png | Bin 0 -> 1675 bytes .../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 -> 2162 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 -> 1576 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 | 96 + .../impl/publisher_handler.hpp.func.html | 96 + .../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 -> 769 bytes ...ervice_client_handler.hpp.func-sort-c.html | 116 + .../impl/service_client_handler.hpp.func.html | 116 + ...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 -> 973 bytes .../subscribe_handler.hpp.func-sort-c.html | 196 + .../impl/subscribe_handler.hpp.func.html | 196 + .../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 -> 1399 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 | 228 + .../mrs_lib/impl/transformer.hpp.func.html | 228 + .../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 -> 857 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 | 318 + .../include/mrs_lib/index-detail-sort-l.html | 318 + mrs_lib/include/mrs_lib/index-detail.html | 318 + mrs_lib/include/mrs_lib/index-sort-f.html | 222 + mrs_lib/include/mrs_lib/index-sort-l.html | 222 + mrs_lib/include/mrs_lib/index.html | 222 + .../include/mrs_lib/lkf.h.func-sort-c.html | 168 + mrs_lib/include/mrs_lib/lkf.h.func.html | 168 + .../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 -> 1773 bytes .../mrs_lib/param_loader.h.func-sort-c.html | 280 + .../include/mrs_lib/param_loader.h.func.html | 280 + .../mrs_lib/param_loader.h.gcov.frameset.html | 19 + .../include/mrs_lib/param_loader.h.gcov.html | 1344 +++ .../mrs_lib/param_loader.h.gcov.overview.html | 335 + .../include/mrs_lib/param_loader.h.gcov.png | Bin 0 -> 4201 bytes .../publisher_handler.h.func-sort-c.html | 88 + .../mrs_lib/publisher_handler.h.func.html | 88 + .../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 .../mrs_lib/repredictor.h.func-sort-c.html | 160 + .../include/mrs_lib/repredictor.h.func.html | 160 + .../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 .../service_client_handler.h.func-sort-c.html | 92 + .../service_client_handler.h.func.html | 92 + ...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 | 160 + .../mrs_lib/subscribe_handler.h.func.html | 160 + .../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 -> 1770 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 | 148 + .../include/mrs_lib/transformer.h.func.html | 148 + .../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 -> 2329 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 .../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 -> 1548 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 + .../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 -> 746 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 -> 672 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 .../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 -> 1976 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 | 160 + .../src/automatic_start.cpp.func.html | 160 + .../automatic_start.cpp.gcov.frameset.html | 19 + .../src/automatic_start.cpp.gcov.html | 919 ++ .../automatic_start.cpp.gcov.overview.html | 229 + .../src/automatic_start.cpp.gcov.png | Bin 0 -> 2767 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + mrs_uav_autostart/src/index-detail.html | 110 + mrs_uav_autostart/src/index-sort-f.html | 102 + mrs_uav_autostart/src/index-sort-l.html | 102 + mrs_uav_autostart/src/index.html | 102 + .../include/common.h.func-sort-c.html | 116 + .../include/common.h.func.html | 116 + .../include/common.h.gcov.frameset.html | 19 + .../include/common.h.gcov.html | 189 + .../include/common.h.gcov.overview.html | 47 + mrs_uav_controllers/include/common.h.gcov.png | Bin 0 -> 436 bytes .../include/index-detail-sort-f.html | 128 + .../include/index-detail-sort-l.html | 128 + mrs_uav_controllers/include/index-detail.html | 128 + mrs_uav_controllers/include/index-sort-f.html | 112 + mrs_uav_controllers/include/index-sort-l.html | 112 + mrs_uav_controllers/include/index.html | 112 + .../include/pid.hpp.func-sort-c.html | 100 + mrs_uav_controllers/include/pid.hpp.func.html | 100 + .../include/pid.hpp.gcov.frameset.html | 19 + mrs_uav_controllers/include/pid.hpp.gcov.html | 184 + .../include/pid.hpp.gcov.overview.html | 45 + mrs_uav_controllers/include/pid.hpp.gcov.png | Bin 0 -> 533 bytes .../src/common.cpp.func-sort-c.html | 116 + mrs_uav_controllers/src/common.cpp.func.html | 116 + .../src/common.cpp.gcov.frameset.html | 19 + mrs_uav_controllers/src/common.cpp.gcov.html | 471 + .../src/common.cpp.gcov.overview.html | 117 + mrs_uav_controllers/src/common.cpp.gcov.png | Bin 0 -> 1368 bytes .../failsafe_controller.cpp.func-sort-c.html | 124 + .../src/failsafe_controller.cpp.func.html | 124 + ...failsafe_controller.cpp.gcov.frameset.html | 19 + .../src/failsafe_controller.cpp.gcov.html | 635 ++ ...failsafe_controller.cpp.gcov.overview.html | 158 + .../src/failsafe_controller.cpp.gcov.png | Bin 0 -> 1815 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 -> 6660 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 -> 5841 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 .../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 -> 979 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 + .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../estimation_manager/index-detail.html | 110 + .../estimation_manager/index-sort-f.html | 102 + .../estimation_manager/index-sort-l.html | 102 + .../estimation_manager/index.html | 102 + .../support.h.func-sort-c.html | 140 + .../estimation_manager/support.h.func.html | 140 + .../support.h.gcov.frameset.html | 19 + .../estimation_manager/support.h.gcov.html | 405 + .../support.h.gcov.overview.html | 101 + .../estimation_manager/support.h.gcov.png | Bin 0 -> 1253 bytes .../mrs_uav_managers/index-detail-sort-f.html | 110 + .../mrs_uav_managers/index-detail-sort-l.html | 110 + .../mrs_uav_managers/index-detail.html | 110 + .../mrs_uav_managers/index-sort-f.html | 102 + .../mrs_uav_managers/index-sort-l.html | 102 + .../include/mrs_uav_managers/index.html | 102 + .../tracker.h.func-sort-c.html | 88 + .../mrs_uav_managers/tracker.h.func.html | 88 + .../tracker.h.gcov.frameset.html | 19 + .../mrs_uav_managers/tracker.h.gcov.html | 296 + .../tracker.h.gcov.overview.html | 73 + .../mrs_uav_managers/tracker.h.gcov.png | Bin 0 -> 751 bytes .../index-detail-sort-f.html | 120 + .../index-detail-sort-l.html | 120 + .../transform_manager/index-detail.html | 120 + .../transform_manager/index-sort-f.html | 112 + .../transform_manager/index-sort-l.html | 112 + .../include/transform_manager/index.html | 112 + .../tf_mapping_origin.h.func-sort-c.html | 100 + .../tf_mapping_origin.h.func.html | 100 + .../tf_mapping_origin.h.gcov.frameset.html | 19 + .../tf_mapping_origin.h.gcov.html | 474 + .../tf_mapping_origin.h.gcov.overview.html | 118 + .../tf_mapping_origin.h.gcov.png | Bin 0 -> 1536 bytes .../tf_source.h.func-sort-c.html | 136 + .../transform_manager/tf_source.h.func.html | 136 + .../tf_source.h.gcov.frameset.html | 19 + .../transform_manager/tf_source.h.gcov.html | 701 ++ .../tf_source.h.gcov.overview.html | 175 + .../transform_manager/tf_source.h.gcov.png | Bin 0 -> 2207 bytes .../constraint_manager.cpp.func-sort-c.html | 112 + .../src/constraint_manager.cpp.func.html | 112 + .../constraint_manager.cpp.gcov.frameset.html | 19 + .../src/constraint_manager.cpp.gcov.html | 706 ++ .../constraint_manager.cpp.gcov.overview.html | 176 + .../src/constraint_manager.cpp.gcov.png | Bin 0 -> 2171 bytes .../common/common.cpp.func-sort-c.html | 204 + .../common/common.cpp.func.html | 204 + .../common/common.cpp.gcov.frameset.html | 19 + .../common/common.cpp.gcov.html | 1301 +++ .../common/common.cpp.gcov.overview.html | 325 + .../common/common.cpp.gcov.png | Bin 0 -> 2419 bytes .../common/index-detail-sort-f.html | 110 + .../common/index-detail-sort-l.html | 110 + .../control_manager/common/index-detail.html | 110 + .../control_manager/common/index-sort-f.html | 102 + .../control_manager/common/index-sort-l.html | 102 + .../src/control_manager/common/index.html | 102 + .../control_manager.cpp.func-sort-c.html | 524 + .../control_manager.cpp.func.html | 524 + .../control_manager.cpp.gcov.frameset.html | 19 + .../control_manager.cpp.gcov.html | 8819 +++++++++++++++++ .../control_manager.cpp.gcov.overview.html | 2204 ++++ .../control_manager.cpp.gcov.png | Bin 0 -> 25909 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 -> 346 bytes .../estimation_manager.cpp.func-sort-c.html | 176 + .../estimation_manager.cpp.func.html | 176 + .../estimation_manager.cpp.gcov.frameset.html | 19 + .../estimation_manager.cpp.gcov.html | 1313 +++ .../estimation_manager.cpp.gcov.overview.html | 328 + .../estimation_manager.cpp.gcov.png | Bin 0 -> 4305 bytes .../estimator.cpp.func-sort-c.html | 172 + .../estimator.cpp.func.html | 172 + .../estimator.cpp.gcov.frameset.html | 19 + .../estimator.cpp.gcov.html | 290 + .../estimator.cpp.gcov.overview.html | 72 + .../estimation_manager/estimator.cpp.gcov.png | Bin 0 -> 799 bytes .../agl_estimator.cpp.func-sort-c.html | 92 + .../estimators/agl_estimator.cpp.func.html | 92 + .../agl_estimator.cpp.gcov.frameset.html | 19 + .../estimators/agl_estimator.cpp.gcov.html | 182 + .../agl_estimator.cpp.gcov.overview.html | 45 + .../estimators/agl_estimator.cpp.gcov.png | Bin 0 -> 454 bytes .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimation_manager/estimators/index.html | 112 + .../state_estimator.cpp.func-sort-c.html | 120 + .../estimators/state_estimator.cpp.func.html | 120 + .../state_estimator.cpp.gcov.frameset.html | 19 + .../estimators/state_estimator.cpp.gcov.html | 263 + .../state_estimator.cpp.gcov.overview.html | 65 + .../estimators/state_estimator.cpp.gcov.png | Bin 0 -> 753 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../src/estimation_manager/index-detail.html | 128 + .../src/estimation_manager/index-sort-f.html | 112 + .../src/estimation_manager/index-sort-l.html | 112 + .../src/estimation_manager/index.html | 112 + .../src/gain_manager.cpp.func-sort-c.html | 108 + .../src/gain_manager.cpp.func.html | 108 + .../src/gain_manager.cpp.gcov.frameset.html | 19 + .../src/gain_manager.cpp.gcov.html | 720 ++ .../src/gain_manager.cpp.gcov.overview.html | 179 + .../src/gain_manager.cpp.gcov.png | Bin 0 -> 2109 bytes mrs_uav_managers/src/index-detail-sort-f.html | 164 + mrs_uav_managers/src/index-detail-sort-l.html | 164 + mrs_uav_managers/src/index-detail.html | 164 + mrs_uav_managers/src/index-sort-f.html | 132 + mrs_uav_managers/src/index-sort-l.html | 132 + mrs_uav_managers/src/index.html | 132 + .../src/null_tracker.cpp.func-sort-c.html | 160 + .../src/null_tracker.cpp.func.html | 160 + .../src/null_tracker.cpp.gcov.frameset.html | 19 + .../src/null_tracker.cpp.gcov.html | 332 + .../src/null_tracker.cpp.gcov.overview.html | 82 + .../src/null_tracker.cpp.gcov.png | Bin 0 -> 831 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 -> 3319 bytes .../src/uav_manager.cpp.func-sort-c.html | 232 + .../src/uav_manager.cpp.func.html | 232 + .../src/uav_manager.cpp.gcov.frameset.html | 19 + .../src/uav_manager.cpp.gcov.html | 2732 +++++ .../src/uav_manager.cpp.gcov.overview.html | 682 ++ mrs_uav_managers/src/uav_manager.cpp.gcov.png | Bin 0 -> 7136 bytes .../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 -> 5110 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 | 110 + .../estimators/state/index-detail-sort-l.html | 110 + .../estimators/state/index-detail.html | 110 + .../estimators/state/index-sort-f.html | 102 + .../estimators/state/index-sort-l.html | 102 + .../estimators/state/index.html | 102 + .../state/state_generic.h.func-sort-c.html | 92 + .../state/state_generic.h.func.html | 92 + .../state/state_generic.h.gcov.frameset.html | 19 + .../state/state_generic.h.gcov.html | 183 + .../state/state_generic.h.gcov.overview.html | 45 + .../estimators/state/state_generic.h.gcov.png | Bin 0 -> 438 bytes .../processors/index-detail-sort-f.html | 182 + .../processors/index-detail-sort-l.html | 182 + .../processors/index-detail.html | 182 + .../processors/index-sort-f.html | 142 + .../processors/index-sort-l.html | 142 + .../processors/index.html | 142 + .../proc_excessive_tilt.h.func-sort-c.html | 104 + .../proc_excessive_tilt.h.func.html | 104 + .../proc_excessive_tilt.h.gcov.frameset.html | 19 + .../proc_excessive_tilt.h.gcov.html | 206 + .../proc_excessive_tilt.h.gcov.overview.html | 51 + .../processors/proc_excessive_tilt.h.gcov.png | Bin 0 -> 656 bytes .../proc_median_filter.h.func-sort-c.html | 104 + .../processors/proc_median_filter.h.func.html | 104 + .../proc_median_filter.h.gcov.frameset.html | 19 + .../processors/proc_median_filter.h.gcov.html | 192 + .../proc_median_filter.h.gcov.overview.html | 47 + .../processors/proc_median_filter.h.gcov.png | Bin 0 -> 635 bytes .../proc_saturate.h.func-sort-c.html | 104 + .../processors/proc_saturate.h.func.html | 104 + .../proc_saturate.h.gcov.frameset.html | 19 + .../processors/proc_saturate.h.gcov.html | 202 + .../proc_saturate.h.gcov.overview.html | 50 + .../processors/proc_saturate.h.gcov.png | Bin 0 -> 676 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 -> 685 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 .../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 -> 2783 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 -> 1187 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 -> 2961 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 | 164 + .../estimators/state/index-detail-sort-l.html | 164 + .../src/estimators/state/index-detail.html | 164 + .../src/estimators/state/index-sort-f.html | 132 + .../src/estimators/state/index-sort-l.html | 132 + .../src/estimators/state/index.html | 132 + .../estimators/state/rtk.cpp.func-sort-c.html | 96 + .../src/estimators/state/rtk.cpp.func.html | 96 + .../state/rtk.cpp.gcov.frameset.html | 19 + .../src/estimators/state/rtk.cpp.gcov.html | 109 + .../state/rtk.cpp.gcov.overview.html | 27 + .../src/estimators/state/rtk.cpp.gcov.png | Bin 0 -> 229 bytes .../state/state_generic.cpp.func-sort-c.html | 128 + .../state/state_generic.cpp.func.html | 128 + .../state_generic.cpp.gcov.frameset.html | 19 + .../state/state_generic.cpp.gcov.html | 633 ++ .../state_generic.cpp.gcov.overview.html | 158 + .../state/state_generic.cpp.gcov.png | Bin 0 -> 1960 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 -> 1765 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 -> 4833 bytes .../src/line_tracker/index-detail-sort-f.html | 110 + .../src/line_tracker/index-detail-sort-l.html | 110 + .../src/line_tracker/index-detail.html | 110 + .../src/line_tracker/index-sort-f.html | 102 + .../src/line_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/line_tracker/index.html | 102 + .../line_tracker.cpp.func-sort-c.html | 200 + .../line_tracker/line_tracker.cpp.func.html | 200 + .../line_tracker.cpp.gcov.frameset.html | 19 + .../line_tracker/line_tracker.cpp.gcov.html | 1359 +++ .../line_tracker.cpp.gcov.overview.html | 339 + .../line_tracker/line_tracker.cpp.gcov.png | Bin 0 -> 3773 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../index-detail.html | 110 + .../index-sort-f.html | 102 + .../index-sort-l.html | 102 + .../src/midair_activation_tracker/index.html | 102 + ...ir_activation_tracker.cpp.func-sort-c.html | 152 + .../midair_activation_tracker.cpp.func.html | 152 + ..._activation_tracker.cpp.gcov.frameset.html | 19 + .../midair_activation_tracker.cpp.gcov.html | 426 + ..._activation_tracker.cpp.gcov.overview.html | 106 + .../midair_activation_tracker.cpp.gcov.png | Bin 0 -> 1156 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 -> 12690 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 -> 3032 bytes ruby.png | Bin 0 -> 141 bytes snow.png | Bin 0 -> 141 bytes updown.png | Bin 0 -> 117 bytes 792 files changed, 132732 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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/rtk.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png create mode 100644 mrs_uav_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 ruby.png create mode 100644 snow.png create mode 100644 updown.png diff --git a/.nojekyll b/.nojekyll new file mode 100644 index 0000000000..e69de29bb2 diff --git a/amber.png b/amber.png new file mode 100644 index 0000000000000000000000000000000000000000..2cab170d8359081983a4e343848dfe06bc490f12 GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^G2tW}LqE04T&+ z;1OBOz`!j8!i<;h*8KqrvZOouIx;Y9?C1WI$O`1M1^9%x{(levWGtest coveragetest coverage61.3%61.3% \ No newline at end of file diff --git a/emerald.png b/emerald.png new file mode 100644 index 0000000000000000000000000000000000000000..38ad4f4068b935643d2486f323005fb294a9bd7e GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^Jb!lvI6;R0X`wF(yt=9xVZRt1vCRixIA4P dLn>}1Cji+@42)0J?}79&c)I$ztaD0e0sy@GAL0N2 literal 0 HcmV?d00001 diff --git a/gcov.css b/gcov.css new file mode 100644 index 0000000000..bfd0a83e10 --- /dev/null +++ b/gcov.css @@ -0,0 +1,519 @@ +/* All views: initial background and text color */ +body +{ + color: #000000; + background-color: #FFFFFF; +} + +/* All views: standard link format*/ +a:link +{ + color: #284FA8; + text-decoration: underline; +} + +/* All views: standard link - visited format */ +a:visited +{ + color: #00CB40; + text-decoration: underline; +} + +/* All views: standard link - activated format */ +a:active +{ + color: #FF0040; + text-decoration: underline; +} + +/* All views: main title format */ +td.title +{ + text-align: center; + padding-bottom: 10px; + font-family: sans-serif; + font-size: 20pt; + font-style: italic; + font-weight: bold; +} + +/* All views: header item format */ +td.headerItem +{ + text-align: right; + padding-right: 6px; + font-family: sans-serif; + font-weight: bold; + vertical-align: top; + white-space: nowrap; +} + +/* All views: header item value format */ +td.headerValue +{ + text-align: left; + color: #284FA8; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; +} + +/* All views: header item coverage table heading */ +td.headerCovTableHead +{ + text-align: center; + padding-right: 6px; + padding-left: 6px; + padding-bottom: 0px; + font-family: sans-serif; + font-size: 80%; + white-space: nowrap; +} + +/* All views: header item coverage table entry */ +td.headerCovTableEntry +{ + text-align: right; + color: #284FA8; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #DAE7FE; +} + +/* All views: header item coverage table entry for high coverage rate */ +td.headerCovTableEntryHi +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #A7FC9D; +} + +/* All views: header item coverage table entry for medium coverage rate */ +td.headerCovTableEntryMed +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #FFEA20; +} + +/* All views: header item coverage table entry for ow coverage rate */ +td.headerCovTableEntryLo +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #FF0000; +} + +/* All views: header legend value for legend entry */ +td.headerValueLeg +{ + text-align: left; + color: #000000; + font-family: sans-serif; + font-size: 80%; + white-space: nowrap; + padding-top: 4px; +} + +/* All views: color of horizontal ruler */ +td.ruler +{ + background-color: #6688D4; +} + +/* All views: version string format */ +td.versionInfo +{ + text-align: center; + padding-top: 2px; + font-family: sans-serif; + font-style: italic; +} + +/* Directory view/File view (all)/Test case descriptions: + table headline format */ +td.tableHead +{ + text-align: center; + color: #FFFFFF; + background-color: #6688D4; + font-family: sans-serif; + font-size: 120%; + font-weight: bold; + white-space: nowrap; + padding-left: 4px; + padding-right: 4px; +} + +span.tableHeadSort +{ + padding-right: 4px; +} + +/* Directory view/File view (all): filename entry format */ +td.coverFile +{ + text-align: left; + padding-left: 10px; + padding-right: 20px; + color: #284FA8; + background-color: #DAE7FE; + font-family: monospace; +} + +/* Directory view/File view (all): bar-graph entry format*/ +td.coverBar +{ + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; +} + +/* Directory view/File view (all): bar-graph outline color */ +td.coverBarOutline +{ + background-color: #000000; +} + +/* Directory view/File view (all): percentage entry for files with + high coverage rate */ +td.coverPerHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #A7FC9D; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + high coverage rate */ +td.coverNumHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #A7FC9D; + white-space: nowrap; + font-family: sans-serif; +} + +/* Directory view/File view (all): percentage entry for files with + medium coverage rate */ +td.coverPerMed +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FFEA20; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + medium coverage rate */ +td.coverNumMed +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FFEA20; + white-space: nowrap; + font-family: sans-serif; +} + +/* Directory view/File view (all): percentage entry for files with + low coverage rate */ +td.coverPerLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + low coverage rate */ +td.coverNumLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + white-space: nowrap; + font-family: sans-serif; +} + +/* File view (all): "show/hide details" link format */ +a.detail:link +{ + color: #B8D0FF; + font-size:80%; +} + +/* File view (all): "show/hide details" link - visited format */ +a.detail:visited +{ + color: #B8D0FF; + font-size:80%; +} + +/* File view (all): "show/hide details" link - activated format */ +a.detail:active +{ + color: #FFFFFF; + font-size:80%; +} + +/* File view (detail): test name entry */ +td.testName +{ + text-align: right; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* File view (detail): test percentage entry */ +td.testPer +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* File view (detail): test lines count entry */ +td.testNum +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* Test case descriptions: test name format*/ +dt +{ + font-family: sans-serif; + font-weight: bold; +} + +/* Test case descriptions: description table body */ +td.testDescription +{ + padding-top: 10px; + padding-left: 30px; + padding-bottom: 10px; + padding-right: 30px; + background-color: #DAE7FE; +} + +/* Source code view: function entry */ +td.coverFn +{ + text-align: left; + padding-left: 10px; + padding-right: 20px; + color: #284FA8; + background-color: #DAE7FE; + font-family: monospace; +} + +/* Source code view: function entry zero count*/ +td.coverFnLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + font-weight: bold; + font-family: sans-serif; +} + +/* Source code view: function entry nonzero count*/ +td.coverFnHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-weight: bold; + font-family: sans-serif; +} + +/* Source code view: source code format */ +pre.source +{ + font-family: monospace; + white-space: pre; + margin-top: 2px; +} + +/* Source code view: line number format */ +span.lineNum +{ + background-color: #EFE383; +} + +/* Source code view: format for lines which were executed */ +td.lineCov, +span.lineCov +{ + background-color: #CAD7FE; +} + +/* Source code view: format for Cov legend */ +span.coverLegendCov +{ + padding-left: 10px; + padding-right: 10px; + padding-bottom: 2px; + background-color: #CAD7FE; +} + +/* Source code view: format for lines which were not executed */ +td.lineNoCov, +span.lineNoCov +{ + background-color: #FF6230; +} + +/* Source code view: format for NoCov legend */ +span.coverLegendNoCov +{ + padding-left: 10px; + padding-right: 10px; + padding-bottom: 2px; + background-color: #FF6230; +} + +/* Source code view (function table): standard link - visited format */ +td.lineNoCov > a:visited, +td.lineCov > a:visited +{ + color: black; + text-decoration: underline; +} + +/* Source code view: format for lines which were executed only in a + previous version */ +span.lineDiffCov +{ + background-color: #B5F7AF; +} + +/* Source code view: format for branches which were executed + * and taken */ +span.branchCov +{ + background-color: #CAD7FE; +} + +/* Source code view: format for branches which were executed + * but not taken */ +span.branchNoCov +{ + background-color: #FF6230; +} + +/* Source code view: format for branches which were not executed */ +span.branchNoExec +{ + background-color: #FF6230; +} + +/* Source code view: format for the source code heading line */ +pre.sourceHeading +{ + white-space: pre; + font-family: monospace; + font-weight: bold; + margin: 0px; +} + +/* All views: header legend value for low rate */ +td.headerValueLegL +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 4px; + padding-right: 2px; + background-color: #FF0000; + font-size: 80%; +} + +/* All views: header legend value for med rate */ +td.headerValueLegM +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 2px; + padding-right: 2px; + background-color: #FFEA20; + font-size: 80%; +} + +/* All views: header legend value for hi rate */ +td.headerValueLegH +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 2px; + padding-right: 4px; + background-color: #A7FC9D; + font-size: 80%; +} + +/* All views except source code view: legend format for low coverage */ +span.coverLegendCovLo +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #FF0000; +} + +/* All views except source code view: legend format for med coverage */ +span.coverLegendCovMed +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #FFEA20; +} + +/* All views except source code view: legend format for hi coverage */ +span.coverLegendCovHi +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #A7FC9D; +} diff --git a/glass.png b/glass.png new file mode 100644 index 0000000000000000000000000000000000000000..e1abc00680a3093c49fdb775ae6bdb6764c95af2 GIT binary patch literal 167 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)gaEa{HEjtmSN`?>!lvI6;R0X`wF z|Ns97GD8ntt^-nxB|(0{3=Yq3q=7g|-tI089jvk*Kn`btM`SSr1Gf+eGhVt|_XjA* zUgGKN%6^Gmn4d%Ph(nkFP>9RZ#WAE}PI3Z}&BVayv3^M*kj3EX>gTe~DWM4f=_Dpv literal 0 HcmV?d00001 diff --git a/index-sort-f.html b/index-sort-f.html new file mode 100644 index 0000000000..968ec522c3 --- /dev/null +++ b/index-sort-f.html @@ -0,0 +1,512 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:105231860656.6 %
Date:2023-11-30 22:31:42Functions:936152761.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/src/geometry +
14.2%14.2%
+ 		14.2 %63 / 44513.8 %13 / 94
mrs_uav_managers/include/mrs_uav_managers/control_manager +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 27
mrs_uav_state_estimators/src/estimators/heading +
18.2%18.2%
+ 		18.2 %85 / 46622.2 %12 / 54
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 430.0 %3 / 10
mrs_uav_trackers/src/speed_tracker +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 19
mrs_uav_trackers/src/joy_tracker +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 18
mrs_uav_managers/src/control_manager/common +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 31
mrs_lib/include/mrs_lib/geometry +
69.9%69.9%
+ 		69.9 %65 / 9339.2 %47 / 120
mrs_uav_trackers/src/midair_activation_tracker +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 18
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
67.2%67.2%
+ 		67.2 %117 / 17450.0 %17 / 34
mrs_uav_trackers/src/mpc_tracker +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 49
mrs_uav_managers/src/control_manager +
45.8%45.8%
+ 		45.8 %1701 / 371251.2 %63 / 123
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_managers/include/transform_manager +
47.1%47.1%
+ 		47.1 %186 / 39557.9 %11 / 19
mrs_uav_trackers/src/landoff_tracker +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 31
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
mrs_lib/src/transformer +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 26
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
mrs_lib/include/mrs_lib/impl +
72.1%72.1%
+ 		72.1 %289 / 40168.2 %75 / 110
mrs_uav_managers/src +
55.1%55.1%
+ 		55.1 %931 / 169072.6 %53 / 73
mrs_uav_state_estimators/src/estimators/altitude +
55.5%55.5%
+ 		55.5 %206 / 37173.3 %22 / 30
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
57.0%57.0%
+ 		57.0 %394 / 69174.7 %68 / 91
mrs_uav_managers/src/transform_manager +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 13
mrs_uav_managers/src/estimation_manager +
63.8%63.8%
+ 		63.8 %391 / 61378.7 %37 / 47
mrs_uav_state_estimators/src/estimators/state +
67.2%67.2%
+ 		67.2 %207 / 30879.2 %19 / 24
mrs_uav_autostart/src +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
mrs_lib/include/mrs_lib +
78.7%78.7%
+ 		78.7 %570 / 72481.0 %132 / 163
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/src/estimators/lateral +
57.5%57.5%
+ 		57.5 %233 / 40583.3 %25 / 30
mrs_uav_controllers/src +
80.1%80.1%
+ 		80.1 %1740 / 217283.3 %55 / 66
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
mrs_lib/src/attitude_converter +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 41
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
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_lib/src/param_loader +
70.9%70.9%
+ 		70.9 %39 / 55100.0 %4 / 4
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
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index-sort-l.html b/index-sort-l.html new file mode 100644 index 0000000000..64dc5a9866 --- /dev/null +++ b/index-sort-l.html @@ -0,0 +1,512 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:105231860656.6 %
Date:2023-11-30 22:31:42Functions:936152761.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_uav_managers/include/mrs_uav_managers/control_manager +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 27
mrs_uav_trackers/src/speed_tracker +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 19
mrs_lib/src/geometry +
14.2%14.2%
+ 		14.2 %63 / 44513.8 %13 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.2%18.2%
+ 		18.2 %85 / 46622.2 %12 / 54
mrs_uav_managers/src/control_manager/common +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 31
mrs_uav_trackers/src/joy_tracker +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 18
mrs_uav_managers/src/control_manager +
45.8%45.8%
+ 		45.8 %1701 / 371251.2 %63 / 123
mrs_uav_managers/include/transform_manager +
47.1%47.1%
+ 		47.1 %186 / 39557.9 %11 / 19
mrs_uav_trackers/src/mpc_tracker +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 49
mrs_uav_managers/src +
55.1%55.1%
+ 		55.1 %931 / 169072.6 %53 / 73
mrs_uav_state_estimators/src/estimators/altitude +
55.5%55.5%
+ 		55.5 %206 / 37173.3 %22 / 30
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
57.0%57.0%
+ 		57.0 %394 / 69174.7 %68 / 91
mrs_uav_state_estimators/src/estimators/lateral +
57.5%57.5%
+ 		57.5 %233 / 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_lib/src/transformer +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 26
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_managers/src/transform_manager +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 13
mrs_uav_managers/src/estimation_manager +
63.8%63.8%
+ 		63.8 %391 / 61378.7 %37 / 47
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
67.2%67.2%
+ 		67.2 %117 / 17450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/state +
67.2%67.2%
+ 		67.2 %207 / 30879.2 %19 / 24
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
mrs_uav_trackers/src/landoff_tracker +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 31
mrs_lib/include/mrs_lib/geometry +
69.9%69.9%
+ 		69.9 %65 / 9339.2 %47 / 120
mrs_uav_trackers/src/midair_activation_tracker +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 18
mrs_uav_autostart/src +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
mrs_lib/src/param_loader +
70.9%70.9%
+ 		70.9 %39 / 55100.0 %4 / 4
mrs_lib/include/mrs_lib/impl +
72.1%72.1%
+ 		72.1 %289 / 40168.2 %75 / 110
mrs_lib/include/mrs_lib +
78.7%78.7%
+ 		78.7 %570 / 72481.0 %132 / 163
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_uav_controllers/src +
80.1%80.1%
+ 		80.1 %1740 / 217283.3 %55 / 66
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/attitude_converter +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 41
mrs_lib/src/timer +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
mrs_uav_controllers/include +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 430.0 %3 / 10
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index.html b/index.html new file mode 100644 index 0000000000..4f9087d303 --- /dev/null +++ b/index.html @@ -0,0 +1,512 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:105231860656.6 %
Date:2023-11-30 22:31:42Functions:936152761.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib +
78.7%78.7%
+ 		78.7 %570 / 72481.0 %132 / 163
mrs_lib/include/mrs_lib/geometry +
69.9%69.9%
+ 		69.9 %65 / 9339.2 %47 / 120
mrs_lib/include/mrs_lib/impl +
72.1%72.1%
+ 		72.1 %289 / 40168.2 %75 / 110
mrs_lib/src/attitude_converter +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 41
mrs_lib/src/geometry +
14.2%14.2%
+ 		14.2 %63 / 44513.8 %13 / 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 +
70.9%70.9%
+ 		70.9 %39 / 55100.0 %4 / 4
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/transformer +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 26
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_autostart/src +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
mrs_uav_controllers/include +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_uav_controllers/src +
80.1%80.1%
+ 		80.1 %1740 / 217283.3 %55 / 66
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 430.0 %3 / 10
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
mrs_uav_managers/include/mrs_uav_managers/control_manager +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 27
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
mrs_uav_managers/include/transform_manager +
47.1%47.1%
+ 		47.1 %186 / 39557.9 %11 / 19
mrs_uav_managers/src +
55.1%55.1%
+ 		55.1 %931 / 169072.6 %53 / 73
mrs_uav_managers/src/control_manager +
45.8%45.8%
+ 		45.8 %1701 / 371251.2 %63 / 123
mrs_uav_managers/src/control_manager/common +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 31
mrs_uav_managers/src/estimation_manager +
63.8%63.8%
+ 		63.8 %391 / 61378.7 %37 / 47
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_managers/src/transform_manager +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 13
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
57.0%57.0%
+ 		57.0 %394 / 69174.7 %68 / 91
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 %5 / 566.7 %2 / 3
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
67.2%67.2%
+ 		67.2 %117 / 17450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/altitude +
55.5%55.5%
+ 		55.5 %206 / 37173.3 %22 / 30
mrs_uav_state_estimators/src/estimators/heading +
18.2%18.2%
+ 		18.2 %85 / 46622.2 %12 / 54
mrs_uav_state_estimators/src/estimators/lateral +
57.5%57.5%
+ 		57.5 %233 / 40583.3 %25 / 30
mrs_uav_state_estimators/src/estimators/state +
67.2%67.2%
+ 		67.2 %207 / 30879.2 %19 / 24
mrs_uav_trackers/src/joy_tracker +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 18
mrs_uav_trackers/src/landoff_tracker +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 31
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
mrs_uav_trackers/src/midair_activation_tracker +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 18
mrs_uav_trackers/src/mpc_tracker +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 49
mrs_uav_trackers/src/speed_tracker +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 19
+
+
+ + + + +
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..946657cdf6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2023-11-30 22:31:42Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5645ea2bb184ba5c13faadff6ae080a7ed21a006 GIT binary patch literal 1675 zcmV;626Xv}P)R0{{R3v^8aD0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp^O~xJw}!I*vg`@baJIpOoIavV`$(K?>!`bo|a^C) za8g(t(GB3SHwi*Y*B(ABA)4cq7R?deK-XjKEeJ~&vr9LR}k6~|SE zj-$qCkGRR)tetw3kCxx*!;6UEC@RHf4BS;>N2h>L$)pCW@@QNm!OX>*`BO)Lk71D%>OnE>K^hjQTBMES? zGvhv}xdH-50z4|BgOy^!kpa(GH^`~foRA8S^Y>UW#vJMJOe}{A4}y>ij~qH^H4MF) zneqsxd6A=h9u5{hcDe;P(zMKk1rd-Ue&^knPPR3jA9^gb>#8x~Ik7N~R1Fdq@=|ju z0T!S8M{bY&wr%zW06bfshHfN{MDlGm(rkM>62Wo>ikiawavEnFLStBy1HdIWEFe%Ww^Ngwm;Hcq)jl z0*{0W;oTtCZ~llxle+lG>3#O+D&&gJrG@=)lao1h2seK7gq0SK9kjuxg-NrKGR!%~ z`au99dU_T5N&6~=-sZqq>k<~8`<@qW?#HHsHY0qrv%R~ejG3dmdiYQNZF4O4*G{-$ z^G7N8QUf@SO&a1DH?d4h1F$$gpRd=K`+0r3pV#NJ)po+CrwQ%T-SgEVtiNn>JQv4Q z$!a>`qaW=;`3?H`0rb(Ur2TajeM}|3XxAlSQK}p$B;JRoy~&s`4k)2hNq1Dm2(-&Z z%ZP0UeT*pTteufi+CMJX^?uWrH*Wvgee-&=U5j%JyB%`DkeX;_W9brNyMAW@LV=NF z`vbMAFhtINPYBIhx=Oy)`A1x1t=K11!#H_Sq=yYc#`?Mxm2Dy{cz^))?R~`tEg7 zo(>_l>s;xc@cdbu``xNXAjEdvf~QT$fk&n`v;a?Mx|iT-@?_UO)>ViPPuH7ayKcbK zIe5W=N5r<4w5LOe?HZq((Kwy}I4+3;`PQSeHB3yo++}$V=9qWo+^i(35sOuwsIM>rafH+-<*caYfll7-k8qBGOLd1CnUsn zot2`+1`_h(bt-s#>%p`$vc0^nk+7M>5i7}n<3->Z68->q@ErZ91s_QKmCsa&B%_`q z1D@+YQz7;U=C6FFLhb=op7xZL`xqZRZtpz_n1U--iU~*Zp}2TpDV}{(xy0affm7%5 z+W7_^4dR!LZX*O5l2^DZ1 z?A~WeTGw4u;hC=z$ot`PUf>U&F~>i6h8%ghnJNAcf#T%Hdy|jS=_H)EsjMJ8?!*5I z51!+@7iT$1QUD{R;|$UhBs~Y1;Zi2XPgaWJl!jXIElj|TK46~NkO7}ca2>#x;XcI* z^CV0j8!}-zAteJI3FZ@GU`1FF5~pzC_+7=3X)_ASQIrJ;gUR_iSNsD<>R>{U1Y^?W zvYR!Sg}4SeuBe%R`Cnmz$V;iPVjSJHb9JE2c3kC8_@f)zqlD5AEDo&}n_5NIC62c= zqkgYRb;F6tx*!Fl3jb2?u8vgi2?nd`J-wb + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/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:598767.8 %
Date:2023-11-30 22:31:42Functions:4411737.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::sdegrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::sradians const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::value() const0
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::convert<mrs_lib::geometry::radians>() const1
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::value() const4
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>&&)6
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::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>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)10000
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::radians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)10008
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)10008
mrs_lib::geometry::operator-(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10009
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(double)20000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)20001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)20063
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>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)40018
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)40024
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)40042
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>)40071
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)110035
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)130040
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)180139
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)240139
+
+
+ + + +
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..5ba5c08a7a --- /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:598767.8 %
Date:2023-11-30 22:31:42Functions:4411737.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)40018
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)20001
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>)10008
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)130040
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)10000
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&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)40042
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)110035
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)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)10000
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>)40071
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)20063
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)240139
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)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)10000
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&)40024
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)180139
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..f72ebd16bd --- /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:598767.8 %
Date:2023-11-30 22:31:42Functions:4411737.6 %
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      330179 :       cyclic(const flt val) : val(wrap(val)){};
+      62             :       /*!
+      63             :        * \brief Copy constructor.
+      64             :        *
+      65             :        * \param val initialization value.
+      66             :        */
+      67       80066 :       cyclic(const cyclic& other) : val(other.val){};
+      68             :       /*!
+      69             :        * \brief Copy constructor.
+      70             :        *
+      71             :        * \param val initialization value.
+      72             :        */
+      73           0 :       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      430184 :       static flt wrap(const flt val)
+     115             :       {
+     116             :         // these few ifs should cover most cases, improving speed and precision
+     117      430184 :         if (val >= minimum)
+     118             :         {
+     119      260004 :           if (val < supremum)  // value is actually in range and doesn't need to be wrapped
+     120       86318 :             return val;
+     121      173686 :           else if (val < supremum + range)
+     122       11202 :             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      170180 :           if (val >= minimum - range)
+     126        6246 :             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      326418 :         const flt rem = std::fmod(val - minimum, range);
+     131      326418 :         const flt wrapped = rem + minimum + std::signbit(rem) * range;
+     132      326418 :         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       40000 :       static flt unwrap(const flt what, const flt from)
+     158             :       {
+     159       40000 :         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       60064 :       static flt diff(const flt minuend, const flt subtrahend)
+     194             :       {
+     195       60064 :         return diff(cyclic(minuend), cyclic(subtrahend));
+     196             :       }
+     197             : 
+     198      100089 :       static flt diff(const cyclic minuend, const cyclic subtrahend)
+     199             :       {
+     200      100089 :         const flt d = minuend.val - subtrahend.val;
+     201      100089 :         if (d < -half_range)
+     202       12212 :           return d + range;
+     203       87877 :         if (d >= half_range)
+     204       12611 :           return d - range;
+     205       75266 :         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       20000 :       static flt dist(const flt from, const flt to)
+     220             :       {
+     221       20000 :         return dist(cyclic(from), cyclic(to));
+     222             :       }
+     223             : 
+     224       20016 :       static flt dist(const cyclic from, const cyclic to)
+     225             :       {
+     226       20016 :         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       10000 :       static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
+     243             :       {
+     244       10000 :         return interpUnwrapped(cyclic(from), cyclic(to), coeff);
+     245             :       }
+     246             : 
+     247       10000 :       static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     248             :       {
+     249       10000 :         const flt dang = diff(to, from);
+     250       10000 :         const flt intp = from.val + coeff * dang;
+     251       10000 :         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       10000 :       static flt interp(const flt from, const flt to, const flt coeff)
+     265             :       {
+     266       10000 :         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..2bc61fa45a83080d877d7955295bae3078fed0ad GIT binary patch literal 2162 zcmV-&2#xoNP)zB+G2P&NG9k9Ocm$pz;3(-AMo&1ML*9IP@1q$AP(I~63?2!Z_fu~q@jDvtKuvL262yvhGlK>s5l;fkvS4U<{ zfL=5v0n}5>WP5aIh#rlX)#9}@$;^(K`ILA98=@#E?H>LEfQ}W2&LAd)W+u##EPF}J z#th>lD|aY&4LBO4#E~(Uw7wV$8jbbANP1WZ*g}O4Hch~xJ$`+CeZSV@@txLJV*%i^ z^{EC_T$8_^!Zk%N@@%x$9r7`*Z@1eW*82LlZFgMTZMOihI9geX>*F{u&vgr{nQM5o z82|s`8ZG=~L z9e@MrA&izKS-}OWHKVAq1YZJjo1|K)RO9A=eKiFl$ymH(X3vFJPWnZ=hmopp^fO;z zwTVa9ZBk9rW)t5wjAmExRBG%*L*g+xt+jMZp6W0N&9A@=v^%%~aHs;f5lW~fwl$`aggrj5SZZwd1UQfv z5aT^!S^-hS+yq2Xu8H=VYMWHu;QKW((~`2r7}ux@l^}t7hne9$9`4YX3m5dQK2K?f z(GaO1Ot0%s?g|h5ZDlhb>mDhB2N{|1WIzKhX%zB$O5bT>T)2Sh&n#Yh4AH}0anf~zB?E>C z*l(NtDBw%-NcyZs90W$QB~fkfQtsK!YMP$H|7FQN+;@Y zW!*F!&TvnZ*AHylAK1G;cIEIyc}?q4n*e;Iyxubtdxi9u^J8TuHXSg_s5c5ZU?cPatcdxHjq7~q52DV@R4 zHARvEvz5>AQ9!e8%3Vr&RT)mia9;9u+X0>&!*|SJZ7OexA1gDl3f@%BTvoi;bJ52K z7-5&~ej>xTSH!y*W6#xwVZcjdSKsyjiGsc3|8x0Th6Ig@nDR9h@zkSN0psyNHHDzv zRRESfQrW6QX|d1QNP)#(#TDivMay~qqe!u0vpyH3_RPe0Ex-#nS15dpD_YEjqokeF zekury9sRI#$%Kq0PzNM0EHf)Ql$n{IZ_dynS5EGf?><> zpM0|Yf$b+eeZSSQ(|NXCk9v`ow3;-hEgUiBLnKL}UT1~B?Am7LC$ekq3SzhD1?&v( zxxzP|dw!}Z*2jleZRegDUg1FMQ5nP40OPC+9&Pa)lm6!;#a|lUt*_j|^{^RVlEwW- zcijz}g`YQ>Nd%sIz)Lt6&4jeMO%8^m)S5Ka%@$7K!=OTd9J{=VD1qgl{LgwhH>c(Lk`i~7CbQC5O;^p-#1#Xl z%QR;vTezU}5_d_L-iqKwjoU#ZX7_YY{QcnEsL+Lm2BW&s&FCK zZeN2mrNVCPpj}if6FgWp^CUX7Cs^3VsUT`*ru2Pe&7xWv%B^hGXNYV zUFf<8_&Hs#E#Xh;dhMNO|8=_V_v>%@bz?KXN!MGYfZ756B3&Ov@yT2AdKd0yfV~PJ z83j$Q2}R%UH8t?en{7cwH8DAF~AdlRD}o+0;(x@esPhSGbF%K(s1hmUlvKf;eZ1KM@MWJ zKV{(gFq*EX_Bdcz>_dehLy9UG+$g2oK;2ZN{~HZ=$Zl_Hz@}zmR07F?Z5*RvUCYdH z1QzH#Ng8VWnb|*WW^^P+F>}vpnlvT3mm9L(oVqMHfveY03Gfp?tP(^(ft*FvBbeb$ zzk0 + + + + + + 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:659369.9 %
Date:2023-11-30 22:31:42Functions:4712039.2 %
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 +
67.8%67.8%
+ 		67.8 %59 / 8737.6 %44 / 117
<unnamed>67.8 %59 / 8737.6 %44 / 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..aad1840638 --- /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:659369.9 %
Date:2023-11-30 22:31:42Functions:4712039.2 %
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 +
67.8%67.8%
+ 		67.8 %59 / 8737.6 %44 / 117
<unnamed>67.8 %59 / 8737.6 %44 / 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..34f1b7c61f --- /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:659369.9 %
Date:2023-11-30 22:31:42Functions:4712039.2 %
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 +
67.8%67.8%
+ 		67.8 %59 / 8737.6 %44 / 117
<unnamed>67.8 %59 / 8737.6 %44 / 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..0ed314636d --- /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:659369.9 %
Date:2023-11-30 22:31:42Functions:4712039.2 %
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 +
67.8%67.8%
+ 		67.8 %59 / 8737.6 %44 / 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..750a22ecc4 --- /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:659369.9 %
Date:2023-11-30 22:31:42Functions:4712039.2 %
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 +
67.8%67.8%
+ 		67.8 %59 / 8737.6 %44 / 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..2b9c94db24 --- /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:659369.9 %
Date:2023-11-30 22:31:42Functions:4712039.2 %
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 +
67.8%67.8%
+ 		67.8 %59 / 8737.6 %44 / 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..ace401bbff --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2023-11-30 22:31:42Functions: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&)6
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.func.html b/mrs_lib/include/mrs_lib/geometry/misc.h.func.html new file mode 100644 index 0000000000..01eded6cf5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2023-11-30 22:31:42Functions: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&)6
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..beda3bb152 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html @@ -0,0 +1,408 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2023-11-30 22:31:42Functions: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          12 :     double headingFromRot(const T& rot)
+      58             :     {
+      59          12 :       const vec3_t rot_vec = rot*vec3_t::UnitX();
+      60          24 :       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:9213667.6 %
Date:2023-11-30 22:31:42Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMLetterDesignator(double)2
mrs_lib::LLtoUTM(double, double, double&, double&, char*)2
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)2
mrs_lib::UTM(double, double, double*, double*)4
+
+
+ + + +
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..1d0b17dd83 --- /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:9213667.6 %
Date:2023-11-30 22:31:42Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMLetterDesignator(double)2
mrs_lib::UTM(double, double, double*, double*)4
mrs_lib::LLtoUTM(double, double, double&, double&, char*)2
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)2
+
+
+ + + +
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..f6f11d588a --- /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:9213667.6 %
Date:2023-11-30 22:31:42Functions: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           4 :   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           4 :     int cm = ((lon >= 0.0) ? ((int)lon - ((int)lon) % 6 + 3) : ((int)lon - ((int)lon) % 6 - 3));
+      63             : 
+      64             :     // convert degrees into radians
+      65           4 :     double rlat  = lat * RADIANS_PER_DEGREE;
+      66           4 :     double rlon  = lon * RADIANS_PER_DEGREE;
+      67           4 :     double rlon0 = cm * RADIANS_PER_DEGREE;
+      68             : 
+      69             :     // compute trigonometric functions
+      70           4 :     double slat = sin(rlat);
+      71           4 :     double clat = cos(rlat);
+      72           4 :     double tlat = tan(rlat);
+      73             : 
+      74             :     // decide the false northing at origin
+      75           4 :     double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
+      76             : 
+      77           4 :     double T = tlat * tlat;
+      78           4 :     double C = UTM_EP2 * clat * clat;
+      79           4 :     double A = (rlon - rlon0) * clat;
+      80           4 :     double M = WGS84_A * (m0 * rlat + m1 * sin(2 * rlat) + m2 * sin(4 * rlat) + m3 * sin(6 * rlat));
+      81           4 :     double V = WGS84_A / sqrt(1 - UTM_E2 * slat * slat);
+      82             : 
+      83             :     // compute the easting-northing coordinates
+      84           4 :     *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           4 :     *y = fn +
+      86           4 :          UTM_K0 *
+      87           4 :              (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           4 :     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           2 :   static inline char UTMLetterDesignator(double Lat) {
+     102             :     char LetterDesignator;
+     103             : 
+     104           2 :     if ((84 >= Lat) && (Lat >= 72))
+     105           0 :       LetterDesignator = 'X';
+     106           2 :     else if ((72 > Lat) && (Lat >= 64))
+     107           0 :       LetterDesignator = 'W';
+     108           2 :     else if ((64 > Lat) && (Lat >= 56))
+     109           0 :       LetterDesignator = 'V';
+     110           2 :     else if ((56 > Lat) && (Lat >= 48))
+     111           0 :       LetterDesignator = 'U';
+     112           2 :     else if ((48 > Lat) && (Lat >= 40))
+     113           0 :       LetterDesignator = 'T';
+     114           2 :     else if ((40 > Lat) && (Lat >= 32))
+     115           0 :       LetterDesignator = 'S';
+     116           2 :     else if ((32 > Lat) && (Lat >= 24))
+     117           0 :       LetterDesignator = 'R';
+     118           2 :     else if ((24 > Lat) && (Lat >= 16))
+     119           0 :       LetterDesignator = 'Q';
+     120           2 :     else if ((16 > Lat) && (Lat >= 8))
+     121           0 :       LetterDesignator = 'P';
+     122           2 :     else if ((8 > Lat) && (Lat >= 0))
+     123           2 :       LetterDesignator = 'N';
+     124           0 :     else if ((0 > Lat) && (Lat >= -8))
+     125           0 :       LetterDesignator = 'M';
+     126           0 :     else if ((-8 > Lat) && (Lat >= -16))
+     127           0 :       LetterDesignator = 'L';
+     128           0 :     else if ((-16 > Lat) && (Lat >= -24))
+     129           0 :       LetterDesignator = 'K';
+     130           0 :     else if ((-24 > Lat) && (Lat >= -32))
+     131           0 :       LetterDesignator = 'J';
+     132           0 :     else if ((-32 > Lat) && (Lat >= -40))
+     133           0 :       LetterDesignator = 'H';
+     134           0 :     else if ((-40 > Lat) && (Lat >= -48))
+     135           0 :       LetterDesignator = 'G';
+     136           0 :     else if ((-48 > Lat) && (Lat >= -56))
+     137           0 :       LetterDesignator = 'F';
+     138           0 :     else if ((-56 > Lat) && (Lat >= -64))
+     139           0 :       LetterDesignator = 'E';
+     140           0 :     else if ((-64 > Lat) && (Lat >= -72))
+     141           0 :       LetterDesignator = 'D';
+     142           0 :     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           0 :       LetterDesignator = 'Z';
+     147           2 :     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           2 :   static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, char* UTMZone) {
+     160           2 :     double a          = WGS84_A;
+     161           2 :     double eccSquared = UTM_E2;
+     162           2 :     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           2 :     double LongTemp = (Long + 180) - int((Long + 180) / 360) * 360 - 180;
+     170             : 
+     171           2 :     double LatRad  = Lat * RADIANS_PER_DEGREE;
+     172           2 :     double LongRad = LongTemp * RADIANS_PER_DEGREE;
+     173             :     double LongOriginRad;
+     174             :     int    ZoneNumber;
+     175             : 
+     176           2 :     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           2 :     if (ZoneNumber > 99)
+     180           0 :       ZoneNumber = 99;
+     181           2 :     if (ZoneNumber < -9)
+     182           0 :       ZoneNumber = -9;
+     183             : 
+     184           2 :     if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
+     185           0 :       ZoneNumber = 32;
+     186             : 
+     187             :     // Special zones for Svalbard
+     188           2 :     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           2 :     LongOrigin    = (ZoneNumber - 1) * 6 - 180 + 3;
+     200           2 :     LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
+     201             : 
+     202             :     // compute the UTM Zone from the latitude and longitude
+     203           2 :     snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+     204             : 
+     205           2 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     206             : 
+     207           2 :     N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
+     208           2 :     T = tan(LatRad) * tan(LatRad);
+     209           2 :     C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+     210           2 :     A = cos(LatRad) * (LongRad - LongOriginRad);
+     211             : 
+     212           2 :     M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad -
+     213           2 :              (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad) +
+     214           2 :              (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad) -
+     215           2 :              (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
+     216             : 
+     217           2 :     UTMEasting =
+     218           2 :         (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           2 :     UTMNorthing = (double)(k0 * (M + N * tan(LatRad) *
+     221           2 :                                          (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
+     222           2 :                                           (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
+     223           2 :     if (Lat < 0)
+     224           0 :       UTMNorthing += 10000000.0;  // 10000000 meter offset for southern hemisphere
+     225           2 :   }
+     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           2 :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char* UTMZone, double& Lat, double& Long) {
+     246           2 :     double                  k0         = UTM_K0;
+     247           2 :     double                  a          = WGS84_A;
+     248           2 :     double                  eccSquared = UTM_E2;
+     249             :     double                  eccPrimeSquared;
+     250           2 :     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           2 :     x = UTMEasting - 500000.0;  // remove 500,000 meter offset for longitude
+     261           2 :     y = UTMNorthing;
+     262             : 
+     263           2 :     ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+     264           2 :     if ((*ZoneLetter - 'N') >= 0)
+     265           2 :       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           2 :     LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3;  //+3 puts origin in middle of zone
+     272             : 
+     273           2 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     274             : 
+     275           2 :     M  = y / k0;
+     276           2 :     mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
+     277             : 
+     278           2 :     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           2 :               (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
+     280           2 :     phi1 = phi1Rad * DEGREES_PER_RADIAN;
+     281             : 
+     282           2 :     N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
+     283           2 :     T1 = tan(phi1Rad) * tan(phi1Rad);
+     284           2 :     C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+     285           2 :     R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
+     286           2 :     D  = x / (N1 * k0);
+     287             : 
+     288           2 :     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           2 :                                                 (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
+     290           2 :     Lat = Lat * DEGREES_PER_RADIAN;
+     291             : 
+     292           2 :     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           2 :            cos(phi1Rad);
+     294           2 :     Long = LongOrigin + Long * DEGREES_PER_RADIAN;
+     295           2 :   }
+     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..64858eb80844f0d15a82b7985d5b61e06e120a8f GIT binary patch literal 1576 zcmV+@2G{wCP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpP{ZZ&zdDZMowhM!ZWCuIDrW!tnxY!#+YS z<7Q=erEzQqgXd8-VJQ+3WQE4Z$wp%@lcxs*iiwCBjO?78# znaIJUi$SmL6_8jPo@(*CX=fyUybjzMNX%1+ywdGh;o6@W4uf_tuX^vX6by}J&&^Z^ zBV~;uQkZv3>%?y%1Q&vbPd?L~^YD$k#kSpUh`3#}tu%Ht9+=udBXiP43q(%_4SqqU zxAp|M3t%)C;2BYR+3%6bW^p8{Cu+)}(i~_CKYeV)7u%KoN&)4VhOM7FLo?*uM&2Ie(R?XzqWS$m%~>4tMi&l)K@?9RX7TQFy&fmjUp$O zl4TFZyv5cK*nN4NP`uyoKhz$NpW}YkP(}=cQ<`6jUy3SFUW{Oxu#Ix z@UPS8Wy67$wain*(+;c!{e$gCan9~U($Pcica8%%7SWt5F_ z5AFvqusbQJ5(gcTterL6%7}B7ms?~TL!}F;7ISScF`X)$J#KWjr{0!5?wcu2pLVQS zGn7Q4s+^QyKEyg#xkhE<+SZkf%_^@}&jW-!C|DlDToYu}^agvKqB%xiyn6K6=F=3j zm`4x%JBsJ&K?O`Eo*plJG%YxdXs zT_Wp@GA2%_+Eng|Op3CyOdA={?mhf7MaexWjZH^SQ8xEao!;V9 z!2GUWB1sls1FL7}?<$CNvtJYnXUtdzG2wJyXtmc^PxQq=tKS@e(<#MJ*eRWf;{27J zWf`lH + + + + + + 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:28940172.1 %
Date:2023-11-30 22:31:42Functions:7511068.2 %
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 +
57.5%57.5%
+ 		57.5 %69 / 12055.2 %16 / 29
<unnamed>57.5 %69 / 12055.2 %16 / 29
transformer.hpp +
68.8%68.8%
+ 		68.8 %44 / 6462.2 %23 / 37
<unnamed>68.8 %44 / 6462.2 %23 / 37
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1475.0 %12 / 16
<unnamed>64.3 %9 / 1475.0 %12 / 16
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
publisher_handler.hpp +
90.0%90.0%
+ 		90.0 %27 / 30100.0 %4 / 4
<unnamed>90.0 %27 / 30100.0 %4 / 4
service_client_handler.hpp +
92.5%92.5%
+ 		92.5 %49 / 53100.0 %9 / 9
<unnamed>92.5 %49 / 53100.0 %9 / 9
+
+
+ + + + +
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..1bf6075c56 --- /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:28940172.1 %
Date:2023-11-30 22:31:42Functions:7511068.2 %
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 +
57.5%57.5%
+ 		57.5 %69 / 12055.2 %16 / 29
<unnamed>57.5 %69 / 12055.2 %16 / 29
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1475.0 %12 / 16
<unnamed>64.3 %9 / 1475.0 %12 / 16
transformer.hpp +
68.8%68.8%
+ 		68.8 %44 / 6462.2 %23 / 37
<unnamed>68.8 %44 / 6462.2 %23 / 37
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
publisher_handler.hpp +
90.0%90.0%
+ 		90.0 %27 / 30100.0 %4 / 4
<unnamed>90.0 %27 / 30100.0 %4 / 4
service_client_handler.hpp +
92.5%92.5%
+ 		92.5 %49 / 53100.0 %9 / 9
<unnamed>92.5 %49 / 53100.0 %9 / 9
+
+
+ + + + +
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..e7970d8ce8 --- /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:28940172.1 %
Date:2023-11-30 22:31:42Functions:7511068.2 %
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 / 1475.0 %12 / 16
<unnamed>64.3 %9 / 1475.0 %12 / 16
publisher_handler.hpp +
90.0%90.0%
+ 		90.0 %27 / 30100.0 %4 / 4
<unnamed>90.0 %27 / 30100.0 %4 / 4
service_client_handler.hpp +
92.5%92.5%
+ 		92.5 %49 / 53100.0 %9 / 9
<unnamed>92.5 %49 / 53100.0 %9 / 9
subscribe_handler.hpp +
57.5%57.5%
+ 		57.5 %69 / 12055.2 %16 / 29
<unnamed>57.5 %69 / 12055.2 %16 / 29
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
68.8%68.8%
+ 		68.8 %44 / 6462.2 %23 / 37
<unnamed>68.8 %44 / 6462.2 %23 / 37
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..a5c39be739 --- /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:28940172.1 %
Date:2023-11-30 22:31:42Functions:7511068.2 %
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 +
57.5%57.5%
+ 		57.5 %69 / 12055.2 %16 / 29
transformer.hpp +
68.8%68.8%
+ 		68.8 %44 / 6462.2 %23 / 37
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1475.0 %12 / 16
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
publisher_handler.hpp +
90.0%90.0%
+ 		90.0 %27 / 30100.0 %4 / 4
service_client_handler.hpp +
92.5%92.5%
+ 		92.5 %49 / 53100.0 %9 / 9
+
+
+ + + + +
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..5cb88fbc66 --- /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:28940172.1 %
Date:2023-11-30 22:31:42Functions:7511068.2 %
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 +
57.5%57.5%
+ 		57.5 %69 / 12055.2 %16 / 29
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1475.0 %12 / 16
transformer.hpp +
68.8%68.8%
+ 		68.8 %44 / 6462.2 %23 / 37
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
publisher_handler.hpp +
90.0%90.0%
+ 		90.0 %27 / 30100.0 %4 / 4
service_client_handler.hpp +
92.5%92.5%
+ 		92.5 %49 / 53100.0 %9 / 9
+
+
+ + + + +
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..42c811c052 --- /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:28940172.1 %
Date:2023-11-30 22:31:42Functions:7511068.2 %
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 / 1475.0 %12 / 16
publisher_handler.hpp +
90.0%90.0%
+ 		90.0 %27 / 30100.0 %4 / 4
service_client_handler.hpp +
92.5%92.5%
+ 		92.5 %49 / 53100.0 %9 / 9
subscribe_handler.hpp +
57.5%57.5%
+ 		57.5 %69 / 12055.2 %16 / 29
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
68.8%68.8%
+ 		68.8 %44 / 6462.2 %23 / 37
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..962099f34c --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2023-11-30 22:31:42Functions:121675.0 %
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::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) 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::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) 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<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) 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::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const1
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> >&) const2
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> > > >&) const2
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> >&) const2
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> >&) const2
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> > > >&) const2
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> >&) const2
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5
+
+
+ + + +
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..63ea3837bf --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2023-11-30 22:31:42Functions:121675.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
bool mrs_lib::ParamProvider::getParamImpl<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&) const2
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> > > >&) const2
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> >&) const2
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&) const0
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const1
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5
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> >&) const2
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> > > >&) const2
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> >&) const2
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&) const0
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const1
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5
+
+
+ + + +
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..31db38b93c --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2023-11-30 22:31:42Functions:121675.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<std_msgs::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
+
+
+ + + +
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..ed38a0d19b --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html @@ -0,0 +1,96 @@ + + + + + + + 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:273090.0 %
Date:2023-11-30 22:31:42Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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_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
+
+
+ + + +
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..45a97a5794 --- /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:273090.0 %
Date:2023-11-30 22:31:42Functions:44100.0 %
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           2 : PublisherHandler_impl<TopicType>::PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+      23           2 :                                                         const double& rate) {
+      24             : 
+      25             :   {
+      26           2 :     std::scoped_lock lock(mutex_publisher_);
+      27             : 
+      28           2 :     publisher_ = nh.advertise<TopicType>(address, buffer_size, latch);
+      29             : 
+      30           2 :     if (rate > 0.0) {
+      31             : 
+      32           1 :       throttle_ = true;
+      33             : 
+      34           1 :       throttle_min_dt_ = 1.0 / rate;
+      35             : 
+      36             :     } else {
+      37             : 
+      38           1 :       throttle_ = false;
+      39             : 
+      40           1 :       throttle_min_dt_ = 0;
+      41             :     }
+      42             : 
+      43           2 :     last_time_published_ = ros::Time(0);
+      44             :   }
+      45             : 
+      46           2 :   publisher_initialized_ = true;
+      47           2 : }
+      48             : 
+      49             : //}
+      50             : 
+      51             : /* publish(TopicType& msg) //{ */
+      52             : 
+      53             : template <class TopicType>
+      54         110 : void PublisherHandler_impl<TopicType>::publish(const TopicType& msg) {
+      55             : 
+      56         110 :   if (!publisher_initialized_) {
+      57           0 :     return;
+      58             :   }
+      59             : 
+      60             :   {
+      61         110 :     std::scoped_lock lock(mutex_publisher_);
+      62             : 
+      63         110 :     if (throttle_) {
+      64             : 
+      65         100 :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+      66          90 :         return;
+      67             :       }
+      68             : 
+      69          10 :       last_time_published_ = ros::Time::now();
+      70             :     }
+      71             : 
+      72             :     try {
+      73          20 :       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             : void PublisherHandler_impl<TopicType>::publish(const boost::shared_ptr<TopicType const>& msg) {
+     119             : 
+     120             :   if (!publisher_initialized_) {
+     121             :     return;
+     122             :   }
+     123             : 
+     124             :   {
+     125             :     std::scoped_lock lock(mutex_publisher_);
+     126             : 
+     127             :     if (throttle_) {
+     128             : 
+     129             :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+     130             :         return;
+     131             :       }
+     132             : 
+     133             :       last_time_published_ = ros::Time::now();
+     134             :     }
+     135             : 
+     136             :     try {
+     137             :       publisher_.publish(msg);
+     138             :     }
+     139             :     catch (...) {
+     140             :       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             : PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other) {
+     169             : 
+     170             :   if (this == &other) {
+     171             :     return *this;
+     172             :   }
+     173             : 
+     174             :   if (other.impl_) {
+     175             :     this->impl_ = other.impl_;
+     176             :   }
+     177             : 
+     178             :   return *this;
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* copy constructor //{ */
+     184             : 
+     185             : template <class TopicType>
+     186             : PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other) {
+     187             : 
+     188             :   if (other.impl_) {
+     189             :     this->impl_ = other.impl_;
+     190             :   }
+     191             : }
+     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           2 : PublisherHandler<TopicType>::PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+     199           2 :                                               const double& rate) {
+     200             : 
+     201           2 :   impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(nh, address, buffer_size, latch, rate);
+     202           2 : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* publish(const TopicType& msg) //{ */
+     207             : 
+     208             : template <class TopicType>
+     209         110 : void PublisherHandler<TopicType>::publish(const TopicType& msg) {
+     210             : 
+     211         110 :   impl_->publish(msg);
+     212         110 : }
+     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             : void PublisherHandler<TopicType>::publish(const boost::shared_ptr<TopicType const>& msg) {
+     230             : 
+     231             :   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..68f23079e14b64ba7890a2736ea1776e9125fc7a GIT binary patch literal 769 zcmV+c1OEJpP)=p0^W(B%Iv;Qjf39{ou!Y0IhsL3njDPPi;EPQ0*oHM=)o3esd zL<@J2RjeMyBGWT33Of`-f8*#G8S$)-GoM>|urMx-HN`?GdmUVnneLFO%rH#?^Te7< zf?YI^D?)_&o0fvCt6a@n%B83X1wUxC0q@Bn2}HDMhbTl8sXWZk5rqL%6jT(^h{j2& zY3+*WBGW^`c2zrq!Q$eI>2@yqBaob+6*Y8%9X+tO(MZDbFnnjryIwGK4%BbV-_tGG) zm2T@1QNsq$_mXNA2_i3OQhGvzN_XI6p*!P!)D`}#UbXsFih3+K`7vM8|LnK@wGyR> z8w>afA8Gy=O(SPBi9CmhpNHpG72)S6qH!=~FHv0f&mkT$MMs-$2i0@h1sr?N(H*SBN`S}(Jcz*h06Jm;v zHrr2seEMU_^wS^T5aHna?E2bS z+Wu%Fq%e7;AsvbP0VXLk)m>quI9z7n2)k`FHC}R(8Y!~-j@0(PXB&sbcSPI&Ei*15 z-?V^4?@a`dsLi<%GQ-R|0rw*|oM=8EKRLZWsqeCmZ@$CU00000NkvXXu0mjf4$ENA literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html new file mode 100644 index 0000000000..5242f8dc2e --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + 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:495392.5 %
Date:2023-11-30 22:31:42Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)1
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)1
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&)2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> 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_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
+
+
+ + + +
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..4ba6d314a1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html @@ -0,0 +1,116 @@ + + + + + + + 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:495392.5 %
Date:2023-11-30 22:31:42Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)1
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&)2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)1
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&)2
+
+
+ + + +
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..919e5d7cb7 --- /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:495392.5 %
Date:2023-11-30 22:31:42Functions:99100.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           2 : ServiceClientHandler_impl<ServiceType>::ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address) {
+      23             : 
+      24             :   {
+      25           2 :     std::scoped_lock lock(mutex_service_client_);
+      26             : 
+      27           2 :     service_client_ = nh.serviceClient<ServiceType>(address);
+      28             :   }
+      29             : 
+      30           2 :   _address_       = address;
+      31           2 :   async_attempts_ = 1;
+      32             : 
+      33             :   /* thread_oneshot_ = std::make_shared<std::thread>(std::thread(&ServiceClientHandler_impl::threadOneshot, this, true, false)); */
+      34             : 
+      35           2 :   service_initialized_ = true;
+      36           2 : }
+      37             : 
+      38             : //}
+      39             : 
+      40             : /* call(ServiceType& srv) //{ */
+      41             : 
+      42             : template <class ServiceType>
+      43           1 : bool ServiceClientHandler_impl<ServiceType>::call(ServiceType& srv) {
+      44             : 
+      45           1 :   if (!service_initialized_) {
+      46           0 :     return false;
+      47             :   }
+      48             : 
+      49           1 :   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           2 : ServiceClientHandler<ServiceType>& ServiceClientHandler<ServiceType>::operator=(const ServiceClientHandler<ServiceType>& other) {
+     205             : 
+     206           2 :   if (this == &other) {
+     207           0 :     return *this;
+     208             :   }
+     209             : 
+     210           2 :   if (other.impl_) {
+     211           2 :     this->impl_ = other.impl_;
+     212             :   }
+     213             : 
+     214           2 :   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           2 : ServiceClientHandler<ServiceType>::ServiceClientHandler(ros::NodeHandle& nh, const std::string& address) {
+     235             : 
+     236           2 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     237           2 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : /* initialize(ros::NodeHandle& nh, const std::string& address) //{ */
+     242             : 
+     243             : template <class ServiceType>
+     244             : void ServiceClientHandler<ServiceType>::initialize(ros::NodeHandle& nh, const std::string& address) {
+     245             : 
+     246             :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     247             : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* call(ServiceType& srv) //{ */
+     252             : 
+     253             : template <class ServiceType>
+     254           1 : bool ServiceClientHandler<ServiceType>::call(ServiceType& srv) {
+     255             : 
+     256           1 :   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..f4258008c1557d544e2cafc247c684db92e5e460 GIT binary patch literal 973 zcmV;;12X)HP)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%93cM}SlNe;W}N zE}rvXb|79o+6>VM|2-e)>HU~g84T3 z;_;o(Q=|<3*dX>$o?f6m-)H5+i_a1yPnMoB#$5392e@ie8!yYfq@Q2v55F6Y*kHr zLL{xt3#W)&K{4HPQXlmwhe|5smHrVbne$Yx#4;cAm_#~Tp{>m#LjBtLV&U>63^zG62I|Jf*E?zNL}f$d%^7fouU47%7OdeTbW_7M6$m@L0Ks v_>pnV!Y^8ke^Xp@7Rpd3Y;B#~c4gpSkDJi`U`o!900000NkvXXu0mjf`=G(A 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..59469f2c32 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html @@ -0,0 +1,196 @@ + + + + + + + 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:6912057.5 %
Date:2023-11-30 22:31:42Functions:162955.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
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::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() 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::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::usedMsg() const0
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&)1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().21
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&)1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().21
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const2
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()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<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const10000
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const10000
+
+
+ + + +
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..045fe565c1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html @@ -0,0 +1,196 @@ + + + + + + + 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:6912057.5 %
Date:2023-11-30 22:31:42Functions:162955.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
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&)1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().21
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&)20
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()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()0
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&)1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().21
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const10000
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() 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::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const10000
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const0
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]() const2
+
+
+ + + +
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..a860aab999 --- /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:6912057.5 %
Date:2023-11-30 22:31:42Functions:162955.2 %
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           1 :     Impl(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+      29           1 :         : m_nh(options.nh),
+      30           1 :           m_topic_name(options.topic_name),
+      31           1 :           m_node_name(options.node_name),
+      32             :           m_got_data(false),
+      33             :           m_new_data(false),
+      34             :           m_used_data(false),
+      35           1 :           m_timeout_manager(options.timeout_manager),
+      36             :           m_latest_message_time(0),
+      37             :           m_latest_message(nullptr),
+      38             :           m_message_callback(message_callback),
+      39           1 :           m_queue_size(options.queue_size),
+      40           1 :           m_transport_hints(options.transport_hints)
+      41             :     {
+      42           1 :       if (options.no_message_timeout != mrs_lib::no_timeout)
+      43             :       {
+      44             :         // initialize a new TimeoutManager if not provided by the user
+      45           1 :         if (!m_timeout_manager)
+      46           1 :           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           2 :         std::function<void(const ros::Time&)> timeout_mgr_callback;
+      50           1 :         if (options.timeout_callback)
+      51           1 :           timeout_mgr_callback = std::bind(options.timeout_callback, topicName(), std::placeholders::_1);
+      52             :         else
+      53           0 :           timeout_mgr_callback = std::bind(&Impl::default_timeout_callback, this, topicName(), std::placeholders::_1);
+      54             : 
+      55             :         // register the timeout callback with the TimeoutManager
+      56           1 :         m_timeout_id = m_timeout_manager->registerNew(options.no_message_timeout, timeout_mgr_callback);
+      57             :       }
+      58             : 
+      59           3 :       const std::string msg = "Subscribed to topic '" + m_topic_name + "' -> '" + topicName() + "'";
+      60           1 :       if (m_node_name.empty())
+      61           1 :         ROS_INFO_STREAM(msg);
+      62             :       else
+      63           0 :         ROS_INFO_STREAM("[" << m_node_name << "]: " << msg);
+      64           1 :     }
+      65             :     //}
+      66             : 
+      67           1 :     virtual ~Impl() = default;
+      68             : 
+      69             :   public:
+      70             :     /* getMsg() method //{ */
+      71           0 :     virtual typename MessageType::ConstPtr getMsg()
+      72             :     {
+      73           0 :       m_new_data = false;
+      74           0 :       m_used_data = true;
+      75           0 :       return peekMsg();
+      76             :     }
+      77             :     //}
+      78             : 
+      79             :     /* peekMsg() method //{ */
+      80           0 :     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           0 :       return m_latest_message;
+      87             :     }
+      88             :     //}
+      89             : 
+      90             :     /* hasMsg() method //{ */
+      91       10000 :     virtual bool hasMsg() const
+      92             :     {
+      93       10000 :       return m_got_data;
+      94             :     }
+      95             :     //}
+      96             : 
+      97             :     /* newMsg() method //{ */
+      98           0 :     virtual bool newMsg() const
+      99             :     {
+     100           0 :       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           5 :     virtual ros::Time lastMsgTime() const
+     130             :     {
+     131           5 :       return m_latest_message_time;
+     132             :     };
+     133             :     //}
+     134             : 
+     135             :     /* topicName() method //{ */
+     136           2 :     virtual std::string topicName() const
+     137             :     {
+     138           2 :       std::string ret = m_sub.getTopic();
+     139           2 :       if (ret.empty())
+     140           2 :         ret = m_nh.resolveName(m_topic_name);
+     141           2 :       return ret;
+     142             :     }
+     143             :     //}
+     144             : 
+     145             :     /* start() method //{ */
+     146           5 :     virtual void start()
+     147             :     {
+     148           5 :       if (m_timeout_manager)
+     149           5 :         m_timeout_manager->start(m_timeout_id);
+     150           5 :       m_sub = m_nh.subscribe(m_topic_name, m_queue_size, &Impl::data_callback, this, m_transport_hints);
+     151           5 :     }
+     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          20 :     void process_new_message(const typename MessageType::ConstPtr& msg)
+     210             :     {
+     211          20 :       m_latest_message_time = ros::Time::now();
+     212          20 :       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          20 :       m_new_data = !m_message_callback;
+     216          20 :       m_got_data = true;
+     217          20 :       m_new_data_cv.notify_one();
+     218          20 :     }
+     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           1 :     ImplThreadsafe(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+     254           1 :         : impl_class_t::Impl(options, message_callback)
+     255             :     {
+     256           1 :     }
+     257             : 
+     258             :   public:
+     259       10000 :     virtual bool hasMsg() const override
+     260             :     {
+     261       20000 :       std::lock_guard lck(m_mtx);
+     262       20000 :       return impl_class_t::hasMsg();
+     263             :     }
+     264           0 :     virtual bool newMsg() const override
+     265             :     {
+     266           0 :       std::lock_guard lck(m_mtx);
+     267           0 :       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           0 :     virtual typename MessageType::ConstPtr getMsg() override
+     275             :     {
+     276           0 :       std::lock_guard lck(m_mtx);
+     277           0 :       return impl_class_t::getMsg();
+     278             :     }
+     279           0 :     virtual typename MessageType::ConstPtr peekMsg() const override
+     280             :     {
+     281           0 :       std::lock_guard lck(m_mtx);
+     282           0 :       return impl_class_t::peekMsg();
+     283             :     }
+     284           5 :     virtual ros::Time lastMsgTime() const override
+     285             :     {
+     286          10 :       std::lock_guard lck(m_mtx);
+     287          10 :       return impl_class_t::lastMsgTime();
+     288             :     };
+     289           0 :     virtual std::string topicName() const override
+     290             :     {
+     291           0 :       std::lock_guard lck(m_mtx);
+     292           0 :       return impl_class_t::topicName();
+     293             :     };
+     294           5 :     virtual void start() override
+     295             :     {
+     296          10 :       std::lock_guard lck(m_mtx);
+     297          10 :       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           2 :     virtual ~ImplThreadsafe() override = default;
+     306             : 
+     307             :   protected:
+     308          20 :     virtual void data_callback(const typename MessageType::ConstPtr& msg) override
+     309             :     {
+     310             :       {
+     311          40 :         std::scoped_lock lck(m_mtx, this->m_new_data_mtx);
+     312          20 :         if (this->m_timeout_manager)
+     313          20 :           this->m_timeout_manager->reset(this->m_timeout_id);
+     314          20 :         impl_class_t::process_new_message(msg);
+     315             :       }
+     316             : 
+     317             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     318          20 :       if (this->m_message_callback)
+     319          20 :         impl_class_t::m_message_callback(msg);
+     320          20 :     }
+     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..2356d3f7049e74289025996500885f7036a04857 GIT binary patch literal 1399 zcmV--1&I2IP)T2>A#hgcLE878Ft*|yKnTo-@FM4c#lObi(cG#%o$4} zXosEv9ARzsfkOb4P#k1EOBpSpC1(bWqwOEf*++Ru@Q(_js7N3L4NzNk^hUmJSxKyo z+{}=-in>(eJ)lMq{Ek!;uZ$4xxfD_UNxTi%Q3WMc^M!SN+Oa+J*u#VT?7F*v>$)e% z$_@uypT!_vAb}L&0@z?3O(gTE#37_Igrqf)F;T&~CubedwXh! z@pQ%mac-X#fqNXq6E4D|W9(#Uy^?^J%OpT2UsDFTjF)+46t=a&eLcw)qfLY3xE)dk zmc#9e#;y`R6?ZCtN-#= z#MP+-6jHp=JTbzJzp(xsO(~`wI)5X2zh}Fe%{OG$-gtq19}w3LaQ32$eH)lvz<<*alL2E)|>5k`|uX(8b-}+3tCR_v<5Q@TLg2 z8@!oy#$}vtaM4a?M*5=yDR9y$(x8m7J(B{zE~C%(52fQCmfzibJ9#BlVh7m;E>M30 z&dV5@S62d#$rx%^1@`PfIjgyfvZ2dZ;G&UwZQTS)LTQ?e&^1jU+$dmQs39K)x|{;w z+6SUYoFSvo;ONkdU2$AtPuBG&{+dwPk93+RAa$m`A>0)}10h7$0x7!9+t5qJU)N#T zoy2m79TGLh#9rlu9TL-Q0oDgASS6#-JhDyc0fTq?ya*hLGdu;k>d+Pd=;UF0qT`uZ z{*D*yzN+ABb_m>BMYgYdcE88&(CO5|*H`hcUhVLqKw57Uu?|GmTj%r)0p(gZ%BlNH z*IqzCQt*Z_ zR-g>RO1HBaQliz(L!b<)^8QG&m#?le;F#9noXcwQv5d$pjkU1Y`efEv zIqULNic5Xt2i>WDZ39g==MH6DbcV$3pnpXf8al2_{Rsj#)xYu#dAtwY^e;R^9%&Gh z@hb^{-P{+RePqw%Un&cH#WOTke$E9`^ERoec-J%hsg%5*N&De^s{f8MAF~z@a3Gc{ z3CfjY3%jU*gwf*%(c3>=IV0CvR)<+MWQw?!TI2c8uwEg_YU~BY76JgL_t(NMu2Pzx zT% + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2023-11-30 22:31:42Functions: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..5c45b97e1f --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2023-11-30 22:31:42Functions: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..94164fa6ff --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html @@ -0,0 +1,179 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2023-11-30 22:31:42Functions: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..ba491365e4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html @@ -0,0 +1,228 @@ + + + + + + + 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:446468.8 %
Date:2023-11-30 22:31:42Functions:233762.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::doTransform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
geometry_msgs::Quaternion_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::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&)0
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&)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<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&)0
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&)0
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)0
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)0
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)0
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&)1
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<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&)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<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&)1
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&)1
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > 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::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&)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<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&)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<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&)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
+
+
+ + + +
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..9d85ef096f --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html @@ -0,0 +1,228 @@ + + + + + + + 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:446468.8 %
Date:2023-11-30 22:31:42Functions:233762.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
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&)1
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&)6
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&)1
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&)0
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&)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::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&)1
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&)1
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&)0
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&)0
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)0
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)1
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)0
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<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::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&)6
+
+
+ + + +
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..ec1fbb63a8 --- /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:446468.8 %
Date:2023-11-30 22:31:42Functions:233762.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef TRANSFORMER_HPP
+       2             : #define TRANSFORMER_HPP
+       3             : 
+       4             : // clang: MatousFormat
+       5             : 
+       6             : namespace mrs_lib
+       7             : {
+       8             : 
+       9             :   // | --------------------- helper methods --------------------- |
+      10             : 
+      11             :   /* getHeader() overloads for different message types (pointers, pointclouds etc) //{ */
+      12             : 
+      13             :   template <typename msg_t>
+      14           1 :   std_msgs::Header Transformer::getHeader(const msg_t& msg)
+      15             :   {
+      16           1 :     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           0 :   void Transformer::setHeader(msg_t& msg, const std_msgs::Header& header)
+      33             :   {
+      34           0 :     msg.header = header;
+      35           0 :   }
+      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           0 :   T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
+      49             :   {
+      50           0 :     T ret = what;
+      51             :     if constexpr (has_header_member_v<T>)
+      52             :     {
+      53           0 :       std_msgs::Header new_header = getHeader(what);
+      54           0 :       new_header.frame_id = frame_id;
+      55           0 :       setHeader(ret, new_header);
+      56             :     }
+      57           0 :     return ret;
+      58             :   }
+      59             : 
+      60             :   //}
+      61             : 
+      62             :   /* transformImpl() //{ */
+      63             : 
+      64             :   template <class T>
+      65          50 :   std::optional<T> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const T& what)
+      66             :   {
+      67         100 :     const std::string from_frame = frame_from(tf);
+      68         100 :     const std::string to_frame = frame_to(tf);
+      69             : 
+      70          50 :     if (from_frame == to_frame)
+      71           0 :       return copyChangeFrame(what, from_frame);
+      72             : 
+      73         100 :     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          23 :       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          21 :       else if (to_frame == latlon_frame_name)
+      89             :       {
+      90           2 :         const std::optional<T> tmp = doTransform(what, tf);
+      91           2 :         if (!tmp.has_value())
+      92           0 :           return std::nullopt;
+      93           2 :         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          27 :       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          44 :     return doTransform(what, tf);
+     108             :   }
+     109             : 
+     110             :   //}
+     111             : 
+     112             :   /* doTransform() //{ */
+     113             : 
+     114             :   template <class T>
+     115          48 :   std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::TransformStamped& tf)
+     116             :   {
+     117             :     try
+     118             :     {
+     119          55 :       T result;
+     120          48 :       tf2::doTransform(what, result, tf);
+     121          48 :       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           1 :   std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
+     139             :   {
+     140           2 :     const std_msgs::Header orig_header = getHeader(what);
+     141           2 :     return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
+     142             :   }
+     143             : 
+     144             :   template <class T>
+     145           2 :   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           4 :     std::scoped_lock lck(mutex_);
+     148             : 
+     149           2 :     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           4 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     156           4 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     157           4 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     158             : 
+     159             :     // get the transform
+     160           4 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     161           2 :     if (!tf_opt.has_value())
+     162           0 :       return std::nullopt;
+     163           2 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     164             : 
+     165             :     // do the transformation
+     166           4 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     167           2 :     return transformImpl(tf_resolved, what);
+     168             :   }
+     169             : 
+     170             :   //}
+     171             : 
+     172             :   /* transform() //{ */
+     173             : 
+     174             :   template <class T>
+     175          24 :   std::optional<T> Transformer::transform(const T& what, const geometry_msgs::TransformStamped& tf)
+     176             :   {
+     177          48 :     std::scoped_lock lck(mutex_);
+     178             : 
+     179          24 :     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          48 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     186          48 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     187          48 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     188             : 
+     189          24 :     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..5533b1e26e8c222ed68d63883b1e3b3d83316b7d GIT binary patch literal 857 zcmV-f1E&0mP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpu|zopoME%UO4>&-7Y<9sG|$Xqu@`^$@bVL9x| zv4gN3z3BiaLI%qN#h_QSNZ;BSk=A1K z#2$~RVj8fd_wd|nbCHG6x<+>f2sRJTR(I(iwH!6+Z_q&V-r(azpW7!(d`3s%H8@W^ zKQ@8S=Qk{1mZhe}B#j~oXf}>0*b?xQabBkgi*4gB*AW)ITq8OR)pEy~GLH2y+BgPu z_ncY~$6{94ACF^NFc+Me04{lGohb#5DU|;WwaGod+bgBua+zjyHie1EVEp^f5tg!m z_bo0D_(T|86uAu=Z;{dzJFGG|IR<6Gw7SOpGnyux7qFdX~8KN-6qehD4-h(@Z zjsp#h@U9y?(EzB@oC`rvz$7d jiCd}sj|22I9Ma5R + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2023-11-30 22:31:42Functions: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..3742a70f1a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2023-11-30 22:31:42Functions: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..527cab8b4b --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2023-11-30 22:31:42Functions: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..1323c55450 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-f.html @@ -0,0 +1,318 @@ + + + + + + + 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:57072478.7 %
Date:2023-11-30 22:31:42Functions:13216381.0 %
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
lkf.h +
63.0%63.0%
+ 		63.0 %34 / 5440.9 %9 / 22
<unnamed>63.0 %34 / 5440.9 %9 / 22
subscribe_handler.h +
77.5%77.5%
+ 		77.5 %31 / 4065.0 %13 / 20
<unnamed>77.5 %31 / 4065.0 %13 / 20
transformer.h +
84.2%84.2%
+ 		84.2 %48 / 5776.5 %13 / 17
<unnamed>84.2 %48 / 5776.5 %13 / 17
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
<unnamed>85.2 %23 / 2781.8 %9 / 11
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
param_loader.h +
80.1%80.1%
+ 		80.1 %213 / 26696.0 %48 / 50
<unnamed>80.1 %213 / 26696.0 %48 / 50
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
<unnamed>100.0 %4 / 4-0 / 0
publisher_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %2 / 2
<unnamed>100.0 %3 / 3100.0 %2 / 2
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %3 / 3
<unnamed>100.0 %3 / 3100.0 %3 / 3
gps_conversions.h +
67.6%67.6%
+ 		67.6 %92 / 136100.0 %4 / 4
<unnamed>67.6 %92 / 136100.0 %4 / 4
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
<unnamed>100.0 %15 / 15100.0 %5 / 5
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 111100.0 %20 / 20
<unnamed>90.1 %100 / 111100.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..019e6660e4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-l.html @@ -0,0 +1,318 @@ + + + + + + + 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:57072478.7 %
Date:2023-11-30 22:31:42Functions:13216381.0 %
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
lkf.h +
63.0%63.0%
+ 		63.0 %34 / 5440.9 %9 / 22
<unnamed>63.0 %34 / 5440.9 %9 / 22
gps_conversions.h +
67.6%67.6%
+ 		67.6 %92 / 136100.0 %4 / 4
<unnamed>67.6 %92 / 136100.0 %4 / 4
subscribe_handler.h +
77.5%77.5%
+ 		77.5 %31 / 4065.0 %13 / 20
<unnamed>77.5 %31 / 4065.0 %13 / 20
param_loader.h +
80.1%80.1%
+ 		80.1 %213 / 26696.0 %48 / 50
<unnamed>80.1 %213 / 26696.0 %48 / 50
transformer.h +
84.2%84.2%
+ 		84.2 %48 / 5776.5 %13 / 17
<unnamed>84.2 %48 / 5776.5 %13 / 17
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
<unnamed>85.2 %23 / 2781.8 %9 / 11
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 111100.0 %20 / 20
<unnamed>90.1 %100 / 111100.0 %20 / 20
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %3 / 3
<unnamed>100.0 %3 / 3100.0 %3 / 3
publisher_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %2 / 2
<unnamed>100.0 %3 / 3100.0 %2 / 2
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
<unnamed>100.0 %4 / 4-0 / 0
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..4f01bd864d --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail.html @@ -0,0 +1,318 @@ + + + + + + + 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:57072478.7 %
Date:2023-11-30 22:31:42Functions:13216381.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
<unnamed>85.2 %23 / 2781.8 %9 / 11
gps_conversions.h +
67.6%67.6%
+ 		67.6 %92 / 136100.0 %4 / 4
<unnamed>67.6 %92 / 136100.0 %4 / 4
lkf.h +
63.0%63.0%
+ 		63.0 %34 / 5440.9 %9 / 22
<unnamed>63.0 %34 / 5440.9 %9 / 22
param_loader.h +
80.1%80.1%
+ 		80.1 %213 / 26696.0 %48 / 50
<unnamed>80.1 %213 / 26696.0 %48 / 50
publisher_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %2 / 2
<unnamed>100.0 %3 / 3100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 111100.0 %20 / 20
<unnamed>90.1 %100 / 111100.0 %20 / 20
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %3 / 3
<unnamed>100.0 %3 / 3100.0 %3 / 3
subscribe_handler.h +
77.5%77.5%
+ 		77.5 %31 / 4065.0 %13 / 20
<unnamed>77.5 %31 / 4065.0 %13 / 20
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 +
84.2%84.2%
+ 		84.2 %48 / 5776.5 %13 / 17
<unnamed>84.2 %48 / 5776.5 %13 / 17
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
+
+
+ + + + +
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..ac487ac040 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-f.html @@ -0,0 +1,222 @@ + + + + + + + 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:57072478.7 %
Date:2023-11-30 22:31:42Functions:13216381.0 %
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
lkf.h +
63.0%63.0%
+ 		63.0 %34 / 5440.9 %9 / 22
subscribe_handler.h +
77.5%77.5%
+ 		77.5 %31 / 4065.0 %13 / 20
transformer.h +
84.2%84.2%
+ 		84.2 %48 / 5776.5 %13 / 17
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
param_loader.h +
80.1%80.1%
+ 		80.1 %213 / 26696.0 %48 / 50
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
publisher_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %2 / 2
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %3 / 3
gps_conversions.h +
67.6%67.6%
+ 		67.6 %92 / 136100.0 %4 / 4
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 111100.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..0344434e59 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-l.html @@ -0,0 +1,222 @@ + + + + + + + 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:57072478.7 %
Date:2023-11-30 22:31:42Functions:13216381.0 %
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
lkf.h +
63.0%63.0%
+ 		63.0 %34 / 5440.9 %9 / 22
gps_conversions.h +
67.6%67.6%
+ 		67.6 %92 / 136100.0 %4 / 4
subscribe_handler.h +
77.5%77.5%
+ 		77.5 %31 / 4065.0 %13 / 20
param_loader.h +
80.1%80.1%
+ 		80.1 %213 / 26696.0 %48 / 50
transformer.h +
84.2%84.2%
+ 		84.2 %48 / 5776.5 %13 / 17
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 111100.0 %20 / 20
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %3 / 3
publisher_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %2 / 2
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
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..a9e875894b --- /dev/null +++ b/mrs_lib/include/mrs_lib/index.html @@ -0,0 +1,222 @@ + + + + + + + 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:57072478.7 %
Date:2023-11-30 22:31:42Functions:13216381.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
gps_conversions.h +
67.6%67.6%
+ 		67.6 %92 / 136100.0 %4 / 4
lkf.h +
63.0%63.0%
+ 		63.0 %34 / 5440.9 %9 / 22
param_loader.h +
80.1%80.1%
+ 		80.1 %213 / 26696.0 %48 / 50
publisher_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 111100.0 %20 / 20
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %3 / 3
subscribe_handler.h +
77.5%77.5%
+ 		77.5 %31 / 4065.0 %13 / 20
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
transformer.h +
84.2%84.2%
+ 		84.2 %48 / 5776.5 %13 / 17
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
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/lkf.h.func-sort-c.html b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html new file mode 100644 index 0000000000..c0b858f883 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html @@ -0,0 +1,168 @@ + + + + + + + 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:345463.0 %
Date:2023-11-30 22:31:42Functions:92240.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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
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::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<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5824
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&) const5824
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&) const5824
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&) const5824
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&)16240
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)16240
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) const16240
+
+
+ + + +
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..5463874ef0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func.html @@ -0,0 +1,168 @@ + + + + + + + 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:345463.0 %
Date:2023-11-30 22:31:42Functions:92240.9 %
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&)16240
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)16240
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5824
mrs_lib::LKF<2, 1, 1>::LKF()3
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::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) const16240
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&) const5824
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&) const5824
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&) const5824
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<(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
+
+
+ + + +
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..d11478266a --- /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:345463.0 %
Date:2023-11-30 22:31:42Functions:92240.9 %
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           0 :     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        5824 :     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        5824 :       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           0 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
+     139             :     {
+     140           0 :       statecov_t ret;
+     141           0 :       ret.x = state_predict(A, sc.x, B, u);
+     142           0 :       ret.P = covariance_predict(A, sc.P, Q, dt);
+     143           0 :       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       16240 :     static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
+     155             :     {
+     156       16240 :       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       16240 :     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       16240 :       return A * x + B * u;
+     172             :     }
+     173             :     //}
+     174             : 
+     175             :   protected:
+     176             :     /* invert_W() method //{ */
+     177        5824 :     static R_t invert_W(R_t W)
+     178             :     {
+     179        5824 :       Eigen::ColPivHouseholderQR<R_t> qr(W);
+     180        5824 :       if (!qr.isInvertible())
+     181             :       {
+     182             :         // add some stuff to the tmp matrix diagonal to make it invertible
+     183           0 :         R_t ident = R_t::Identity(W.rows(), W.cols());
+     184           0 :         W += 1e-9 * ident;
+     185           0 :         qr.compute(W);
+     186           0 :         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        5824 :       const R_t W_inv = qr.inverse();
+     193       11648 :       return W_inv;
+     194             :     }
+     195             :     //}
+     196             : 
+     197             :     /* computeKalmanGain() method //{ */
+     198        5824 :     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        5824 :       const R_t W = H * sc.P * H.transpose() + R;
+     202        5824 :       const R_t W_inv = invert_W(W);
+     203        5824 :       const K_t K = sc.P * H.transpose() * W_inv;
+     204       11648 :       return K;
+     205             :     }
+     206             :     //}
+     207             : 
+     208             :     /* correction_impl() method //{ */
+     209             :     template<int check=n>
+     210        5824 :     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        5824 :       statecov_t ret;
+     214        5824 :       const K_t K = computeKalmanGain(sc, z, R, H);
+     215        5824 :       ret.x = sc.x + K * (z - (H * sc.x));
+     216        5824 :       ret.P = (P_t::Identity() - (K * H)) * sc.P;
+     217       11648 :       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       16240 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
+     317             :     {
+     318       16240 :       statecov_t ret;
+     319       16240 :       A_t A = m_generateA(dt);
+     320       16240 :       B_t B = m_generateB(dt);
+     321       16240 :       ret.x = Base_class::state_predict(A, sc.x, B, u);
+     322       16240 :       ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
+     323       32480 :       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..0a7b09d5816afee05f060ecd15de68104540dce9 GIT binary patch literal 1773 zcmVXb@9EKN?Yn!UKO`gponnX)Vjk1s zh_E6<2ZV!2LIg<9kf}qYcX|W_M*IsTLsO0b9=f&t9&NX}>M^TQ!8i=$F9Y<-SbH9tD+>;pM+N}0MnrxKV2}~@#1H@yM*Wwj zRNnF%s2fm3ZNGMeoqG%?c zU4eyo^slmK`iWv-D!_^q`k)6CPQmiaPfGLn?HE}vTChUlaQGU|027vhaf2O@(Y4rS zpxO?KhdLMoA%Zm~%Uq-VGP)NsnYduNHkOVkP7*@U8P*U6S_sYqBmH;_q-YQqV;?8C z2X78kWys=Ua#lA;CKT3J0ps^5qIn8Cn$(_(2wjZF>}csgo(VI75Y%GCv$xUASJfF|^d&yt#SN?iGTO$FN=5HFnrx1H(iZ7)9#_#cU@lza` z>#J)`W6-NO>(aI<$130eMIp$gr9_pKOUl|TuYs$CpdmlG4Su^l)^Y2OsShFGupI-( z^8s*(BECw5;hNV20vB2pwgrqWtkd8*v{D4Umh!Pk6fY8m-rx$xbq;u+T=lrCVa37J zJ%b*OX46D2FV-V(F+lHyo+ zkoDzR;qSyuD0(;n4p2N7GZHWyE+Mkxh9G^5YyvQnDNi=IC=u<-IAop^!L<`vTVOM^ zG-(oR!%*?uNqVAiSZjzRN+{}Pp(-*4EztvqS782u6FW{PH&eh^kc?#QcszGBMl#nz zMWA`SuuNR|F*&QkCzh_6!tn>F#-$1r%hILwnHoqbbxWUE_ZVkjyPVztzgfCYDQIKq zK1GiyOI!(*uGVyj6bubu++xR+apI%)iBusrtw3o;6HQKdYqqH#{uD=y|juT5&{9p^OHA3-?H!`00ED5{z{`{4?go%HyaJtqe#u82-kVP@*B0I9domn%|J7AhZJ4BS?|DWW6%JSCL~)grG}h@ zOirRCx=BdSoT-+*4IBd`I=#%b0$iCA+y~u4wt~rkko(5)2%$zl!#?0&*oGDs* z+lk^S>}vE=rm&N+1#~O}u6m73B$JM>qjN32&JpVyxzaJ^VhRg6=O%E*bhtkAeD70K zXHMYs?VeNg!g|Ol{B!M_!ur++K;b0vIt^7)t|??8gTv9WuiWhTG=Y%oxBz&#wCCfo zxUUmsJ1KtgG4bh4kzgC`_?brh z_39NLiL2%71ioJHax5il>eH7!k`-{j`Wb2Mo#q*HideQy#?9sSPX;jG zLmtd(dSa{Dv5cuUx)&sDcW>iVS8R58p91b6)yDcRAh zzRep$o8G$7#Gcr^nh*BEZX>6pF4PU1+~32`-@{LlA!ro-ox{&ki0O-$8g3&S8fG`+ zBH+XBkMPn{ZIZNs*$#6d_sGu2_by%>=aw4T&qf2lAJGEzR$Sc?kyvg5e@JED-D$cU z_*>a`fNY+BuCfQHFOh7y*7j znQE^Jxb9@m@;`F6n_5OT100ZXBy&YAn09e))>k_Za5;8a!x^O?JEr*usS2ZaETmPe P00000NkvXXu0mjfi*HD% 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..df80fbc4a1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html @@ -0,0 +1,280 @@ + + + + + + + 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:21326680.1 %
Date:2023-11-30 22:31:42Functions:485096.0 %
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::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)1
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&)1
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)1
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
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
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)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::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)1
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)1
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&)1
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> >&)1
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)1
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&)2
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
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
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)2
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)2
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)2
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> > > >&)2
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)2
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&)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::printValue_recursive(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, unsigned int)3
mrs_lib::ParamLoader::loadedSuccessfully()4
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
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)4
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, -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)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
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)17
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)19
+
+
+ + + +
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..61693975c7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func.html @@ -0,0 +1,280 @@ + + + + + + + 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:21326680.1 %
Date:2023-11-30 22:31:42Functions:485096.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)17
mrs_lib::ParamLoader::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)1
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&)1
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&)2
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&)3
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)1
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::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
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)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
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<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<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)2
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()4
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ParamLoader::printValue_recursive(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, unsigned int)3
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)19
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<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)4
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)1
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)2
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)2
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)1
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9
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&)1
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> >&)1
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> > > >&)2
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)1
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)2
+
+
+ + + +
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..f79ebde5ef --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.html @@ -0,0 +1,1344 @@ + + + + + + + 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:21326680.1 %
Date:2023-11-30 22:31:42Functions:485096.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines ParamLoader - a convenience class for loading static ROS parameters.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef PARAM_LOADER_H
+       8             : #define PARAM_LOADER_H
+       9             : 
+      10             : #include <ros/ros.h>
+      11             : #include <string>
+      12             : #include <map>
+      13             : #include <unordered_set>
+      14             : #include <iostream>
+      15             : #include <Eigen/Dense>
+      16             : #include <std_msgs/ColorRGBA.h>
+      17             : #include <mrs_lib/param_provider.h>
+      18             : 
+      19             : namespace mrs_lib
+      20             : {
+      21             : 
+      22             : /*** ParamLoader CLASS //{ **/
+      23             : 
+      24             : /**
+      25             : * \brief Convenience class for loading parameters from rosparam server.
+      26             : *
+      27             : * The parameters can be loaded as compulsory. If a compulsory parameter is not found
+      28             : * on the rosparam server (e.g. because it is missing in the launchfile or the yaml config file),
+      29             : * an internal flag is set to false, indicating that the parameter loading procedure failed.
+      30             : * This flag can be checked using the loaded_successfully() method after all parameters were
+      31             : * attempted to be loaded (see usage example usage below).
+      32             : *
+      33             : * The loaded parameter names and corresponding values are printed to stdout by default
+      34             : * for user convenience. Special cases such as loading of Eigen matrices or loading
+      35             : * of std::vectors of various values are also provided.
+      36             : *
+      37             : * To load parameters into the `rosparam` server, use a launchfile prefferably.
+      38             : * See documentation of ROS launchfiles here: http://wiki.ros.org/roslaunch/XML.
+      39             : * Specifically, the `param` XML tag is used for loading parameters directly from the launchfile: http://wiki.ros.org/roslaunch/XML/param,
+      40             : * and the `rosparam` XML tag tag is used for loading parameters from a `yaml` file: http://wiki.ros.org/roslaunch/XML/rosparam.
+      41             : *
+      42             : */
+      43             : class ParamLoader
+      44             : {
+      45             : 
+      46             : private:
+      47             :   enum unique_t
+      48             :   {
+      49             :     UNIQUE = true,
+      50             :     REUSABLE = false
+      51             :   };
+      52             :   enum optional_t
+      53             :   {
+      54             :     OPTIONAL = true,
+      55             :     COMPULSORY = false
+      56             :   };
+      57             :   enum swap_t
+      58             :   {
+      59             :     SWAP = true,
+      60             :     NO_SWAP = false
+      61             :   };
+      62             : 
+      63             :   template <typename T>
+      64             :   using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+      65             : 
+      66             : private:
+      67             :   bool m_load_successful, m_print_values;
+      68             :   std::string m_node_name;
+      69             :   std::string m_prefix;
+      70             :   const ros::NodeHandle& m_nh;
+      71             :   mrs_lib::ParamProvider m_pp;
+      72             :   std::unordered_set<std::string> m_loaded_params;
+      73             : 
+      74             :   /* printing helper functions //{ */
+      75             :   /* printError and print_warning functions //{*/
+      76          17 :   void printError(const std::string& str)
+      77             :   {
+      78          17 :     if (m_node_name.empty())
+      79          17 :       ROS_ERROR_STREAM(str);
+      80             :     else
+      81           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: " << str);
+      82          17 :   }
+      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           3 :   void printValue(const std::string& name, const T& value)
+      96             :   {
+      97           3 :     if (m_node_name.empty())
+      98           3 :       std::cout << "\t" << name << ":\t" << value << std::endl;
+      99             :     else
+     100           0 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << value);
+     101           3 :   }
+     102             : 
+     103             :   template <typename T>
+     104           1 :   void printValue(const std::string& name, const std::vector<T>& value)
+     105             :   {
+     106           2 :     std::stringstream strstr;
+     107           1 :     if (m_node_name.empty())
+     108           1 :       strstr << "\t";
+     109           1 :     strstr << name << ":\t";
+     110           1 :     size_t it = 0;
+     111           5 :     for (const auto& elem : value)
+     112             :     {
+     113           4 :       strstr << elem;
+     114           4 :       if (it < value.size() - 1)
+     115           3 :         strstr << ", ";
+     116           4 :       it++;
+     117             :     }
+     118           1 :     if (m_node_name.empty())
+     119           1 :       std::cout << strstr.str() << std::endl;
+     120             :     else
+     121           0 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << strstr.str());
+     122           1 :   }
+     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           3 :   void printValue(const std::string& name, const MatrixX<T>& value)
+     147             :   {
+     148           6 :     std::stringstream strstr;
+     149             :     /* const Eigen::IOFormat fmt(4, 0, ", ", "\n", "\t\t[", "]"); */
+     150             :     /* strstr << value.format(fmt); */
+     151           9 :     const Eigen::IOFormat fmt;
+     152           3 :     strstr << value.format(fmt);
+     153           3 :     if (m_node_name.empty())
+     154           3 :       std::cout << "\t" << name << ":\t" << std::endl << strstr.str() << std::endl;
+     155             :     else
+     156           0 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':" << std::endl << strstr.str());
+     157           3 :   }
+     158             : 
+     159           3 :   std::string printValue_recursive(const std::string& name, XmlRpc::XmlRpcValue& value, unsigned depth = 0)
+     160             :   {
+     161           6 :     std::stringstream strstr;
+     162           6 :     for (unsigned it = 0; it < depth; it++)
+     163           3 :       strstr << "\t";
+     164           3 :     strstr << name << ":";
+     165           3 :     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           1 :       default:
+     189             :         {
+     190           1 :           strstr << "\t" << value;
+     191           1 :           break;
+     192             :         }
+     193             :     }
+     194           6 :     return strstr.str();
+     195             :   }
+     196             : 
+     197           1 :   void printValue(const std::string& name, XmlRpc::XmlRpcValue& value)
+     198             :   {
+     199           2 :     const std::string txt = printValue_recursive(name, value);
+     200           1 :     if (m_node_name.empty())
+     201           1 :       std::cout << txt << std::endl;
+     202             :     else
+     203           0 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << txt);
+     204           1 :   }
+     205             : 
+     206             :   //}
+     207             :   
+     208           9 :   std::string resolved(const std::string& param_name)
+     209             :   {
+     210           9 :     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          19 :   bool check_duplicit_loading(const std::string& name)
+     216             :   {
+     217          19 :     if (m_loaded_params.count(name))
+     218             :     {
+     219          12 :       printError(std::string("Tried to load parameter ") + name + std::string(" twice"));
+     220          12 :       m_load_successful = false;
+     221          12 :       return true;
+     222             :     } else
+     223             :     {
+     224           7 :       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          14 :   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          28 :     const std::string name_prefixed = m_prefix + name;
+     247          14 :     MatrixX<T> loaded = default_value;
+     248             :     // first, check if the user already tried to load this parameter
+     249          14 :     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           2 :     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           2 :     bool expect_zero_matrix = rows == 0;
+     261           2 :     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           2 :     bool cur_load_successful = true;
+     273           2 :     bool check_size_exact = true;
+     274           2 :     if (cols <= 0)  // this means that the cols dimension is dynamic or a zero matrix is expected
+     275           1 :       check_size_exact = false;
+     276             : 
+     277           4 :     std::vector<T> tmp_vec;
+     278             :     // try to load the parameter
+     279           2 :     bool success = m_pp.getParam(name_prefixed, tmp_vec);
+     280             :     // check if the loaded vector has correct length
+     281           2 :     bool correct_size = (int)tmp_vec.size() == rows * cols;
+     282           2 :     if (!check_size_exact && !expect_zero_matrix)
+     283           0 :       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           2 :     if (success && correct_size)
+     286             :     {
+     287             :       // if successfully loaded, everything is in order
+     288             :       // transform the vector to the matrix
+     289           2 :       if (cols <= 0 && rows > 0)
+     290           0 :         cols = tmp_vec.size() / rows;
+     291           2 :       if (swap)
+     292             :       {
+     293           0 :         int tmp = cols;
+     294           0 :         cols = rows;
+     295           0 :         rows = tmp;
+     296             :       }
+     297           2 :       loaded = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>, Eigen::Unaligned>(tmp_vec.data(), rows, cols);
+     298             :     } else
+     299             :     {
+     300           0 :       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           0 :       loaded = default_value;
+     315           0 :       if (!optional)
+     316             :       {
+     317             :         // if the parameter was compulsory, alert the user and set the flag
+     318           0 :         printError(std::string("Could not load non-optional parameter ") + resolved(name_prefixed));
+     319           0 :         cur_load_successful = false;
+     320             :       }
+     321             :     }
+     322             : 
+     323             :     // check if load was a success
+     324           2 :     if (cur_load_successful)
+     325             :     {
+     326           2 :       if (m_print_values && printValues)
+     327           1 :         printValue(name_prefixed, loaded);
+     328           2 :       m_loaded_params.insert(name_prefixed);
+     329             :     } else
+     330             :     {
+     331           0 :       m_load_successful = false;
+     332             :     }
+     333             :     // finally, return the resulting value
+     334           2 :     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           4 :   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           8 :     MatrixX<T> loaded = default_value;
+     409             :     // first, check that at least one dimension is set
+     410           4 :     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           4 :     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           4 :   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           8 :     MatrixX<T> loaded = default_value;
+     426             : 
+     427             :     // next, check that at least one dimension is set
+     428           4 :     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           4 :     swap_t swap = NO_SWAP;
+     436           4 :     if (rows <= 0)
+     437             :     {
+     438           0 :       int tmp = rows;
+     439           0 :       rows = cols;
+     440           0 :       cols = tmp;
+     441           0 :       swap = SWAP;
+     442             :     }
+     443           4 :     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           6 :   std::pair<T, bool> load(const std::string& name, const T& default_value, optional_t optional = OPTIONAL, unique_t unique = UNIQUE)
+     462             :   {
+     463          12 :     const std::string name_prefixed = m_prefix + name;
+     464          11 :     T loaded = default_value;
+     465           6 :     if (unique && check_duplicit_loading(name_prefixed))
+     466           0 :       return {loaded, false};
+     467             : 
+     468           6 :     bool cur_load_successful = true;
+     469             :     // try to load the parameter
+     470           6 :     const bool success = m_pp.getParam(name_prefixed, loaded);
+     471           6 :     if (!success)
+     472             :     {
+     473             :       // if it was not loaded, set the default value
+     474           1 :       loaded = default_value;
+     475           1 :       if (!optional)
+     476             :       {
+     477             :         // if the parameter was compulsory, alert the user and set the flag
+     478           1 :         printError(std::string("Could not load non-optional parameter ") + resolved(name_prefixed));
+     479           1 :         cur_load_successful = false;
+     480             :       }
+     481             :     }
+     482             : 
+     483           6 :     if (cur_load_successful)
+     484             :     {
+     485             :       // everything is fine and just print the name_prefixed and value if required
+     486           5 :       if (m_print_values)
+     487           5 :         printValue(name_prefixed, loaded);
+     488             :       // mark the param name_prefixed as successfully loaded
+     489           5 :       m_loaded_params.insert(name_prefixed);
+     490             :     } else
+     491             :     {
+     492           1 :       m_load_successful = false;
+     493             :     }
+     494             :     // finally, return the resulting value
+     495           6 :     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           2 :   ParamLoader(const ros::NodeHandle& nh, bool printValues = true, std::string_view node_name = std::string())
+     508           2 :       : m_load_successful(true),
+     509             :         m_print_values(printValues),
+     510             :         m_node_name(node_name),
+     511             :         m_nh(nh),
+     512           2 :         m_pp(nh, m_node_name)
+     513             :   {
+     514             :     /* std::cout << "Initialized1 ParamLoader for node " << node_name << std::endl; */
+     515           2 :   }
+     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             :   ParamLoader(const ros::NodeHandle& nh, std::string_view node_name)
+     525             :       : ParamLoader(nh, true, node_name)
+     526             :   {
+     527             :     /* std::cout << "Initialized2 ParamLoader for node " << node_name << std::endl; */
+     528             :   }
+     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             :   void setPrefix(const std::string& prefix)
+     551             :   {
+     552             :     m_prefix = prefix;
+     553             :   }
+     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           1 :   bool addYamlFile(const std::string& filepath)
+     580             :   {
+     581           1 :     return m_pp.addYamlFile(filepath);
+     582             :   }
+     583             :   //}
+     584             : 
+     585             :   /* addYamlFileFromParam() function //{ */
+     586             :   
+     587             :   /*!
+     588             :     * \brief Adds the specified file as a source of static parameters.
+     589             :     *
+     590             :     * \param filepath The full path to the yaml file to be loaded.
+     591             :     * \return true if loading and parsing the file was successful, false otherwise.
+     592             :     */
+     593           1 :   bool addYamlFileFromParam(const std::string& param_name)
+     594             :   {
+     595           2 :     std::string filepath;
+     596           1 :     if (!loadParam(param_name, filepath))
+     597           0 :       return false;
+     598           1 :     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           4 :   bool loadedSuccessfully()
+     609             :   {
+     610           4 :     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           1 :   bool loadParam(const std::string& name, T& out_value, const T& default_value)
+     629             :   {
+     630           1 :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     631           1 :     out_value = ret;
+     632           2 :     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             :   T loadParam2(const std::string& name, const T& default_value)
+     647             :   {
+     648             :     const auto loaded = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     649             :     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           4 :   bool loadParam(const std::string& name, T& out_value)
+     703             :   {
+     704           7 :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
+     705           4 :     out_value = ret;
+     706           7 :     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 and convenience functions for Eigen dynamic matrix type //{ */
+     843             : 
+     844             :   /*!
+     845             :     * \brief An overload for loading Eigen matrices.
+     846             :     *
+     847             :     * For compulsory Eigen matrices, use loadMatrixStatic() or loadMatrixDynamic().
+     848             :     * Matrix dimensions are deduced from the provided default value.
+     849             :     * 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),
+     850             :     * the default value is used.
+     851             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     852             :     *
+     853             :     * \param name          Name of the parameter in the rosparam server.
+     854             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     855             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     856             :     */
+     857             :   template <typename T>
+     858             :   void loadParam(const std::string& name, MatrixX<T>& mat, const MatrixX<T>& default_value)
+     859             :   {
+     860             :     mat = loadParam2(name, default_value);
+     861             :   }
+     862             : 
+     863             :   /*!
+     864             :     * \brief An overload for loading Eigen matrices.
+     865             :     *
+     866             :     * For compulsory Eigen matrices, use loadMatrixStatic() or loadMatrixDynamic().
+     867             :     * Matrix dimensions are deduced from the provided default value.
+     868             :     * 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),
+     869             :     * the default value is used.
+     870             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     871             :     *
+     872             :     * \param name          Name of the parameter in the rosparam server.
+     873             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     874             :     * \return              The loaded parameter value.
+     875             :     */
+     876             :   template <typename T>
+     877             :   MatrixX<T> loadParam2(const std::string& name, const MatrixX<T>& default_value)
+     878             :   {
+     879             :     int rows = default_value.rows();
+     880             :     int cols = default_value.cols();
+     881             :     MatrixX<T> loaded;
+     882             :     loadMatrixDynamic(name, loaded, default_value, rows, cols);
+     883             :     return loaded;
+     884             :   }
+     885             :   
+     886             :   //}
+     887             : 
+     888             :   // loadMatrixStatic function for loading of static Eigen::Matrices //{
+     889             : 
+     890             :   /*!
+     891             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+     892             :     *
+     893             :     * 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).
+     894             :     * 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),
+     895             :     * the loading process is unsuccessful.
+     896             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     897             :     *
+     898             :     * \tparam rows  Expected number of rows of the matrix.
+     899             :     * \tparam cols  Expected number of columns of the matrix.
+     900             :     *
+     901             :     * \param name  Name of the parameter in the rosparam server.
+     902             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     903             :     *
+     904             :     */
+     905             :   template <int rows, int cols, typename T>
+     906           1 :   void loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat)
+     907             :   {
+     908           1 :     mat = loadMatrixStatic2<rows, cols, T>(name);
+     909           1 :   }
+     910             : 
+     911             :   /*!
+     912             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+     913             :     *
+     914             :     * 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).
+     915             :     * 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),
+     916             :     * the default value is used.
+     917             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     918             :     *
+     919             :     * \tparam rows          Expected number of rows of the matrix.
+     920             :     * \tparam cols          Expected number of columns of the matrix.
+     921             :     *
+     922             :     * \param name          Name of the parameter in the rosparam server.
+     923             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     924             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     925             :     *
+     926             :     */
+     927             :   template <int rows, int cols, typename T, typename Derived>
+     928           1 :   void loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat, const Eigen::MatrixBase<Derived>& default_value)
+     929             :   {
+     930           1 :     mat = loadMatrixStatic2<rows, cols, T>(name, default_value);
+     931           1 :   }
+     932             : 
+     933             :   /*!
+     934             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+     935             :     *
+     936             :     * 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).
+     937             :     * 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),
+     938             :     * the loading process is unsuccessful.
+     939             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     940             :     *
+     941             :     * \tparam rows  Expected number of rows of the matrix.
+     942             :     * \tparam cols  Expected number of columns of the matrix.
+     943             :     *
+     944             :     * \param name  Name of the parameter in the rosparam server.
+     945             :     * \return      The loaded parameter value.
+     946             :     *
+     947             :     */
+     948             :   template <int rows, int cols, typename T = double>
+     949           3 :   Eigen::Matrix<T, rows, cols> loadMatrixStatic2(const std::string& name)
+     950             :   {
+     951           3 :     return loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>::Zero(), COMPULSORY, UNIQUE);
+     952             :   }
+     953             : 
+     954             :   /*!
+     955             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+     956             :     *
+     957             :     * 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).
+     958             :     * 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),
+     959             :     * the default value is used.
+     960             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     961             :     *
+     962             :     * \tparam rows          Expected number of rows of the matrix.
+     963             :     * \tparam cols          Expected number of columns of the matrix.
+     964             :     *
+     965             :     * \param name          Name of the parameter in the rosparam server.
+     966             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     967             :     * \return              The loaded parameter value.
+     968             :     *
+     969             :     */
+     970             :   template <int rows, int cols, typename T, typename Derived>
+     971           2 :   Eigen::Matrix<T, rows, cols> loadMatrixStatic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value)
+     972             :   {
+     973           2 :     return loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>(default_value), OPTIONAL, UNIQUE);
+     974             :   }
+     975             :   //}
+     976             : 
+     977             :   // loadMatrixStatic function for loading of Eigen matrices with known dimensions //{
+     978             : 
+     979             :   /*!
+     980             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+     981             :     *
+     982             :     * 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).
+     983             :     * 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),
+     984             :     * the loading process is unsuccessful.
+     985             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     986             :     *
+     987             :     * \param name  Name of the parameter in the rosparam server.
+     988             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     989             :     * \param rows  Expected number of rows of the matrix.
+     990             :     * \param cols  Expected number of columns of the matrix.
+     991             :     */
+     992             :   template <typename T>
+     993           1 :   void loadMatrixStatic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+     994             :   {
+     995           1 :     mat = loadMatrixStatic2<T>(name, rows, cols);
+     996           1 :   }
+     997             : 
+     998             :   /*!
+     999             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1000             :     *
+    1001             :     * 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).
+    1002             :     * 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),
+    1003             :     * the default value is used.
+    1004             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1005             :     *
+    1006             :     * \param name          Name of the parameter in the rosparam server.
+    1007             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1008             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1009             :     * \param rows          Expected number of rows of the matrix.
+    1010             :     * \param cols          Expected number of columns of the matrix.
+    1011             :     */
+    1012             :   template <typename T, typename Derived>
+    1013           1 :   void loadMatrixStatic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1014             :   {
+    1015           1 :     mat = loadMatrixStatic2<T>(name, default_value, rows, cols);
+    1016           1 :   }
+    1017             : 
+    1018             :   /*!
+    1019             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+    1020             :     *
+    1021             :     * 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).
+    1022             :     * 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),
+    1023             :     * the loading process is unsuccessful.
+    1024             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1025             :     *
+    1026             :     * \param name  Name of the parameter in the rosparam server.
+    1027             :     * \param rows  Expected number of rows of the matrix.
+    1028             :     * \param cols  Expected number of columns of the matrix.
+    1029             :     * \return      The loaded parameter value.
+    1030             :     */
+    1031             :   template <typename T = double>
+    1032           2 :   MatrixX<T> loadMatrixStatic2(const std::string& name, int rows, int cols)
+    1033             :   {
+    1034           2 :     return loadMatrixStatic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1035             :   }
+    1036             : 
+    1037             :   /*!
+    1038             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1039             :     *
+    1040             :     * 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).
+    1041             :     * 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),
+    1042             :     * the default value is used.
+    1043             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1044             :     *
+    1045             :     * \param name          Name of the parameter in the rosparam server.
+    1046             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1047             :     * \param rows          Expected number of rows of the matrix.
+    1048             :     * \param cols          Expected number of columns of the matrix.
+    1049             :     * \return              The loaded parameter value.
+    1050             :     */
+    1051             :   template <typename T, typename Derived>
+    1052           2 :   MatrixX<T> loadMatrixStatic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1053             :   {
+    1054           2 :     return loadMatrixStatic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1055             :   }
+    1056             :   //}
+    1057             : 
+    1058             :   // loadMatrixDynamic function for half-dynamic loading of MatrixX<T> //{
+    1059             : 
+    1060             :   /*!
+    1061             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1062             :     *
+    1063             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1064             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1065             :     * the loading process is unsuccessful.
+    1066             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1067             :     *
+    1068             :     * \param name  Name of the parameter in the rosparam server.
+    1069             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1070             :     * \param rows  Expected number of rows of the matrix (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).
+    1071             :     * \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).
+    1072             :     */
+    1073             :   template <typename T>
+    1074           1 :   void loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1075             :   {
+    1076           1 :     mat = loadMatrixDynamic2<T>(name, rows, cols);
+    1077           1 :   }
+    1078             : 
+    1079             :   /*!
+    1080             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1081             :     *
+    1082             :     * 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).
+    1083             :     * 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),
+    1084             :     * the default value is used.
+    1085             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1086             :     *
+    1087             :     * \param name          Name of the parameter in the rosparam server.
+    1088             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1089             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1090             :     * \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).
+    1091             :     * \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).
+    1092             :     */
+    1093             :   template <typename T, typename Derived>
+    1094           1 :   void loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1095             :   {
+    1096           1 :     mat = loadMatrixDynamic2<T>(name, default_value, rows, cols);
+    1097           1 :   }
+    1098             : 
+    1099             :   /*!
+    1100             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1101             :     *
+    1102             :     * 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).
+    1103             :     * 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),
+    1104             :     * the loading process is unsuccessful.
+    1105             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1106             :     *
+    1107             :     * \param name  Name of the parameter in the rosparam server.
+    1108             :     * \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).
+    1109             :     * \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).
+    1110             :     * \return      The loaded parameter value.
+    1111             :     */
+    1112             :   template <typename T = double>
+    1113           2 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, int rows, int cols)
+    1114             :   {
+    1115           2 :     return loadMatrixDynamic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1116             :   }
+    1117             : 
+    1118             :   /*!
+    1119             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1120             :     *
+    1121             :     * 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).
+    1122             :     * 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),
+    1123             :     * the default value is used.
+    1124             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1125             :     *
+    1126             :     * \param name          Name of the parameter in the rosparam server.
+    1127             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1128             :     * \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).
+    1129             :     * \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).
+    1130             :     * \return              The loaded parameter value.
+    1131             :     */
+    1132             :   template <typename T, typename Derived>
+    1133           2 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1134             :   {
+    1135           2 :     return loadMatrixDynamic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1136             :   }
+    1137             : 
+    1138             :   //}
+    1139             : 
+    1140             :   // loadMatrixArray function for loading of an array of MatrixX<T> with known dimensions //{
+    1141             :   /*!
+    1142             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1143             :     *
+    1144             :     * 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
+    1145             :     * 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
+    1146             :     * 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
+    1147             :     * \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.
+    1148             :     * A typical structure of a \c yaml file, specifying the
+    1149             :     * matrix array to be loaded using this method, is
+    1150             :     *
+    1151             :     * \code{.yaml}
+    1152             :     *
+    1153             :     * matrix_array:
+    1154             :     *   rows: 3
+    1155             :     *   cols: [1, 2]
+    1156             :     *   data: [-5.0, Eigen::Dynamic0.0, 23.0,
+    1157             :     *          -5.0, Eigen::Dynamic0.0, 12.0,
+    1158             :     *           2.0,   4.0,  7.0]
+    1159             :     *
+    1160             :     * \endcode
+    1161             :     *
+    1162             :     * 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.
+    1163             :     *
+    1164             :     * 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).
+    1165             :     * 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),
+    1166             :     * the loading process is unsuccessful.
+    1167             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1168             :     *
+    1169             :     * \param name  Name of the parameter in the rosparam server.
+    1170             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1171             :     *
+    1172             :     */
+    1173             :   template <typename T>
+    1174           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat)
+    1175             :   {
+    1176           1 :     mat = loadMatrixArray2<double>(name);
+    1177           1 :   }
+    1178             : 
+    1179             :   /*!
+    1180             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1181             :     *
+    1182             :     * 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
+    1183             :     * found in the \c rosparam server, instead of causing an unsuccessful load. This makes specifying the parameter value in the \c rosparam server optional.
+    1184             :     *
+    1185             :     * \param name           Name of the parameter in the rosparam server.
+    1186             :     * \param mat            Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1187             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1188             :     *
+    1189             :     */
+    1190             :   template <typename T>
+    1191           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat, const std::vector<MatrixX<T>>& default_value)
+    1192             :   {
+    1193           1 :     mat = loadMatrixArray2(name, default_value);
+    1194           1 :   }
+    1195             : 
+    1196             :   /*!
+    1197             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1198             :     *
+    1199             :     * This method works in the same way as the loadMatrixArray() method for compulsory parameters, except that the loaded
+    1200             :     * parameter is returned and not stored in the reference parameter.
+    1201             :     *
+    1202             :     * \param name           Name of the parameter in the rosparam server.
+    1203             :     * \returns              The loaded parameter or a default constructed object of the respective type.
+    1204             :     *
+    1205             :     */
+    1206             :   template <typename T = double>
+    1207           3 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name)
+    1208             :   {
+    1209           3 :     return loadMatrixArray_internal(name, std::vector<MatrixX<T>>(), COMPULSORY, UNIQUE);
+    1210             :   }
+    1211             : 
+    1212             :   /*!
+    1213             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1214             :     *
+    1215             :     * This method works in the same way as the loadMatrixArray() method for optional parameters, except that the loaded
+    1216             :     * parameter is returned and not stored in the reference parameter.
+    1217             :     *
+    1218             :     * \param name           Name of the parameter in the rosparam server.
+    1219             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1220             :     * \returns              The loaded parameter or the default value.
+    1221             :     *
+    1222             :     */
+    1223             :   template <typename T>
+    1224           2 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name, const std::vector<MatrixX<T>>& default_value)
+    1225             :   {
+    1226           2 :     return loadMatrixArray_internal(name, default_value, OPTIONAL, UNIQUE);
+    1227             :   }
+    1228             :   //}
+    1229             : 
+    1230             :   //}
+    1231             : 
+    1232             : };
+    1233             : //}
+    1234             : 
+    1235             :   /*!
+    1236             :     * \brief An overload for loading ros::Duration.
+    1237             :     *
+    1238             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1239             :     *
+    1240             :     * \param name          Name of the parameter in the rosparam server.
+    1241             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1242             :     * \return              The loaded parameter value.
+    1243             :     */
+    1244             :   template <>
+    1245             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name, const ros::Duration& default_value);
+    1246             : 
+    1247             :   /*!
+    1248             :     * \brief An overload for loading ros::Duration.
+    1249             :     *
+    1250             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1251             :     *
+    1252             :     * \param name          Name of the parameter in the rosparam server.
+    1253             :     * \return              The loaded parameter value.
+    1254             :     */
+    1255             :   template <>
+    1256             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name);
+    1257             : 
+    1258             : }  // namespace mrs_lib
+    1259             : 
+    1260             : #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..1e40241a4b --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html @@ -0,0 +1,335 @@ + + + + + + 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 + + +
+ 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..76c7b7685e475c9f9512697ad54545838239633f GIT binary patch literal 4201 zcmV-v5SH(WP)C=52hE5sW(D08|XWcY*Tt5go2&4u|4N<9B1!G8u+~IL;_26$wN=>HwDTgDfIPv-XHL zz8Sm=KrbNi2n@df$Na(t1dDIU5qWSF;p=R_+ z;6g1xH41HuBh(K@Z6y?7y5iAdRA0-vsza(=xP{3f$y5cXf;3266%>qv(T!$MLfQtS zVbn4h-NeMKB4{GJnzZ&p+Z}p26?id9%Y56+57@jl&_m%FXqT?M=m#-FPRc)KeBTYnaSkH_Ob zx}LB2>-FFBzvuk=^`J{Xm>+yk#s~=idn_*Zcr9MDp6<=q7>cO-alYmAIPx|V+TCT&WbbD*25dWWY^*qlMrrG%5UnD!F&rcGg-4x_p z!}}N5$I~(xCE$L6-|5|yNE-w$5Y|6sotZcn3eq!NsV)`3sW*=^;Dj=n8c2Ttb@H(c zW|q&GMATq4Dn&v$Hc=+h3N|f+u^X58y`*xaaV6(|u5m#wP|W~1U87uu3cesRU|YhD z2%DdhWn~woS7pNq4x0t_Pej0=sgS5e5fj}ypshN?9>?7J^tJqzs`H`8Dx>U?*0HHC z#Vu1fNK&;^{3gHz)+y}QJ|_%DodQTF z)h4ZV&FZU`Du{bV($bwwOJ|V*n!|9@7>*3Xsd02lg>-aYmrGB-rX8gI z@a7-bfReOACLfrBB-?nwI$kuBrv!Yx8mn=_xH>lTP+FJEDyb>?ii&t-ws?dbZ_25e z0aYkx@u(!80k$NLfL)2tfGUZ*Gunm9E3cXt4nhiC6bp?W4I0;v$|UCy{R1a7_tfdQ zk20MZTM2`q%Y_I#!7K4N0%r{mM57+_NG*CE<)~pwNS;%Apv*Gy_>8@Y z2Slmonvfi__+X6cVI+=vfa4mi zRgBYAWRJ+EZ0_p;b(g!960Zorl}6aWs3$!E@4Zc5VpGI~nlNESFydeJdW$~lxURx& z;iN`bC>G*Py*+DZxWlM*tJ{&ilts)uutn0$6l^UL+W*+HL~DpsZN0og=cAi)S*nlj zP7Yh8sSY#;hc1o^qwt?1JiuM@6x1Z0$n(OjWKNEXVLERj_DrSf7#1KhzyN>@KpXY) z+$>uNVCvIGbNy{Aw9L$eg6^nUxmoGHx?joY!&5hi@U;L@QDjA)EzWZ?47<^=sU{Rh zU=&xI$2-OU&R~SPjI7+us7ZVNUPijU@DnTctt&n#PSQ4<@-54N`h^@g#zCFgH9+XE zHMY=SBd~Wo#VIajv1TXd4%@#MQrxnZqP4QUiktbT@ifKN%F)qr0yyE0p|Kd-SHs(L zoiu!^^-zR(aK)!19+MmueRB{`zi-ZfJt0{z8kSSv@-bFV-f;mo-#0hd(D~=#KzAdc`pWcT)CcyYoOXTYmMttcX9jE1E|1F2 z2Qt}RpSj(ZdoL%Eh{G1ldk)xWu~~RLd@Jzb%Pvm(3Us;RQA5~_&(>#JxNYk*W10>0 z0AgASz_vy6s88AJ+EWRez^w~8i=8MX>GxG1!=Oc}Cmk?{=ml9D#suwA*~N3ZK;@l$ za2e$n1A<{{5tsJ~Fg-_*w<2)YAf%R^j>GJrT?!$%pDJw{g1gc8w2grDQMd3C$;$$= z$!8zJvBzz6!mNs0^hnX$$D9*S=0+!V!f_7T3S4dyE@flXaKtM1kl@%rwa(P=gA_MA zlg}YT+{&4xC|x&h!+Cd|Qk=0%;yc%3TNB-;F^H*dipu!xhx}Qss~>**NfNA@55Z_u zGf^1w^DXm9uqtlAO`5njBcm~6(`yeZ3UAcgH-+>Bza9(kbO36MZ`U5V+)=6x0$MPN zM=ocNVD-}Zn|x2G+r6DdC!Fs=7}bJUikv7)x5HT)xT=JWbSX=2!Rm0Sw0FD_9T{4|QT9 zW5SpoZqn^ci-vCx@B}OTnu5+0Od9UV*)PX+w1!j9h0ALc>A*ZiKqX~(B>6muti>kc zV<14J)8SD~`QsIrv(zC_82kG0HlV0eq?-He)fNqpGf~s>0hkOy_hk{%GOE{!ra(GC zGe_@&jrM1n!XNPAg@P~q_&FNx5B;{7Unl|4^2-`vWB91qO9a$owAu{KuAF|{gXgZ1 z_h`b$v2Ct!!MOyuK+43D?F}vQsm!BugR0J&TYeJ+||T%^KwT5>M;i7J}Q~_ zXd1%+XDIxJomU^zbm4hMI~a0K(g3XcDO7;T9~Rvl`4#}1U(H>TKGL|<*H^=TJxggR zt{jVY(7P3PN$H~$SB-H!d<@$o$Zq3W+Lz6^P(}H>v`O&;&h0mR`XzIPvLRJX8fwyt@$Hi5+PeWgUSgFr}l#AW=Ix3bI-?NVfe%q@gfWdp4GwI8~}%{ zreR+!e0)veS4`n8Ax>!uLSq9du&@G{LW?O3S*>>fU?JBwAddx2(l6>wCB8otU2(Wz z+CufBCp(EpCgX)|v&a))ZcEQzoSH-XC$sQwi>F|pR2|F-o?>Q#1jPQL3rKz0<0RCv!n`I9(P$G4nt7;Ju5_vSGK}$h!f)Z3n4D_Qfm|G zJ;t?m^uF=!m)Vn+_kCF{Z50vkPx$iWZ;1Q%LYyXYF&}M9d}oO3TcP>5+rd`&dj({W zb9!cb_8VQ>;yJX6LyRPu)gZhIxE|Th-6&Zmn)K*Hn-yk>CZhY6dw(nzzN_Yvq+51V z0bJV62Ajc`s{-rWOuD(#W;h0Zi?a_5A4f(7_-w!CvYSHt*=5TwVi36-{WzkODl^4z zDfKzw<1JA^eD3gNz+U*M+l=^T$YX3q#*u3jKA@t3-*^loMvR9WW$hE8R z0glN(E&-)T0j2xR6JqcJzIt;{5?5qz1bEdX?sBH^Ig!gHj8(fSNmuQr4p`WYJG<~% zwV8Nxr_G35Zz902&3sDuI5H|g5_g5dNA>~m$L_+1j~Q?HGDiLNp{DRT;p51t5ZJxH zyYN|lW1~&OTQ(!($a`%@jS(v(E|3DG38Jgu4>MQ#=}a0vtL^wzi>7D(fX+K zj}^rwL790x!I5?p*ZQUCEyd+0?!WtcDvrjeUb#DdEdTuDjS_jRS$1p%HW`;@Km!G* zl`G5kwAVj8*H@lG!@_-_md;VGU?yLmUcUYZHX*_#0I_t(00000NkvXXu0mjf0y7iL 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..e48de54988 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + 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:33100.0 %
Date:2023-11-30 22:31:42Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler_impl()2
+
+
+ + + +
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..7a4a76a0a5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html @@ -0,0 +1,88 @@ + + + + + + + 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:33100.0 %
Date:2023-11-30 22:31:42Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler_impl()2
+
+
+ + + +
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..4696cf6dbb --- /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:33100.0 %
Date:2023-11-30 22:31:42Functions:22100.0 %
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           2 :   ~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             :   PublisherHandler(void){};
+     102             : 
+     103             :   /**
+     104             :    * @brief generic destructor
+     105             :    */
+     106           2 :   ~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           2 :   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..f4a380fc43468404c91a883f5263125fc6b4f3d4 GIT binary patch literal 572 zcmV-C0>k}@P)`Ou6E&^+VFu3GU(v{j=_x7-V=Vnj9cXxzkvkO0a>p`uVt&VD2tlv*_C)rJESoUjoM6r3;-w-;n0WTV- z_t$NE9Rq!WMq}xraOkdUP*vcS?KPaZs_EZqujfS4x3t$n*L-Drtt~Jfn!pBZ=L`h2 zMh_oev3~}-8XcuVcIpDc9mz%Q1-he-#(Ufo_vjqNU(fXx?@|8J`}FS0N-=$k1AzB% zCIau_Oa$J;nRM9v&~Wx4$1&&^#uTRUzwbWjWb{epk=91#5cdyPE^=|I!mz9W0000< KMNUMnLSTZD$pb$C 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..bc2edaafbe --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2023-11-30 22:31:42Functions:2020100.0 %
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>::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&)91
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&)221
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&)287
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&)287
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&)574
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&)574
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)633
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&)2048
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&)5124
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&)15164
+
+
+ + + +
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..992deda63b --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2023-11-30 22:31:42Functions:2020100.0 %
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&)15164
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&)5124
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)633
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&)2048
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&)287
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&)574
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&)91
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&)287
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&)221
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&)574
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
+
+
+ + + +
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..08acf6d2c9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.html @@ -0,0 +1,637 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2023-11-30 22:31:42Functions:2020100.0 %
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       15062 :       do
+      81             :       {
+      82       15164 :         cur_sc.stamp = hist_it->stamp;
+      83             :         // do the correction if current history point is a measurement
+      84       15164 :         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       15164 :         ros::Time next_stamp = to_stamp;
+      90       15164 :         if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
+      91       15062 :           next_stamp = (hist_it + 1)->stamp;
+      92             : 
+      93             :         // do the prediction
+      94       15164 :         cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
+      95       15164 :         cur_stamp = next_stamp;
+      96             : 
+      97             :         // increment the history pointer
+      98       15164 :         hist_it++;
+      99       15164 :       } 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         574 :     std::enable_if_t<check, statecov_t> predictTo(const ros::Time& to_stamp)
+     118             :     {
+     119         574 :       assert(!m_history.empty());
+     120         574 :       const auto& info = m_history.front();
+     121         574 :       auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
+     122         574 :       sc.stamp = to_stamp;
+     123         574 :       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        5224 :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     151        5024 :           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         221 :     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         221 :       if (m_history.empty())
+     170           2 :         m_history.push_back({stamp});
+     171         221 :       m_history.front().u = u;
+     172         221 :       m_history.front().Q = Q;
+     173         221 :       m_history.front().stamp = stamp;
+     174         221 :       m_history.front().predict_model = model;
+     175         221 :     }
+     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          91 :     std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     221             :     {
+     222          91 :       if (m_history.empty())
+     223           0 :         m_history.push_back({stamp});
+     224          91 :       m_history.front().u = u;
+     225          91 :       m_history.front().stamp = stamp;
+     226          91 :       m_history.front().predict_model = model;
+     227          91 :     }
+     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         287 :     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         287 :       if (m_history.empty())
+     325           0 :         m_history.push_back({stamp});
+     326         287 :       auto& info = m_history.front();
+     327         287 :       const ros::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
+     328         287 :       const auto sc = predictTo(to_stamp);
+     329         287 :       info.z = z;
+     330         287 :       info.R = R;
+     331         287 :       info.stamp = to_stamp;
+     332         287 :       info.is_measurement = true;
+     333         287 :       info.meas_id = meas_id;
+     334         287 :       info.correct_model = model;
+     335         287 :       m_sc = correctFrom(sc, info);
+     336         287 :     }
+     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         635 :       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        5124 :       void updateUsing(const info_t& info)
+     431             :       {
+     432        5124 :         u = info.u;
+     433        5124 :         Q = info.Q;
+     434        5124 :         predict_model = info.predict_model;
+     435        5124 :       };
+     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        2048 :     static bool earlier(const info_t& ob1, const info_t& ob2)
+     526             :     {
+     527        2048 :       return ob1.stamp < ob2.stamp;
+     528             :     }
+     529             :     //}
+     530             : 
+     531             :     /* predictFrom() method //{ */
+     532       15738 :     statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const ros::Time& from_stamp, const ros::Time& to_stamp)
+     533             :     {
+     534       31476 :       const auto model = inpt.predict_model == nullptr ? m_default_model : inpt.predict_model;
+     535       15738 :       const auto dt = (to_stamp - from_stamp).toSec();
+     536       31476 :       return model->predict(sc, inpt.u, inpt.Q, dt);
+     537             :     }
+     538             :     //}
+     539             : 
+     540             :     /* correctFrom() method //{ */
+     541        5437 :     statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
+     542             :     {
+     543        5437 :       assert(meas.is_measurement);
+     544       10874 :       const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
+     545        5437 :       auto sc_tmp = sc;
+     546       10874 :       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/service_client_handler.h.func-sort-c.html b/mrs_lib/include/mrs_lib/service_client_handler.h.func-sort-c.html new file mode 100644 index 0000000000..d26b2d292b --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2023-11-30 22:31:42Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()4
+
+
+ + + +
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..fbd69cfec2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2023-11-30 22:31:42Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()4
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()2
+
+
+ + + +
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..76588a8be0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html @@ -0,0 +1,327 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2023-11-30 22:31:42Functions:33100.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           2 :   ~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           2 :   ServiceClientHandler(void){};
+     135             : 
+     136             :   /**
+     137             :    * @brief generic destructor
+     138             :    */
+     139           4 :   ~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..caca6e40c3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html @@ -0,0 +1,160 @@ + + + + + + + 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:314077.5 %
Date:2023-11-30 22:31:42Functions:132065.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()0
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::PointStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)> const&)1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))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> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)1
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()2
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const10000
+
+
+ + + +
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..aeca4380aa --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html @@ -0,0 +1,160 @@ + + + + + + + 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:314077.5 %
Date:2023-11-30 22:31:42Functions:132065.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()0
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&)1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))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()2
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)1
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()1
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() const10000
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::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
+
+
+ + + +
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..a0c70f508e --- /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:314077.5 %
Date:2023-11-30 22:31:42Functions:132065.0 %
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           1 :     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           0 :       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       10000 :       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           0 :       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           5 :       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           0 :       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           5 :       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           1 :       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           1 :       SubscribeHandler(
+     199             :             const SubscribeHandlerOptions& options,
+     200             :             const std::string& topic_name,
+     201             :             Types ... args
+     202             :           )
+     203             :       :
+     204             :         SubscribeHandler(
+     205           1 :             [options, topic_name]()
+     206             :             {
+     207           2 :               SubscribeHandlerOptions opts = options;
+     208           1 :               opts.topic_name = topic_name;
+     209           2 :               return opts;
+     210             :             }(),
+     211             :             args...
+     212           1 :             )
+     213             :       {
+     214           1 :       }
+     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           1 :       SubscribeHandler(
+     224             :             const SubscribeHandlerOptions& options,
+     225             :             const message_callback_t& message_callback = {}
+     226             :           )
+     227           1 :       {
+     228           1 :         if (options.threadsafe)
+     229             :         {
+     230           1 :           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           1 :         if (options.autostart)
+     245           0 :           start();
+     246           1 :       };
+     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             :       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             :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     324             :             args...
+     325             :             )
+     326             :       {
+     327             :       }
+     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           2 :       ~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             :       SubscribeHandler(SubscribeHandler&& other)
+     419             :       {
+     420             :         this->m_pimpl = std::move(other.m_pimpl);
+     421             :         other.m_pimpl = nullptr;
+     422             :       }
+     423           1 :       SubscribeHandler& operator=(SubscribeHandler&& other)
+     424             :       {
+     425           1 :         this->m_pimpl = std::move(other.m_pimpl);
+     426           1 :         other.m_pimpl = nullptr;
+     427           1 :         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..0daf1ec06f1f45f43575edb514042ca4d045f69e GIT binary patch literal 1770 zcmVm&+nau*4PsJHIVTj4AgkBf~i@q$L%(M zUK+>Ly4VyH(`jbPMEwg{K&KyWPgrB3WD%{2H3RK&%3MGVg zwb7(DXpArhY0jB_ZG<{gl3t8y6b{oiNoI0J?$9_p%iRLG&TWX}$gR|#&bcO}sK%;u z?65610tDcplwC6e-(yFy#yg#36Vh6@VHVRf1+f*l@;c zdK%iR1S1J6#a+rK3yE@taRUI6->`*{S7Fc@6BE60GVYA}V{>E2F#J%@G+lb!8C%qb zoH;KZX;=~lOnQoyl!ht@p=&imlBBU32AKHPrXy`M4AckB2j=JJ=ly>F{`q_Vy36s! z*Y0%kH4D0q=^(%*;)=58gJ=GbOQ*t9jm4(Mpm-{R5sg?H>sW3^6p+yL!E-4S!=LHk z6~AsWMf|{P^eZ$k{5$YS^iV$4DJ9|b{X5aaz;KPLY!-)yfnn{B!&B#z!RmfLm3xv2 z7olz$@OHpMqkt)tna+Txqz`kCrRQ_u$vj1Aj)yzop@PEZ)>1IYJwiKe0y>j>bY`eP zNg0s#L=VH88o1CAjvoDN5YYJpI}gMwh>BoDve5jx2}p; zGHq7GQ@@JdQ31$^e>Iu-aSN{uhjBEM{C0FimkWSrpP;EN10AsG#6itG-_jVzv0X*o zE@2$coFVB@DM@M7gaS92z7xe3S`?J@7dinwFH20NXCi=exDIeUF>tBvL&2EFSy?un zlw@`-I~6juH*m@-AGRpjHcq+|%95a+5Ncy+ol82nD`mRyHtZSvvYbSTwYW{J+Xa?E zqltJn-EjgG-?kgx!Cq^9G=ygep%t`yfiq#Pfm{gUeqqbJ5xll{6l=ku3FLF}k(5@sz{mHyO9(c0A8K|{syJ!&M}$oaf_tBPpL8msq6^aG^tMNoUzRtV$jv7euXs_BsP(EzTV(} z3nGHJz|1HAN+q?nRxX^%xb7|tz478Yo`!LIlv|t5)y5uCA1nm90TRPC?}gfU@b2Dp zS|yc?%pl8Or9d)%_wP8Bdu;x<3*z@Iu6aBSrFe@t6g2Mxtq7Xf;_vl~8fXJ|zTdiZfMBUKFDU>0l$ zNUv$z*MsHbYOU9!6g+bOYw59E5_m}C;YE1kzYf}&Q&yvSvA*y2f46i!=G{Vl!~g&Q M07*qoM6N<$f>RStmH+?% 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..f9bf5338b5 --- /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:2023-11-30 22:31:42Functions: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..4e00e436ac --- /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:2023-11-30 22:31:42Functions: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..8f8ce845f1 --- /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:2023-11-30 22:31:42Functions: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           1 :       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          20 :       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           5 :       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:2023-11-30 22:31:42Functions: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..87f1532306 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.func.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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..0ce18481e6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.gcov.html @@ -0,0 +1,328 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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:485784.2 %
Date:2023-11-30 22:31:42Functions:131776.5 %
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
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
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::retryLookupNewest(bool)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&)12
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&)52
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)55
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)55
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)113
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)113
+
+
+ + + +
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..f2002acf5c --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.func.html @@ -0,0 +1,148 @@ + + + + + + + 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:485784.2 %
Date:2023-11-30 22:31:42Functions:131776.5 %
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&)113
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)55
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
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&)52
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)12
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::retryLookupNewest(bool)1
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&)113
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)55
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..7a79f1c399 --- /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:485784.2 %
Date:2023-11-30 22:31:42Functions:131776.5 %
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           1 :     void setDefaultFrame(const std::string& frame_id)
+     131             :     {
+     132           2 :       std::scoped_lock lck(mutex_);
+     133           1 :       default_frame_id_ = frame_id;
+     134           1 :     }
+     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          12 :     void setDefaultPrefix(const std::string& prefix)
+     154             :     {
+     155          24 :       std::scoped_lock lck(mutex_);
+     156          12 :       if (prefix.empty())
+     157           1 :         prefix_ = "";
+     158             :       else
+     159          11 :         prefix_ = prefix + "/";
+     160          12 :     }
+     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           1 :     void retryLookupNewest(const bool retry = true)
+     212             :     {
+     213           1 :       std::scoped_lock lck(mutex_);
+     214           1 :       retry_lookup_newest_ = retry;
+     215           1 :     }
+     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             :     std::string resolveFrame(const std::string& frame_id)
+     249             :     {
+     250             :       std::scoped_lock lck(mutex_);
+     251             :       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         113 :     static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
+     551             :     {
+     552         113 :       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          55 :     static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
+     564             :     {
+     565          55 :       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         113 :     static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
+     575             :     {
+     576         113 :       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          55 :     static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
+     588             :     {
+     589          55 :       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          52 :     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          52 :       geometry_msgs::TransformStamped ret;
+     621          52 :       frame_from(ret) = from_frame;
+     622          52 :       frame_to(ret) = to_frame;
+     623          52 :       ret.header.stamp = time_stamp;
+     624          52 :       ret.transform = tf;
+     625          52 :       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..f40c950e37942848a25e6a44347eec74d2a6093b GIT binary patch literal 2329 zcmV+!3Fh{RP)9UJ-}i4PEjjxG^m3^BFk zoEF0h315H#k=NG{=&`(Bp4HFz`pi7%!0LKs0HgUhJ)eR z**U^2Xl{|lg0MsTRD$~l*i?rj2n`~4@qEOgl{(?!ZgIgWc<^J{{32I3`+edX9CJ5Y2*yURqV373~Bu=G9@hovvBU59~~)nnrDN zeqp29JJ~RT7tzXE9l_eJtzfYlR)afzA_D}o6>W_sVi)ilQcyyJ78xw`jo|v$CNnP8 z*24g}TRESiTq`G3*&YzTG9(8gl8-!sGerYiN>$rkQ9##+%q<@mpr|U0gonyECR9h!E)xpOPYgJo+`fs0$C}SF zWDXt|PmkXE*bz%hqhXkBH07=d$HP)jgV2X44SB*jFysZ4_`rc8p&9X>sbcJg>f0w-Z@)fc_tI0N)-!JV6u)l57je0MZhb9of5RF2B86{GDV(J{>h1$ z>Lg2!jRCETJ8U7IzX{;PiK{A_OFrL6L`B4NvJYQK zL7&T32J%Bn(}I&d=*gB&1A`FjkmY=_uD(S{PCV% z-gH|9%P&T46@07E_66@9jdQAPg4Z~g_|`ckz9X)X{Q+07{Iblr0wPlsB@zb?&5GH) z@&=};#6aH`i3TCNB5@cNT!7|#yWHh=*J>goCEIyuUPR@t6j3lr(R?kl?s^wLJK!^R zcO}(Z*vSjCPj7c2t2XxWjiSWuHirFPk>acsbD4}3ykCx#IjcRShv6!x2kk!U%##yQ z^7tGD41!w=qSIaLu)-rlPhMPUA;l(|dCD}ceQFS2Z{<>FD`V(e(alG-904zeoEth3 zjVYH0QHD$_1Fs9h_(QA0of9hx*_S49S=9#OHww)shG%bRrdW})14U(ygsvnm#t6z4 zm}H?8%=;3R{Ll*^Q71Oym-tK&&{4KGH<|Hm^r1~zOEupIN;v_Ca!@i81x?m;ugU)SO z2a*tvo3PU7m`C7I$S$eV`~N@+7?mG9svklM$}{ipb-NcIy*ci!!Nc!v{+*_yJslrI zoo4>>akE{-$6MM&l~6swuPq)5juHQ??ep{V<1@8C!?q;3@$Vw|Hg=j~llZJYGrtVi zK0_R<1?`C3N6v|%=MWcU0EKEgwbem=elOfi71S@*fh5FdqM7Wri;?`&?6EZC7K)FH zbI(<7rJ(){PDHcjr(}3Z@?VC(UOJOAhr2@kO=r$*k$mf6C*vd52<56Zl`GBowLXJA zC=T2G+N1h8))|T?y6n*(+!*nZCcI{+`Ljrog5KKxe%9@8@xBWFH3U71j~|~OA9DOy z@MHaXFXUbjnoGoHJBqu(#Zj$hX>H>1c$w*btNI zycf+^#08$KkoTwGfj!q=qq(_TIgyVfXziosBQ3~XUjD|OE_=eD&tt^bvvryRQ>1Xz zq<%!~A@)4*Iw?D%m$-bZ4CVI$_Y5p(Roy9l_KT36ie|r7Sywa*LArt1eF4i3dxGb# zp>Zj`WRbOh3GtyOtA)7qi5r?vt>6II zI~Y^MH4FMwhX&+;X%J+>GS%6miNRd?_zp@DbdUM*9nl=x*jw{77a7`QA6;o74*mBD`gV+E#5UC;*Dq!7QqZUY`Tg!Xs9- + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - ukf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:040.0 %
Date:2023-11-30 22:31:42Functions: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..6db6ac2fe5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/ukf.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - ukf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:040.0 %
Date:2023-11-30 22:31:42Functions: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..417bbdaa38 --- /dev/null +++ b/mrs_lib/include/mrs_lib/ukf.h.gcov.html @@ -0,0 +1,285 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - ukf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:040.0 %
Date:2023-11-30 22:31:42Functions: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..e8c1198c50 --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - utils.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1515100.0 %
Date:2023-11-30 22:31:42Functions: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)63
+
+
+ + + +
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..570b93a191 --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - utils.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1515100.0 %
Date:2023-11-30 22:31:42Functions: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)63
+
+
+ + + +
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..9b71abf78f --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.gcov.html @@ -0,0 +1,229 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - utils.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1515100.0 %
Date:2023-11-30 22:31:42Functions: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          63 :   int signum(T val)
+     139             :   {
+     140          63 :     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/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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::AttitudeConverter::setYaw(double const&)0
mrs_lib::Vector3Converter::operator tf2::Vector3() const0
mrs_lib::AttitudeConverter::operator tf2::Transform() 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::getYaw()1
mrs_lib::AttitudeConverter::getRoll()1
mrs_lib::AttitudeConverter::getPitch()1
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)1
mrs_lib::AttitudeConverter::AttitudeConverter(mrs_lib::EulerAttitude const&)1
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()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 geometry_msgs::Quaternion_<std::allocator<void> >() const1
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const1
mrs_lib::AttitudeConverter::operator mrs_lib::EulerAttitude() const1
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::calculateRPY()6
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)63
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)63
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)64
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const133
mrs_lib::AttitudeConverter::setHeading(double const&)1000
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)1020
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1193
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const2005
mrs_lib::AttitudeConverter::getHeading()2063
mrs_lib::AttitudeConverter::getVectorZ()3002
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const3072
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)5003
mrs_lib::AttitudeConverter::validateOrientation()7221
+
+
+ + + +
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..76e61f3200 --- /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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
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&)64
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getHeading()2063
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::getVectorZ()3002
mrs_lib::AttitudeConverter::setHeading(double const&)1000
mrs_lib::AttitudeConverter::calculateRPY()6
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)63
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)63
mrs_lib::AttitudeConverter::validateOrientation()7221
mrs_lib::AttitudeConverter::getYaw()1
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> >)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)5003
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1193
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&)1020
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()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::Vector3Converter::operator tf2::Vector3() const0
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const3072
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const1
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const2005
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const1
mrs_lib::AttitudeConverter::operator tf2::Transform() const0
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const133
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..37d30b605c --- /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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
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           0 : Vector3Converter::Vector3Converter(const Eigen::Vector3d& vector3) {
+      33             : 
+      34           0 :   vector3_[0] = vector3[0];
+      35           0 :   vector3_[1] = vector3[1];
+      36           0 :   vector3_[2] = vector3[2];
+      37           0 : }
+      38             : 
+      39          64 : Vector3Converter::Vector3Converter(const geometry_msgs::Vector3& vector3) {
+      40             : 
+      41          64 :   vector3_[0] = vector3.x;
+      42          64 :   vector3_[1] = vector3.y;
+      43          64 :   vector3_[2] = vector3.z;
+      44          64 : }
+      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        3072 : Vector3Converter::operator Eigen::Vector3d() const {
+      59             : 
+      60        3072 :   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        1020 : AttitudeConverter::AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format) {
+      79             : 
+      80        1020 :   switch (format) {
+      81             : 
+      82        1015 :     case RPY_EXTRINSIC: {
+      83        1015 :       tf2_quaternion_.setRPY(roll, pitch, yaw);
+      84        1015 :       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        1020 :   validateOrientation();
+     104        1018 : }
+     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           1 : AttitudeConverter::AttitudeConverter(const geometry_msgs::Quaternion quaternion) {
+     117           1 :   tf2_quaternion_.setX(quaternion.x);
+     118           1 :   tf2_quaternion_.setY(quaternion.y);
+     119           1 :   tf2_quaternion_.setZ(quaternion.z);
+     120           1 :   tf2_quaternion_.setW(quaternion.w);
+     121             : 
+     122           1 :   validateOrientation();
+     123           1 : }
+     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           1 : AttitudeConverter::AttitudeConverter(const Eigen::Quaterniond quaternion) {
+     132           1 :   tf2_quaternion_.setX(quaternion.x());
+     133           1 :   tf2_quaternion_.setY(quaternion.y());
+     134           1 :   tf2_quaternion_.setZ(quaternion.z());
+     135           1 :   tf2_quaternion_.setW(quaternion.w());
+     136             : 
+     137           1 :   validateOrientation();
+     138           1 : }
+     139             : 
+     140        1193 : AttitudeConverter::AttitudeConverter(const Eigen::Matrix3d matrix) {
+     141        1193 :   Eigen::Quaterniond quaternion = Eigen::Quaterniond(matrix);
+     142             : 
+     143        1193 :   tf2_quaternion_.setX(quaternion.x());
+     144        1193 :   tf2_quaternion_.setY(quaternion.y());
+     145        1193 :   tf2_quaternion_.setZ(quaternion.z());
+     146        1193 :   tf2_quaternion_.setW(quaternion.w());
+     147             : 
+     148        1193 :   validateOrientation();
+     149        1193 : }
+     150             : 
+     151        5003 : AttitudeConverter::AttitudeConverter(const tf2::Quaternion quaternion) {
+     152             : 
+     153        5003 :   tf2_quaternion_ = quaternion;
+     154             : 
+     155        5003 :   validateOrientation();
+     156        5003 : }
+     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        2005 : AttitudeConverter::operator tf2::Quaternion() const {
+     170        2005 :   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           1 : AttitudeConverter::operator geometry_msgs::Quaternion() const {
+     186             : 
+     187           1 :   geometry_msgs::Quaternion geom_quaternion;
+     188             : 
+     189           1 :   geom_quaternion.x = tf2_quaternion_.x();
+     190           1 :   geom_quaternion.y = tf2_quaternion_.y();
+     191           1 :   geom_quaternion.z = tf2_quaternion_.z();
+     192           1 :   geom_quaternion.w = tf2_quaternion_.w();
+     193             : 
+     194           1 :   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         133 : AttitudeConverter::operator Eigen::Matrix3d() const {
+     206             : 
+     207         133 :   Eigen::Quaterniond quaternion(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     208             : 
+     209         266 :   return quaternion.toRotationMatrix();
+     210             : }
+     211             : 
+     212           1 : AttitudeConverter::operator std::tuple<double&, double&, double&>() {
+     213             : 
+     214           1 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     215             : 
+     216           1 :   return std::tuple<double&, double&, double&>{roll_, pitch_, yaw_};
+     217             : }
+     218             : 
+     219           1 : AttitudeConverter::operator tf2::Matrix3x3() const {
+     220             : 
+     221           1 :   return tf2::Matrix3x3(tf2_quaternion_);
+     222             : }
+     223             : 
+     224           0 : AttitudeConverter::operator tf2::Transform() const {
+     225             : 
+     226           0 :   return tf2::Transform(tf2_quaternion_);
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* getters //{ */
+     232             : 
+     233           1 : double AttitudeConverter::getYaw(void) {
+     234             : 
+     235           1 :   calculateRPY();
+     236             : 
+     237           1 :   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        2063 : double AttitudeConverter::getHeading(void) {
+     255             : 
+     256        2063 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     257             : 
+     258        2063 :   tf2::Vector3 x_new = tf2::Transform(tf2_quaternion_) * b1;
+     259             : 
+     260        2063 :   if (fabs(x_new[0]) <= 1e-3 && fabs(x_new[1]) <= 1e-3) {
+     261           1 :     throw GetHeadingException();
+     262             :   }
+     263             : 
+     264        4124 :   return atan2(x_new[1], x_new[0]);
+     265             : }
+     266             : 
+     267          63 : 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          63 :   if (fabs(heading_rate) < 1e-3) {
+     272           0 :     return 0;
+     273             :   }
+     274             : 
+     275             :   // prep
+     276          63 :   Eigen::Matrix3d R = *this;
+     277             : 
+     278             :   // construct the heading orbital velocity vector
+     279          63 :   Eigen::Vector3d heading_vector   = Eigen::Vector3d(R(0, 0), R(1, 0), 0);
+     280          63 :   Eigen::Vector3d orbital_velocity = Eigen::Vector3d(0, 0, heading_rate).cross(heading_vector);
+     281             : 
+     282             :   // projector to the heading orbital velocity vector subspace
+     283          63 :   Eigen::Vector3d b_orb = Eigen::Vector3d(0, 0, 1).cross(heading_vector);
+     284          63 :   b_orb.normalize();
+     285          63 :   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          63 :   Eigen::Vector3d projected = P * R.col(1);
+     289             : 
+     290             : 
+     291          63 :   double orbital_velocity_norm = orbital_velocity.norm();
+     292          63 :   double projected_norm        = projected.norm();
+     293             : 
+     294          63 :   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          63 :   double direction = mrs_lib::signum(orbital_velocity.dot(projected));
+     300             : 
+     301          63 :   double output_yaw_rate = direction * (orbital_velocity_norm / projected_norm);
+     302             : 
+     303          63 :   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          63 :   return output_yaw_rate;
+     310             : }
+     311             : 
+     312          63 : double AttitudeConverter::getHeadingRate(const Vector3Converter& attitude_rate) {
+     313             : 
+     314             :   // prep
+     315          63 :   Eigen::Matrix3d R = *this;
+     316          63 :   Eigen::Vector3d w = attitude_rate;
+     317             : 
+     318             :   // create the angular velocity tensor
+     319          63 :   Eigen::Matrix3d W;
+     320          63 :   W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
+     321             : 
+     322             :   // R derivative
+     323          63 :   Eigen::Matrix3d R_d = R * W;
+     324             : 
+     325             :   // atan2 derivative
+     326          63 :   double rx = R(0, 0);  // x-component of body X
+     327          63 :   double ry = R(1, 0);  // y-component of body Y
+     328             : 
+     329          63 :   double denom = rx * rx + ry * ry;
+     330             : 
+     331          63 :   if (fabs(denom) <= 1e-5) {
+     332           0 :     ROS_ERROR("[AttitudeConverter]: getHeadingRate(): denominator near zero!!!");
+     333           0 :     throw MathErrorException();
+     334             :   }
+     335             : 
+     336          63 :   double atan2_d_x = -ry / denom;
+     337          63 :   double atan2_d_y = rx / denom;
+     338             : 
+     339             :   // atan2 total differential
+     340          63 :   double heading_rate = atan2_d_x * R_d(0, 0) + atan2_d_y * R_d(1, 0);
+     341             : 
+     342          63 :   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        3002 : Vector3Converter AttitudeConverter::getVectorZ(void) {
+     364             : 
+     365        3002 :   tf2::Vector3 b3 = tf2::Vector3(0, 0, 1);
+     366             : 
+     367        3002 :   tf2::Vector3 new_b3 = tf2::Transform(tf2_quaternion_) * b3;
+     368             : 
+     369        6004 :   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        1000 : AttitudeConverter AttitudeConverter::setHeading(const double& heading) {
+     404             : 
+     405             :   // get the Z unit vector after the original rotation
+     406        1000 :   Eigen::Vector3d b3 = getVectorZ();
+     407             : 
+     408             :   // check for singularity: z component of the thrust vector is 0
+     409        1000 :   if (fabs(b3[2]) < 1e-3) {
+     410           3 :     throw SetHeadingException();
+     411             :   }
+     412             : 
+     413             :   // get the desired heading as a vector in 3D
+     414         997 :   Eigen::Vector3d h(cos(heading), sin(heading), 0);
+     415             : 
+     416         997 :   Eigen::Matrix3d new_R;
+     417             : 
+     418         997 :   new_R.col(2) = b3;
+     419             : 
+     420             :   // construct the oblique projection
+     421         997 :   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        1994 :   Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+     425         997 :   A.col(0)          = projector_body_z_compl.col(0);
+     426         997 :   A.col(1)          = projector_body_z_compl.col(1);
+     427             : 
+     428             :   // create the basis of the projection null-space complement
+     429        1994 :   Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+     430         997 :   B.col(0)          = Eigen::Vector3d(1, 0, 0);
+     431         997 :   B.col(1)          = Eigen::Vector3d(0, 1, 0);
+     432             : 
+     433             :   // oblique projector to <range_basis>
+     434        1994 :   Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     435        1994 :   Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     436         997 :   Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     437             : 
+     438         997 :   new_R.col(0) = oblique_projector * h;
+     439         997 :   new_R.col(0).normalize();
+     440             : 
+     441             :   // | ------------------------- body y ------------------------- |
+     442             : 
+     443         997 :   new_R.col(1) = new_R.col(2).cross(new_R.col(0));
+     444         997 :   new_R.col(1).normalize();
+     445             : 
+     446        1994 :   return AttitudeConverter(new_R);
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : // | ------------------------ internal ------------------------ |
+     452             : 
+     453             : /* calculateRPY() //{ */
+     454             : 
+     455           6 : void AttitudeConverter::calculateRPY(void) {
+     456             : 
+     457           6 :   if (!got_rpy_) {
+     458             : 
+     459           6 :     tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     460             :   }
+     461           6 : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* validateOrientation() //{ */
+     466             : 
+     467        7221 : void AttitudeConverter::validateOrientation(void) {
+     468             : 
+     469       14440 :   if (!std::isfinite(tf2_quaternion_.x()) || !std::isfinite(tf2_quaternion_.y()) || !std::isfinite(tf2_quaternion_.z()) ||
+     470        7219 :       !std::isfinite(tf2_quaternion_.w())) {
+     471           2 :     throw InvalidAttitudeException();
+     472             :   }
+     473        7219 : }
+     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..a2cc63bf665d2d852c6e34bf2fa782eb3b5f2fe4 GIT binary patch literal 1548 zcmV+n2J`ueP)IiVs)QS}rW}kr3;A{$Kmh`~X%!`MuYzF>EI)wQv{FBsw7P<9}r4w#j>|}6e zfn&6UG%(^um{q-%CJ*c5(;;dBcAG-?z-=IpU$nK30?k84*gfDO;R845B#=-Q7a%MG(l81` zT}1>(08(hIaq9uUCAv-vta@Pn5lc4d0>`r*2-uBr^A=D=sxW4ZNJNh;VAs@rYSAxn z3Nw9EZ=P$Z!rB~+(hw;uTK_EI93vQYZF_+byA8L^?Bek&7gMy?AuJM^Z5&+Ow0! z3&nHPmUPmwhaRz^f(M!A;Xw1NTd9MqGoHr1r6#DWWk4#%mg-&#NMWWW8Vz{XTDwm{ zDF;`^W5fW-7-6Ko8z6<5JJ~gIbl1<<&QJd$E-QzQEXtLXiw2S3}d{g=L53@ymyAe%pu~rLu*kBI$lHRg(@ z*f2|e|7k$*Z$;xhz^=V`vUn6L(%61RWT=CJ4hq~N9ufFUzxn*UQsBY38V>c^QWK^P zB6*O+pxHBZfB1+15rg)C6dF^HSEsS*nx*mVwqeaa%IupY8d=vhn?R%X%M}_4Q``Re z#^hRYr4F~wa&>B&Pu&gJOQW?Jp|I*7r9_i}5j29860@!9d*1G+cKLU4_M)PD$5jhk`_g-CNt0Fz{bIDABI39N_!Ia~;3zXcfj0&2 z`rpw^P0EoXU?QHB_7RNf)_M;{xa)G*Q!0M~|9*jg!$A(V!E~R8Z+^bacVFhVW)YBp z@z31+Yyna*!qYuVaF)X!c!4f;;GmF8>@76qS;kCN*3}o2a-ACB3QIMOBqzf*7v|>P z6m!)=#(9`Jq{6i;Iae0Z-7_;hi$-QMo z>ByJc&ysT8%@j0vq+GUoUYHt|q|v4?uUn}d4g=DdSpzqaAIQ)yW&~0)AOMn>@ka)g zEiq6E`(39JpPhL|W28MG4*=&JIiM5*0Ufd#KTAim`;;5B3;b_dv_v_N{>vD0#Lt+) z_4Wv6F5FZck#fO>arM+43*#;rPrPA_rt&)*21E(TD8|n2x&FZZJ)Qf`rf1~s#WsK) y$w!V+BJc96-(@w+nz##7|4(bp8FwzCiu)gQjIX>s=zGKf0000 + + + + + + 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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 41
<unnamed>91.1 %204 / 22490.2 %37 / 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..ec1632b635 --- /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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 41
<unnamed>91.1 %204 / 22490.2 %37 / 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..9b603b6cb4 --- /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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 41
<unnamed>91.1 %204 / 22490.2 %37 / 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..5b717f437d --- /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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 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..cea45e5aa4 --- /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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 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..a69ffe6521 --- /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:20422491.1 %
Date:2023-11-30 22:31:42Functions:374190.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
91.1%91.1%
+ 		91.1 %204 / 22490.2 %37 / 41
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/conversions.cpp.func-sort-c.html b/mrs_lib/src/geometry/conversions.cpp.func-sort-c.html new file mode 100644 index 0000000000..5e719a1b6e --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2023-11-30 22:31:42Functions: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::Quaternion_<std::allocator<void> > const&)6
mrs_lib::geometry::fromEigen(Eigen::Quaternion<double, 0> const&)6
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
+
+
+ + + +
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..448f0c01e7 --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2023-11-30 22:31:42Functions: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&)6
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&)6
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..b53d178028 --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.gcov.html @@ -0,0 +1,178 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2023-11-30 22:31:42Functions: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           6 :     geometry_msgs::Quaternion fromEigen(const Eigen::Quaterniond& what)
+      48             :     {
+      49           6 :       geometry_msgs::Quaternion q;
+      50           6 :       q.x = what.x();
+      51           6 :       q.y = what.y();
+      52           6 :       q.z = what.z();
+      53           6 :       q.w = what.w();
+      54           6 :       return q;
+      55             :     }
+      56             :     
+      57           6 :     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           6 :       Eigen::Quaterniond q;
+      61           6 :       q.x() = what.x;
+      62           6 :       q.y() = what.y;
+      63           6 :       q.z() = what.z;
+      64           6 :       q.w() = what.w;
+      65           6 :       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..cfca964542 --- /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:6344514.2 %
Date:2023-11-30 22:31:42Functions:139413.8 %
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 +
48.4%48.4%
+ 		48.4 %31 / 6437.5 %6 / 16
<unnamed>48.4 %31 / 6437.5 %6 / 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..cf11b78410 --- /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:6344514.2 %
Date:2023-11-30 22:31:42Functions:139413.8 %
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 +
48.4%48.4%
+ 		48.4 %31 / 6437.5 %6 / 16
<unnamed>48.4 %31 / 6437.5 %6 / 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..0fcc367cd7 --- /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:6344514.2 %
Date:2023-11-30 22:31:42Functions:139413.8 %
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 +
48.4%48.4%
+ 		48.4 %31 / 6437.5 %6 / 16
<unnamed>48.4 %31 / 6437.5 %6 / 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..67c35f3fb1 --- /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:6344514.2 %
Date:2023-11-30 22:31:42Functions:139413.8 %
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 +
48.4%48.4%
+ 		48.4 %31 / 6437.5 %6 / 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..7e9cea4943 --- /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:6344514.2 %
Date:2023-11-30 22:31:42Functions:139413.8 %
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 +
48.4%48.4%
+ 		48.4 %31 / 6437.5 %6 / 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..53cf8bc402 --- /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:6344514.2 %
Date:2023-11-30 22:31:42Functions:139413.8 %
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 +
48.4%48.4%
+ 		48.4 %31 / 6437.5 %6 / 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..cd117524ab --- /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:316448.4 %
Date:2023-11-30 22:31:42Functions:61637.5 %
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::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 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::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 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)12
+
+
+ + + +
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..b2f171de1b --- /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:316448.4 %
Date:2023-11-30 22:31:42Functions:61637.5 %
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&)3
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)12
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&)0
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..68f3a08a94 --- /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:316448.4 %
Date:2023-11-30 22:31:42Functions:61637.5 %
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           3 :     double angleBetween(const vec3_t& vec1, const vec3_t& vec2)
+      29             :     {
+      30           3 :       const double sin_12 = vec1.cross(vec2).norm();
+      31           3 :       const double cos_12 = vec1.dot(vec2);
+      32           3 :       const double angle = std::atan2(sin_12, cos_12);
+      33           3 :       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          12 :     quat_t quaternionFromHeading(const double heading)
+      93             :     {
+      94          24 :       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           0 :     double dist(const vec3_t& a, const vec3_t& b)
+     177             :     {
+     178             : 
+     179           0 :       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..78bb2aaa8660e7cb7e848aae98079d36c6a70da8 GIT binary patch literal 746 zcmV@CtAtG!p{;E?Pp$Lk6Mjg7GoN)?SF$Sm+?qrz zE3$idg-H@QG${d#fQYi_Ts>v|*hZa&7^O(Y5BU~xS`u@JU8)8)_;|4`=wxq_cItp< zc`mG?8p5P)+bE?wrRKQX*CAdTG6D4d&zb;Q%HvS9?y{4yMV{eH*M^XYzY%;$aH?ch zQoE~8!4J)494S>=s8|yA$Y4s%P=R;uCdMv4_fTnxw`GGZX!&&7l z<>T)3xl$}-JcV%pu8eVHaAK1{&d;)WJ~}6JI@Zbau5=xV51J{BCO)=_qE#u0xNHr=z@L0!B&`D%KX6{zZXT#nZ{e z6GD^vXgrIM;reKq_0%l9OT(}iQoCJ`Yk2d9J#4RjYig-x6XcmF_?YJ@C1appYNyh^ zBi&KAzBajjWbmG%v;+8Soom#inO~$#7fDux_96wi!X`dDr_W5wnNi8+U!$$M@xx8?*MHyR%V-1RpSvQ%7zoRhlPH7|jSf5j zz9Sx;DxWU)Wf-Tw%Q caJMde0irm#>g~n&u>b%707*qoM6N<$f@`N%MgRZ+ literal 0 HcmV?d00001 diff --git a/mrs_lib/src/geometry/shapes.cpp.func-sort-c.html b/mrs_lib/src/geometry/shapes.cpp.func-sort-c.html new file mode 100644 index 0000000000..2e7f670b6f --- /dev/null +++ b/mrs_lib/src/geometry/shapes.cpp.func-sort-c.html @@ -0,0 +1,352 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - shapes.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2023-11-30 22:31:42Functions: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..29008fb25f --- /dev/null +++ b/mrs_lib/src/geometry/shapes.cpp.func.html @@ -0,0 +1,352 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - shapes.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2023-11-30 22:31:42Functions: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..e0646362c6 --- /dev/null +++ b/mrs_lib/src/geometry/shapes.cpp.gcov.html @@ -0,0 +1,730 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - shapes.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2023-11-30 22:31:42Functions:0680.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/src/math/math.cpp.gcov.png b/mrs_lib/src/math/math.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..11021a1f7a3fe1b16c9ed60780115ac61956a437 GIT binary patch literal 388 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1?!VDz0Y_(eeq$C1-LR|m<|Gx?dmcNgUef6J# zVHHpuOr7)IycEdhEbxddW?X?_wfUqO7#M{-T^vI^I^TxHN;NC+EH*!&eSr7D zzjEnZM%SO5_wU^Pv> + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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..67891d9dab --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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..a121f63f74 --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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..30a7912d2d --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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..06d8c3a6da --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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..6a84f2cff0 --- /dev/null +++ b/mrs_lib/src/median_filter/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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..3eac746b9e --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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&&)1
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::operator=(mrs_lib::MedianFilter const&)2
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)4
mrs_lib::MedianFilter::full() const21
mrs_lib::MedianFilter::add(double)26
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::check(double)30
mrs_lib::MedianFilter::median() const46
+
+
+ + + +
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..f7ac91d811 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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)26
mrs_lib::MedianFilter::check(double)30
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)1
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)4
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)2
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::full() const21
mrs_lib::MedianFilter::median() const46
+
+
+ + + +
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..f4525be031 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.html @@ -0,0 +1,286 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2023-11-30 22:31:42Functions: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           4 :   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           4 :       m_max_diff(max_diff)
+      12             :   {
+      13           4 :     m_buffer.set_capacity(buffer_length);
+      14           4 :     m_buffer_sorted.reserve(buffer_length);
+      15           4 :   }
+      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           1 :   MedianFilter::MedianFilter(MedianFilter&& other)
+      32             :   {
+      33           1 :     *this = other;
+      34           1 :   }
+      35             : 
+      36             :   //}
+      37             : 
+      38             :   /* operator=() method and overloads //{ */
+      39           2 :   MedianFilter& MedianFilter::operator=(const MedianFilter& other)
+      40             :   {
+      41           2 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      42             :   
+      43           2 :     m_buffer = other.m_buffer;
+      44           2 :     m_buffer_sorted = other.m_buffer_sorted;
+      45           2 :     m_median = other.m_median;
+      46             :   
+      47             :     // parameters specified by the user
+      48           2 :     m_min_valid = other.m_min_valid;
+      49           2 :     m_max_valid = other.m_max_valid;
+      50           2 :     m_max_diff = other.m_max_diff;
+      51             :   
+      52           4 :     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          26 :   void MedianFilter::add(const double value)
+      74             :   {
+      75          52 :     std::scoped_lock lck(m_mtx);
+      76             :     // add the value to the buffer
+      77          26 :     m_buffer.push_back(value);
+      78             :     // reset the cached median value
+      79          26 :     m_median = std::nullopt;
+      80          26 :   }
+      81             :   //}
+      82             : 
+      83             :   /* check() method //{ */
+      84          30 :   bool MedianFilter::check(const double value)
+      85             :   {
+      86          30 :     std::scoped_lock lck(m_mtx);
+      87             :     // check if all constraints are met
+      88          30 :     const double diff = m_buffer.empty() ? 0.0 : std::abs(median() - value);
+      89          60 :     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          21 :   bool MedianFilter::full() const
+     113             :   {
+     114          42 :     std::scoped_lock lck(m_mtx);
+     115          42 :     return m_buffer.full();
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* median() method //{ */
+     120          46 :   double MedianFilter::median() const
+     121             :   {
+     122          92 :     std::scoped_lock lck(m_mtx);
+     123             :     // if the value was already calculated, just return it
+     124          46 :     if (m_median.has_value())
+     125          17 :       return m_median.value();
+     126             :   
+     127             :     // check if there are even any numbers to calculate the median from
+     128          29 :     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          25 :     m_buffer_sorted.clear();
+     136             :     // copy all elements from the input buffer to buffer_sorted
+     137          25 :     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          25 :     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          25 :     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          25 :     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          25 :     if (even_set)
+     148           9 :       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          25 :     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:395570.9 %
Date:2023-11-30 22:31:42Functions: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 +
70.9%70.9%
+ 		70.9 %39 / 55100.0 %4 / 4
<unnamed>70.9 %39 / 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..9db7f1a4ca --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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 +
70.9%70.9%
+ 		70.9 %39 / 55100.0 %4 / 4
<unnamed>70.9 %39 / 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..09229080be --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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 +
70.9%70.9%
+ 		70.9 %39 / 55100.0 %4 / 4
<unnamed>70.9 %39 / 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..7164691207 --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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 +
70.9%70.9%
+ 		70.9 %39 / 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..a01f2b4851 --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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 +
70.9%70.9%
+ 		70.9 %39 / 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..2169621a80 --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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 +
70.9%70.9%
+ 		70.9 %39 / 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..b6c271175e --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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&) const1
mrs_lib::ParamProvider::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)2
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const13
+
+
+ + + +
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..153a8f5dbd --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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&)2
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)2
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const13
mrs_lib::ParamProvider::getParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&) const1
+
+
+ + + +
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..daa7cbb805 --- /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:395570.9 %
Date:2023-11-30 22:31:42Functions: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           2 :   ParamProvider::ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam)
+      14           2 :   : m_nh(nh), m_node_name(std::move(node_name)), m_use_rosparam(use_rosparam)
+      15             :   {
+      16           2 :   }
+      17             : 
+      18           2 :   bool ParamProvider::addYamlFile(const std::string& filepath)
+      19             :   {
+      20             :     try
+      21             :     {
+      22           4 :       const auto loaded_yaml = YAML::LoadFile(filepath);
+      23           2 :       YAML::Node root;
+      24           2 :       root["root"] = loaded_yaml;
+      25           2 :       m_yamls.emplace_back(root);
+      26           2 :       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           1 :   bool ParamProvider::getParam(const std::string& param_name, XmlRpc::XmlRpcValue& value_out) const
+      47             :   {
+      48           1 :     if (m_use_rosparam && m_nh.getParam(param_name, value_out))
+      49           1 :       return true;
+      50             : 
+      51             :     try
+      52             :     {
+      53           0 :       const auto found_node = findYamlNode(param_name);
+      54           0 :       if (found_node.has_value())
+      55           0 :         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           0 :     return false;
+      62             :   }
+      63             : 
+      64          13 :   std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
+      65             :   {
+      66          16 :     for (const auto& yaml : m_yamls)
+      67             :     {
+      68             :       // Try to load the parameter sequentially as a map.
+      69           5 :       auto cur_node_it = std::cbegin(yaml);
+      70             :       // The root should always be a pam
+      71           5 :       if (!cur_node_it->second.IsMap())
+      72           0 :         continue;
+      73             : 
+      74           5 :       bool loaded = true;
+      75             :       {
+      76           5 :         constexpr char delimiter = '/';
+      77           5 :         auto substr_start = std::cbegin(param_name);
+      78           5 :         auto substr_end = substr_start;
+      79           2 :         do
+      80             :         {
+      81           7 :           substr_end = std::find(substr_start, std::cend(param_name), delimiter);
+      82             :           // why can't substr or string_view take iterators? :'(
+      83           7 :           const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
+      84           7 :           const auto count = std::distance(substr_start, substr_end);
+      85           7 :           const std::string param_substr = param_name.substr(start_pos, count);
+      86           7 :           substr_start = substr_end+1;
+      87             : 
+      88           7 :           bool found = false;
+      89          25 :           for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
+      90             :           {
+      91          11 :             if (node_it->first.as<std::string>() == param_substr)
+      92             :             {
+      93           4 :               cur_node_it = node_it;
+      94           4 :               found = true;
+      95           4 :               break;
+      96             :             }
+      97             :           }
+      98             : 
+      99           7 :           if (!found)
+     100             :           {
+     101           3 :             loaded = false;
+     102           3 :             break;
+     103             :           }
+     104             :         }
+     105           4 :         while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
+     106             :       }
+     107             : 
+     108           5 :       if (loaded)
+     109             :       {
+     110           4 :         return cur_node_it->second;
+     111             :       }
+     112             :     }
+     113             : 
+     114          11 :     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..715a48a05cd2c1465a26ddd996465fd90c23e0b5 GIT binary patch literal 672 zcmV;R0$=@!P)I$$yTtPRG{a5)U1Pq86s(tUL?W443au-5inuD2H*k^iZieSfnfPX=#X)3Nh)!{E3t}1UBS0jov7cYXOYgX zv>m{_SY6;=C4Edck%&rRTZ_}X@MxnTqx!h5GL48)&rJ*q<& zVwytU5)(zF+3&?%s(EE)FjCYLh98Kb25B@0*5f1Xw69OS$B71X^v#jBmp!_o??SnI zN4Bdn^!sgZS<%+o^BLR-LO_T7B2hs21xH(VfF|i& zxc5t_v@8a5*h>@wRf(id!K>;xM{P0I9*KmaD6WAC*YT+3s8JK!UbWOq(XP`Smw+x7 zBrY}MfXCXS$j-??cw}G&leaYi6nm+ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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..5e5b91d160 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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..10a659e548 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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..4307ec88c4 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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..4dbf75e5e9 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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..7468da3371 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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..ea7cab908e --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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::TimeoutManager(ros::NodeHandle const&, ros::Rate const&)2
mrs_lib::TimeoutManager::startAll(ros::Time const&)4
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)5
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)5
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)800
mrs_lib::TimeoutManager::started(unsigned long)1582
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)2098
+
+
+ + + +
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..f86aff0a00 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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)5
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)2098
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)800
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)5
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)1582
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&)2
+
+
+ + + +
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..84073acbe9 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html @@ -0,0 +1,186 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2023-11-30 22:31:42Functions: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           2 :   TimeoutManager::TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate)
+       7           2 :     : m_last_id(0)
+       8             :   {
+       9           2 :     m_main_timer = nh.createTimer(update_rate, &TimeoutManager::main_timer_callback, this);
+      10           2 :   }
+      11             : 
+      12             : 
+      13           5 :   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          10 :     std::scoped_lock lck(m_mtx);
+      16           5 :     const auto new_id = m_timeouts.size();
+      17           5 :     const timeout_info_t new_info = {oneshot, autostart, callback, timeout, last_reset, last_reset};
+      18           5 :     m_timeouts.push_back(new_info);
+      19          10 :     return new_id;
+      20             :   }
+      21             : 
+      22         800 :   void TimeoutManager::reset(const timeout_id_t id, const ros::Time& time)
+      23             :   {
+      24         800 :     std::scoped_lock lck(m_mtx);
+      25         800 :     m_timeouts.at(id).last_reset = time;
+      26         800 :   }
+      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           5 :   void TimeoutManager::start(const timeout_id_t id, const ros::Time& time)
+      35             :   {
+      36           5 :     std::scoped_lock lck(m_mtx);
+      37           5 :     m_timeouts.at(id).started = true;
+      38           5 :     m_timeouts.at(id).last_reset = time;
+      39           5 :   }
+      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        1582 :   bool TimeoutManager::started(const timeout_id_t id)
+      76             :   {
+      77        1582 :     std::scoped_lock lck(m_mtx);
+      78        3164 :     return m_timeouts.at(id).started;
+      79             :   }
+      80             : 
+      81        2098 :   void TimeoutManager::main_timer_callback([[maybe_unused]] const ros::TimerEvent &evt)
+      82             :   {
+      83        2098 :     const auto now = ros::Time::now();
+      84             : 
+      85        4196 :     std::scoped_lock lck(m_mtx);
+      86       10478 :     for (auto& timeout_info : m_timeouts)
+      87             :     {
+      88             :       // don't worry, this'll get optimized out by the compiler
+      89        8380 :       const bool started = timeout_info.started;
+      90        8380 :       const bool last_reset_timeout = now - timeout_info.last_reset >= timeout_info.timeout;
+      91        8380 :       const bool last_callback_timeout = now - timeout_info.last_callback >= timeout_info.timeout;
+      92        8380 :       if (started && last_reset_timeout && last_callback_timeout)
+      93             :       {
+      94        1578 :         timeout_info.callback(timeout_info.last_reset);
+      95        1578 :         timeout_info.last_callback = now;
+      96             :         // if the timeout is oneshot, pause it
+      97        1578 :         if (timeout_info.oneshot)
+      98           0 :           timeout_info.started = false;
+      99             :       }
+     100             :     }
+     101        2098 :   }
+     102             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..0b582c4d4a --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html @@ -0,0 +1,46 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c867c6a7540f7d57f11eda65b1d826deefd78012 GIT binary patch literal 512 zcmV+b0{{JqP)I*~y25zOg^hr1xiNM+LG`58W&G$N9I|OP#X`T26 zz@*c|b>>k1w4ja|9@(hw%rz(Ay>h5(ab;1n0$wqzKKOxp#J!%Up7I)M2LZn5^~iOmac@97IKq74SZ?s&eJvH{0Jg=d3&8 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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..1fabd69285 --- /dev/null +++ b/mrs_lib/src/timer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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..e7ad631eb9 --- /dev/null +++ b/mrs_lib/src/timer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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..601f73169e --- /dev/null +++ b/mrs_lib/src/timer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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..b3f7afb7a9 --- /dev/null +++ b/mrs_lib/src/timer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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..6f9f1d8d66 --- /dev/null +++ b/mrs_lib/src/timer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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..4e9a2ff2da --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func-sort-c.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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()214
+
+
+ + + +
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..ede9224a32 --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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()214
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..33cd46785d --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.gcov.html @@ -0,0 +1,339 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2023-11-30 22:31:42Functions: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         214 : bool ROSTimer::running()
+      50             : {
+      51         428 :   std::scoped_lock lock(mutex_timer_);
+      52             : 
+      53         214 :   if (timer_)
+      54         214 :     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/transformer/index-detail-sort-f.html b/mrs_lib/src/transformer/index-detail-sort-f.html new file mode 100644 index 0000000000..7e3f87baac --- /dev/null +++ b/mrs_lib/src/transformer/index-detail-sort-f.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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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 +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 26
<unnamed>58.9 %165 / 28065.4 %17 / 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..2322627bf9 --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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 +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 26
<unnamed>58.9 %165 / 28065.4 %17 / 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..c9f288ecc7 --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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 +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 26
<unnamed>58.9 %165 / 28065.4 %17 / 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..3d2c7208f9 --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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 +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 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..58cbeeab70 --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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 +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 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..6d179188e2 --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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 +
58.9%58.9%
+ 		58.9 %165 / 28065.4 %17 / 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..172d85580c --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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::PoseStamped_<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::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration 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::setLatLon(double, double)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::UTMtoLL(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::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > 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::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10
mrs_lib::Transformer::Transformer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)12
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&)51
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&)71
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)321
+
+
+ + + +
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..c3865ec4b2 --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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&)51
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&)6
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10
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&)71
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)321
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&)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::UTMtoLL(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::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::Transformer::setLatLon(double, double)1
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)0
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..adec9cfd65 --- /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:16528058.9 %
Date:2023-11-30 22:31:42Functions:172665.4 %
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           0 :   Transformer::Transformer(const ros::NodeHandle& nh, const std::string& node_name, const ros::Duration& cache_time)
+      49           0 :     : 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           0 :   }
+      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          51 :   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         102 :     std::scoped_lock lck(mutex_);
+      81             : 
+      82          51 :     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         102 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+      90         102 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+      91         102 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+      92             : 
+      93          51 :     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           1 :   void Transformer::setLatLon(const double lat, const double lon)
+     118             :   {
+     119           1 :     std::scoped_lock lck(mutex_);
+     120             : 
+     121             :     double utm_x, utm_y;
+     122           1 :     mrs_lib::LLtoUTM(lat, lon, utm_y, utm_x, utm_zone_.data());
+     123           1 :     got_utm_zone_ = true;
+     124           1 :   }
+     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           6 :   std::optional<mrs_msgs::ReferenceStamped> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what)
+     253             :   {
+     254             :     // create a pose message
+     255          12 :     geometry_msgs::PoseStamped pose;
+     256           6 :     pose.header = what.header;
+     257             :   
+     258           6 :     pose.pose.position.x = what.reference.position.x;
+     259           6 :     pose.pose.position.y = what.reference.position.y;
+     260           6 :     pose.pose.position.z = what.reference.position.z;
+     261           6 :     pose.pose.orientation = geometry::fromEigen(geometry::quaternionFromHeading(what.reference.heading));
+     262             :   
+     263             :     // try to transform the pose message
+     264          12 :     const auto pose_opt = transformImpl(tf, pose);
+     265           6 :     if (!pose_opt.has_value())
+     266           0 :       return std::nullopt;
+     267             :     // overwrite the pose with it's transformed value
+     268           6 :     pose = pose_opt.value();
+     269             :   
+     270          12 :     mrs_msgs::ReferenceStamped ret;
+     271           6 :     ret.header = pose.header;
+     272             :   
+     273             :     // copy the new transformed data back
+     274           6 :     ret.reference.position.x = pose.pose.position.x;
+     275           6 :     ret.reference.position.y = pose.pose.position.y;
+     276           6 :     ret.reference.position.z = pose.pose.position.z;
+     277           6 :     ret.reference.heading = geometry::headingFromRot(geometry::toEigen(pose.pose.orientation));
+     278           6 :     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          71 :   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          71 :     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          71 :     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          68 :     if (from_frame == to_frame)
+     319           2 :       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          67 :     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          64 :     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           6 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     337           6 :       auto tf_opt = getTransformImpl(from_frame, utm_frame, time_stamp, latlon_frame);
+     338           3 :       if (!tf_opt.has_value())
+     339           0 :         return std::nullopt;
+     340             :       // change the transformation frames to point to latlon
+     341           3 :       frame_to(*tf_opt) = to_frame;
+     342           3 :       return tf_opt;
+     343             :     }
+     344             : 
+     345         183 :     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          91 :       return tf_buffer_->lookupTransform(to_frame, from_frame, time_stamp, lookup_timeout_);
+     351             :     }
+     352          62 :     catch (tf2::TransformException& e)
+     353             :     {
+     354          31 :       ex = e;
+     355             :     }
+     356             : 
+     357             :     // if that failed, try to get the newest one if requested
+     358          31 :     if (retry_lookup_newest_)
+     359             :     {
+     360             :       try
+     361             :       {
+     362           2 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     363             :       }
+     364           0 :       catch (tf2::TransformException& e)
+     365             :       {
+     366           0 :         ex = e;
+     367             :       }
+     368             :     }
+     369             : 
+     370             :     // if the flow got here, we've failed to look the transform up
+     371          30 :     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          30 :       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          30 :     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         321 :   std::string Transformer::resolveFrameImpl(const std::string& frame_id)
+     464             :   {
+     465             :     // if the frame is empty, return the default frame id
+     466         321 :     if (frame_id.empty())
+     467           6 :       return default_frame_id_;
+     468             : 
+     469             :     // if there is no prefix set, just return the raw frame id
+     470         315 :     if (prefix_.empty())
+     471          67 :       return frame_id;
+     472             : 
+     473             :     // if there is a default prefix set and the frame does not start with it, prefix it
+     474         248 :     if (frame_id.substr(0, prefix_.length()) != prefix_)
+     475         145 :       return prefix_ + frame_id;
+     476             : 
+     477         103 :     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           2 :   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           2 :     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           1 :     geometry_msgs::Point latlon;
+     532           1 :     mrs_lib::UTMtoLL(what.y, what.x, utm_zone_.data(), latlon.x, latlon.y);
+     533           1 :     latlon.z = what.z;
+     534           1 :     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           0 :   std::optional<geometry_msgs::Pose> Transformer::UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix)
+     551             :   {
+     552           0 :     const auto opt = UTMtoLL(what.position, prefix);
+     553           0 :     if (!opt.has_value())
+     554           0 :       return std::nullopt;
+     555             : 
+     556           0 :     geometry_msgs::Pose ret;
+     557           0 :     ret.position = opt.value();
+     558           0 :     ret.orientation = what.orientation;
+     559           0 :     return ret;
+     560             :   }
+     561             : 
+     562           0 :   std::optional<geometry_msgs::PoseStamped> Transformer::UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     563             :   {
+     564           0 :     const auto opt = UTMtoLL(what.pose, prefix);
+     565           0 :     if (!opt.has_value())
+     566           0 :       return std::nullopt;
+     567             : 
+     568           0 :     geometry_msgs::PoseStamped ret;
+     569           0 :     ret.header.frame_id = prefix + "utm_origin";
+     570           0 :     ret.header.stamp = what.header.stamp;
+     571           0 :     ret.pose = opt.value();
+     572           0 :     return ret;
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* getFramePrefix() method //{ */
+     577          10 :   std::string Transformer::getFramePrefix(const std::string& frame_id)
+     578             :   {
+     579          10 :     const auto firstof = frame_id.find_first_of('/');
+     580          10 :     if (firstof == std::string::npos)
+     581           0 :       return "";
+     582             :     else
+     583          10 :       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..e34af02d67a42330ec7c91ef21b32d96ec5bf248 GIT binary patch literal 1976 zcmV;p2S@mcP)W0+RfJIOa*z0>N00W?HX0_(2*zM0}8+W65&++8YY%+Xa@#}onylcSbC}3geCweRc z8VJ61QWS;M6d;)53h?oIovxwzn>k*>H}m#;8Rt>eF5?&iD?t_MIS~w=*p0fv1GpMUxua$t5;P?A2{9PYE+b?|gAkP|sR^b6? zujI@kqvR1jICM78?IwHNfFmHuu;g6=AcM;hHvp$GA9eB(R}qkb3G*=yeJc?K18ODQVCkh^W?r#3W8UTY1)Wu9EK4xZjE(NKwFh-=oqjKI|5oZu! z28Rhwddxv*I}5o?p+Fp5y~SVuB)kgv2@-z3;0no^40xV|BRQXKVs@VU=$_6xppDX; zC!bThn@FD5DVIx1!`fMWG9#JC1d|{VfpOf#t2VC_vX2vEie`c_MiP>iY4>+oT%RDi zyzsxYxYP!4+iNqVH22T3+#~=1rwl))m6+F6c5gUd1vw1^G8_#;q2YD&?Bl0}->8MH z!WrilI`0}ZlbP~hYf6$bIRl{7LN#Y3+k+P5vA;IVx{vF-Di$IV`ztqlKWo-PXD$tc z*3{$BbT#E?alg^x5>3567xyG??*Mwl6g{>F6pj5vMUxt-iXc^uR5o-Sy8!q?BlR-? z-)I1SA=mqbT!|o+p?`+7ilbg#KO@>pl^1Vf$TaetQXT zVDV75$Ip8A7assn%Yf`-b>xG*zGmh#2b}70l-XbRQCS^tL^Y*2PDPh|GT@Fp)~JVj zY`#Hza+ySv6ko1jL$r-_G#S#M);<6tb;fAuYz0Z?WHJCr9-J}AS%?fko!4SaAp(9v zOwnGY;muvP)m6km=Lf=Ktl`{oYHo{4vzk|RD&o{?Dlv?U$yCO;BY z*Cw1XlV^Yw*4ws7n`qD?c|2)~c!;(~C>gRwQX6(1iv5G{-k8~k+HQ_x5la&tL#%)k zk3gu9^7meP=&}OO4uQEIFn0wRg&d$}5x`YcYP8E~;VrJ}_(#_5a-&tlL~sM1BE1l= zcj#!)nJ8gZh4^z8Mh+HP7*D0Z^-ozy>%K7y=bFNvz(RG)%wr`5aHkC(f)Pa{7|@vw zt@iAOBTHULV6hASWRoc@<4p8wQw1Vn^Rue3;W$nEz^C~cP(Db$VuFn6h>2KD_Y4N0 ztyFB??lb#3ZGb8yeSk#I42Wi615`yw-lW`ayTJM$&p7%Ov?^F*2+^XZC4$Qt(Q%rh1sJG$YlLDmB(BeK+ zQK5k|i!LfW&(A1Q0ID)&G)y=)mWr+0JNI$i1U&wj0`^}DP)-4YP-rG&PzA!XH%tK* z5rA+{y!|O2%J)%RWoR;Xb^Vbd3IF&bUV7$uz_xhw z)Gzo}f?zuIYid5|OLSOAOSm%m8ZZnN`@*tmv6&QChl@l{O?Z8;k^bzsr!wtNihG8*gi{)zu!6)Fkc{tY z65m-M9E))|?cUl@6tjJTFMHce0dQn%Ql-&f`ChIb&iCvr75)Rl##4s@quyEo0000< KMNUMnLSTX^N22fm 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..e1562f22f8 --- /dev/null +++ b/mrs_lib/src/utils/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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..88535e0da7 --- /dev/null +++ b/mrs_lib/src/utils/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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..c3fc2605b8 --- /dev/null +++ b/mrs_lib/src/utils/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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..55aee0f967 --- /dev/null +++ b/mrs_lib/src/utils/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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..e7f8f95f5b --- /dev/null +++ b/mrs_lib/src/utils/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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..8417a10606 --- /dev/null +++ b/mrs_lib/src/utils/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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..493560957a --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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>&)1984
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()1984
+
+
+ + + +
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..b7f7ddf411 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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>&)1984
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()1984
+
+
+ + + +
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..3d5e9d5508 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.gcov.html @@ -0,0 +1,101 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2023-11-30 22:31:42Functions: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        1984 : AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>& in)
+       7        1984 :   : variable(in)
+       8             : {
+       9        1984 :   variable = true;
+      10        1984 : }
+      11             : 
+      12        3968 : AtomicScopeFlag::~AtomicScopeFlag()
+      13             : {
+      14        1984 :   variable = false;
+      15        1984 : }
+      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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)0
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()0
mrs_uav_autostart::automatic_start::AutomaticStart::elandImpl()0
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()0
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)1
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()1
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()1
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)2
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)2
mrs_uav_autostart::automatic_start::AutomaticStart::speedCheck()187
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()187
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()187
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()187
mrs_uav_autostart::automatic_start::Topic::getTime()374
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)490
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)1300
mrs_uav_autostart::automatic_start::Topic::updateTime()1300
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&) const1300
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)5725
+
+
+ + + +
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..9f8ce98167 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.func.html @@ -0,0 +1,160 @@ + + + + + + + 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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_autostart::automatic_start::AutomaticStart::speedCheck()187
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()187
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)2
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)1300
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()187
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()187
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)5725
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)1
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)0
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()0
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()1
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()1
mrs_uav_autostart::automatic_start::AutomaticStart::elandImpl()0
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)490
mrs_uav_autostart::automatic_start::Topic::updateTime()1300
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()0
mrs_uav_autostart::automatic_start::Topic::getTime()374
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)2
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&) const1300
+
+
+ + + +
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..21d6eb3dd1 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html @@ -0,0 +1,919 @@ + + + + + + + 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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.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           2 :   Topic(std::string topic_name) : topic_name_(topic_name) {
+      42           2 :     last_time_ = ros::Time(0);
+      43           2 :   }
+      44             : 
+      45        1300 :   void updateTime(void) {
+      46        1300 :     last_time_ = ros::Time::now();
+      47        1300 :   }
+      48             : 
+      49         374 :   ros::Time getTime(void) {
+      50         374 :     return last_time_;
+      51             :   }
+      52             : 
+      53           0 :   std::string getTopicName(void) {
+      54           0 :     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 elandImpl();
+     136             :   bool validateReference();
+     137             : 
+     138             :   bool toggleControlOutput(const bool& value);
+     139             :   bool disarm();
+     140             : 
+     141             :   bool isGazeboSimulation(void);
+     142             :   bool topicCheck(void);
+     143             :   bool speedCheck(void);
+     144             :   bool is_gazebo_simulation_ = false;
+     145             : 
+     146             :   // | ---------------------- other params ---------------------- |
+     147             : 
+     148             :   std::string _body_frame_name_;
+     149             :   double      _action_duration_;
+     150             :   double      _pre_takeoff_sleep_;
+     151             :   bool        _handle_takeoff_ = false;
+     152             :   double      _safety_timeout_;
+     153             :   double      _control_output_timeout_;
+     154             : 
+     155             :   // | ---------------------- state machine --------------------- |
+     156             : 
+     157             :   uint current_state = STATE_IDLE;
+     158             :   void changeState(LandingStates_t new_state);
+     159             : 
+     160             :   // | --------------------- velocity check --------------------- |
+     161             : 
+     162             :   bool   _velocity_check_enabled_ = false;
+     163             :   double _velocity_check_max_speed_;
+     164             : 
+     165             :   // | ---------------- generic topic subscribers --------------- |
+     166             : 
+     167             :   bool                     _topic_check_enabled_ = false;
+     168             :   double                   _topic_check_timeout_;
+     169             :   std::vector<std::string> _topic_check_topic_names_;
+     170             : 
+     171             :   std::vector<Topic>           topic_check_topics_;
+     172             :   std::vector<ros::Subscriber> generic_subscriber_vec_;
+     173             : 
+     174             :   // generic callback, for any topic, to monitor its rate
+     175             :   void genericCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string& topic_name, const int id);
+     176             : };
+     177             : 
+     178             : //}
+     179             : 
+     180             : /* onInit() //{ */
+     181             : 
+     182           1 : void AutomaticStart::onInit() {
+     183             : 
+     184           2 :   ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     185             : 
+     186           1 :   ros::Time::waitForValid();
+     187             : 
+     188           1 :   armed_      = false;
+     189           1 :   armed_time_ = ros::Time(0);
+     190             : 
+     191           1 :   offboard_      = false;
+     192           1 :   offboard_time_ = ros::Time(0);
+     193             : 
+     194           2 :   mrs_lib::ParamLoader param_loader(nh_, "AutomaticStart");
+     195             : 
+     196           2 :   std::string custom_config_path;
+     197             : 
+     198           1 :   param_loader.loadParam("custom_config", custom_config_path);
+     199             : 
+     200           1 :   if (custom_config_path != "") {
+     201           0 :     param_loader.addYamlFile(custom_config_path);
+     202             :   }
+     203             : 
+     204           1 :   param_loader.addYamlFileFromParam("config_private");
+     205           1 :   param_loader.addYamlFileFromParam("config_public");
+     206             : 
+     207           1 :   param_loader.loadParam("uav_name", _uav_name_);
+     208           1 :   param_loader.loadParam("simulation", _simulation_);
+     209             : 
+     210           1 :   param_loader.loadParam("main_timer_rate", _main_timer_rate_);
+     211           1 :   param_loader.loadParam("body_frame_name", _body_frame_name_);
+     212           1 :   param_loader.loadParam("control_output_timeout", _control_output_timeout_);
+     213             : 
+     214           1 :   param_loader.loadParam("safety_timeout", _safety_timeout_);
+     215           1 :   param_loader.loadParam("pre_takeoff_sleep", _pre_takeoff_sleep_);
+     216             : 
+     217           1 :   param_loader.loadParam("handle_takeoff", _handle_takeoff_);
+     218             : 
+     219           1 :   param_loader.loadParam("preflight_check/velocity_check/enabled", _velocity_check_enabled_);
+     220           1 :   param_loader.loadParam("preflight_check/velocity_check/max_speed", _velocity_check_max_speed_);
+     221             : 
+     222           1 :   param_loader.loadParam("preflight_check/topic_check/enabled", _topic_check_enabled_);
+     223           1 :   param_loader.loadParam("preflight_check/topic_check/timeout", _topic_check_timeout_);
+     224           1 :   param_loader.loadParam("preflight_check/topic_check/topics", _topic_check_topic_names_);
+     225             : 
+     226           1 :   if (!param_loader.loadedSuccessfully()) {
+     227           0 :     ROS_ERROR("[AutomaticStart]: Could not load all parameters!");
+     228           0 :     ros::shutdown();
+     229             :   }
+     230             : 
+     231             :   // | ----------------------- subscribers ---------------------- |
+     232             : 
+     233           2 :   mrs_lib::SubscribeHandlerOptions shopts;
+     234           1 :   shopts.nh                 = nh_;
+     235           1 :   shopts.node_name          = "AutomaticStart";
+     236           1 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     237           1 :   shopts.threadsafe         = true;
+     238           1 :   shopts.autostart          = true;
+     239           1 :   shopts.queue_size         = 10;
+     240           1 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     241             : 
+     242           1 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diag_in");
+     243           1 :   sh_hw_api_status_        = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &AutomaticStart::callbackHwApiStatus, this);
+     244           1 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     245           1 :   sh_uav_manager_diag_     = mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>(shopts, "uav_manager_diagnostics_in");
+     246           2 :   sh_gazebo_spawner_diag_  = mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>(shopts, "gazebo_spawner_diagnostics_in",
+     247           1 :                                                                                           &AutomaticStart::callbackGazeboSpawnerDiagnostics, this);
+     248             : 
+     249             :   // | ----------------------- publishers ----------------------- |
+     250             : 
+     251           1 :   ph_can_takeoff_ = mrs_lib::PublisherHandler<std_msgs::Bool>(nh_, "can_takeoff_out");
+     252             : 
+     253             :   // | --------------------- service clients -------------------- |
+     254             : 
+     255           1 :   service_client_takeoff_               = nh_.serviceClient<std_srvs::Trigger>("takeoff_out");
+     256           1 :   service_client_eland_                 = nh_.serviceClient<std_srvs::Trigger>("eland_out");
+     257           1 :   service_client_toggle_control_output_ = nh_.serviceClient<std_srvs::SetBool>("toggle_control_output_out");
+     258           1 :   service_client_arm_                   = nh_.serviceClient<std_srvs::SetBool>("arm_out");
+     259             : 
+     260           1 :   service_client_validate_reference_ = nh_.serviceClient<mrs_msgs::ValidateReference>("validate_reference_out");
+     261             : 
+     262             :   // | ------------------ setup generic topics ------------------ |
+     263             : 
+     264           1 :   if (_topic_check_enabled_) {
+     265             : 
+     266           2 :     boost::function<void(const topic_tools::ShapeShifter::ConstPtr&)> callback;
+     267             : 
+     268           3 :     for (int i = 0; i < int(_topic_check_topic_names_.size()); i++) {
+     269             : 
+     270           4 :       std::string topic_name = _topic_check_topic_names_[i];
+     271             : 
+     272           2 :       if (topic_name.at(0) != '/') {
+     273           2 :         topic_name = "/" + _uav_name_ + "/" + topic_name;
+     274             :       }
+     275             : 
+     276           4 :       Topic tmp_topic(topic_name);
+     277           2 :       topic_check_topics_.push_back(tmp_topic);
+     278             : 
+     279           2 :       int id = i;  // id to identify which topic called the generic callback
+     280             : 
+     281        1302 :       callback                       = [this, topic_name, id](const topic_tools::ShapeShifter::ConstPtr& msg) -> void { genericCallback(msg, topic_name, id); };
+     282           6 :       ros::Subscriber tmp_subscriber = nh_.subscribe(topic_name, 1, callback);
+     283             : 
+     284           2 :       generic_subscriber_vec_.push_back(tmp_subscriber);
+     285             :     }
+     286             :   }
+     287             : 
+     288             :   // | ------------------------- timers ------------------------- |
+     289             : 
+     290           1 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &AutomaticStart::timerMain, this);
+     291             : 
+     292           1 :   is_initialized_ = true;
+     293             : 
+     294           1 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: initialized");
+     295           1 : }
+     296             : 
+     297             : //}
+     298             : 
+     299             : // --------------------------------------------------------------
+     300             : // |                          callbacks                         |
+     301             : // --------------------------------------------------------------
+     302             : 
+     303             : /* genericCallback() //{ */
+     304             : 
+     305        1300 : void AutomaticStart::genericCallback([[maybe_unused]] const topic_tools::ShapeShifter::ConstPtr& msg, [[maybe_unused]] const std::string& topic_name,
+     306             :                                      const int id) {
+     307        1300 :   topic_check_topics_[id].updateTime();
+     308        1300 : }
+     309             : 
+     310             : //}
+     311             : 
+     312             : /* callbackHwApiDiag() //{ */
+     313             : 
+     314        5725 : void AutomaticStart::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+     315             : 
+     316        5725 :   if (!is_initialized_) {
+     317           0 :     return;
+     318             :   }
+     319             : 
+     320        5725 :   ROS_INFO_ONCE("[AutomaticStart]: getting HW API diagnostics");
+     321             : 
+     322       11450 :   std::scoped_lock lock(mutex_hw_api_status_);
+     323             : 
+     324             :   // check armed_ state
+     325        5725 :   if (armed_ == false) {
+     326             : 
+     327             :     // if armed_ state changed to true, please "start the clock"
+     328        2063 :     if (msg->armed) {
+     329             : 
+     330           1 :       armed_      = true;
+     331           1 :       armed_time_ = ros::Time::now();
+     332             :     }
+     333             : 
+     334             :     // if we were armed_ previously
+     335        3662 :   } else if (armed_ == true) {
+     336             : 
+     337             :     // and we are not really now
+     338        3662 :     if (!msg->armed) {
+     339             : 
+     340           0 :       armed_ = false;
+     341             :     }
+     342             :   }
+     343             : 
+     344             :   // check offboard_ state
+     345        5725 :   if (offboard_ == false) {
+     346             : 
+     347             :     // if offboard_ state changed to true, please "start the clock"
+     348        2133 :     if (msg->offboard) {
+     349             : 
+     350           1 :       offboard_      = true;
+     351           1 :       offboard_time_ = ros::Time::now();
+     352             :     }
+     353             : 
+     354             :     // if we were in offboard_ previously
+     355        3592 :   } else if (offboard_ == true) {
+     356             : 
+     357             :     // and we are not really now
+     358        3592 :     if (!msg->offboard) {
+     359             : 
+     360           0 :       offboard_ = false;
+     361             :     }
+     362             :   }
+     363             : 
+     364        5725 :   if (msg->connected) {
+     365        5725 :     got_hw_api_status_ = true;
+     366             :   }
+     367             : }
+     368             : 
+     369             : //}
+     370             : 
+     371             : /* callbackGazeboSpawnerDiagnostics() //{ */
+     372             : 
+     373           0 : void AutomaticStart::callbackGazeboSpawnerDiagnostics(const mrs_msgs::GazeboSpawnerDiagnostics::ConstPtr msg) {
+     374             : 
+     375           0 :   if (!is_initialized_) {
+     376           0 :     return;
+     377             :   }
+     378             : 
+     379           0 :   ROS_INFO_ONCE("[AutomaticStart]: getting spawner diagnostics");
+     380             : 
+     381             :   {
+     382           0 :     std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     383             : 
+     384           0 :     gazebo_spawner_diagnostics_ = *msg;
+     385             : 
+     386           0 :     got_gazebo_spawner_diagnostics = true;
+     387             :   }
+     388             : }
+     389             : 
+     390             : //}
+     391             : 
+     392             : // --------------------------------------------------------------
+     393             : // |                           timers                           |
+     394             : // --------------------------------------------------------------
+     395             : 
+     396             : /* timerMain() //{ */
+     397             : 
+     398         490 : void AutomaticStart::timerMain([[maybe_unused]] const ros::TimerEvent& event) {
+     399             : 
+     400         490 :   if (!is_initialized_) {
+     401         154 :     return;
+     402             :   }
+     403             : 
+     404         490 :   bool got_uav_manager_diag     = sh_uav_manager_diag_.hasMsg();
+     405         490 :   bool got_control_manager_diag = sh_control_manager_diag_.hasMsg();
+     406         490 :   bool got_estimation_diag      = sh_estimation_diag_.hasMsg();
+     407             : 
+     408         490 :   if (!got_control_manager_diag || !got_hw_api_status_ || !got_uav_manager_diag || !got_estimation_diag) {
+     409         154 :     ROS_WARN_THROTTLE(5.0, "[AutomaticStart]: waiting for data: ControManager=%s, UavManager=%s, HW Api=%s, EstimationManager=%s",
+     410             :                       got_control_manager_diag ? "true" : "FALSE", got_uav_manager_diag ? "true" : "FALSE", got_hw_api_status_ ? "true" : "FALSE",
+     411             :                       got_estimation_diag ? "true" : "FALSE");
+     412         154 :     return;
+     413             :   }
+     414             : 
+     415         336 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     416         336 :   auto control_manager_diagnostics                  = sh_control_manager_diag_.getMsg();
+     417             : 
+     418         336 :   switch (current_state) {
+     419             : 
+     420         187 :     case STATE_IDLE: {
+     421             : 
+     422         187 :       bool   control_output   = sh_control_manager_diag_.getMsg()->output_enabled;
+     423         187 :       double time_from_arming = (ros::Time::now() - armed_time).toSec();
+     424             : 
+     425         187 :       std_msgs::Bool can_takeoff_msg;
+     426         187 :       can_takeoff_msg.data = false;
+     427             : 
+     428             :       // | -------------------- preflight checks -------------------- |
+     429             : 
+     430         187 :       bool position_valid = validateReference();
+     431         187 :       bool got_topics     = topicCheck();
+     432         187 :       bool speed_valid    = speedCheck();
+     433             : 
+     434         187 :       bool can_takeoff = got_topics && position_valid && speed_valid;
+     435             : 
+     436             :       // | ---------------------------------------------------------- |
+     437             : 
+     438         187 :       can_takeoff_msg.data = can_takeoff;
+     439         187 :       ph_can_takeoff_.publish(can_takeoff_msg);
+     440             : 
+     441         187 :       if (armed && !control_output) {
+     442             : 
+     443           1 :         if (can_takeoff) {
+     444             : 
+     445           1 :           bool res = toggleControlOutput(true);
+     446             : 
+     447           1 :           if (!res) {
+     448           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON");
+     449             :           }
+     450             :         }
+     451             : 
+     452           1 :         if (time_from_arming > _control_output_timeout_) {
+     453             : 
+     454           0 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON for %.2f secs, disarming", _control_output_timeout_);
+     455           0 :           disarm();
+     456           0 :           changeState(STATE_FINISHED);
+     457             :         }
+     458             :       }
+     459             : 
+     460         187 :       if (_simulation_ && isGazeboSimulation()) {
+     461             : 
+     462           0 :         std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     463             : 
+     464           0 :         if (got_gazebo_spawner_diagnostics) {
+     465             : 
+     466           0 :           if (!gazebo_spawner_diagnostics_.spawn_called || gazebo_spawner_diagnostics_.processing) {
+     467           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) waiting for spawner to finish spawning UAVs");
+     468           0 :             return;
+     469             :           }
+     470             : 
+     471             :         } else {
+     472             : 
+     473           0 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) missing spawner diagnostics");
+     474           0 :           return;
+     475             :         }
+     476             :       }
+     477             : 
+     478             :       // when armed and in offboard, takeoff
+     479         187 :       if (armed && offboard && control_output) {
+     480             : 
+     481         151 :         double armed_time_diff    = (ros::Time::now() - armed_time).toSec();
+     482         151 :         double offboard_time_diff = (ros::Time::now() - offboard_time).toSec();
+     483             : 
+     484         151 :         if ((armed_time_diff > _safety_timeout_) && (offboard_time_diff > _safety_timeout_)) {
+     485             : 
+     486           1 :           if (_handle_takeoff_) {
+     487           1 :             changeState(STATE_TAKEOFF);
+     488             :           } else {
+     489           0 :             changeState(STATE_FINISHED);
+     490             :           }
+     491             : 
+     492             :         } else {
+     493             : 
+     494         150 :           double min = (armed_time_diff < offboard_time_diff) ? armed_time_diff : offboard_time_diff;
+     495             : 
+     496         150 :           ROS_WARN_THROTTLE(1.0, "taking off in %.0f", (_safety_timeout_ - min));
+     497             :         }
+     498             :       }
+     499             : 
+     500         187 :       break;
+     501             :     }
+     502             : 
+     503         148 :     case STATE_TAKEOFF: {
+     504             : 
+     505             :       // if takeoff finished
+     506         148 :       if (control_manager_diagnostics->flying_normally) {
+     507             : 
+     508           1 :         ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: takeoff finished");
+     509             : 
+     510           1 :         changeState(STATE_FINISHED);
+     511             : 
+     512             :       } else {
+     513             : 
+     514         147 :         ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: waiting for the takeoff to finish");
+     515             :       }
+     516             : 
+     517         148 :       break;
+     518             :     }
+     519             : 
+     520           1 :     case STATE_FINISHED: {
+     521             : 
+     522           1 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: finished");
+     523           1 :       ros::requestShutdown();
+     524           1 :       break;
+     525             :     }
+     526             :   }
+     527             : 
+     528             : }  // namespace automatic_start
+     529             : 
+     530             : //}
+     531             : 
+     532             : // --------------------------------------------------------------
+     533             : // |                          routines                          |
+     534             : // --------------------------------------------------------------
+     535             : 
+     536             : /* changeState() //{ */
+     537             : 
+     538           2 : void AutomaticStart::changeState(LandingStates_t new_state) {
+     539             : 
+     540           2 :   ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: switching states %s -> %s", state_names[current_state], state_names[new_state]);
+     541             : 
+     542           2 :   switch (new_state) {
+     543             : 
+     544           0 :     case STATE_IDLE: {
+     545             : 
+     546           0 :       break;
+     547             :     }
+     548             : 
+     549           1 :     case STATE_TAKEOFF: {
+     550             : 
+     551           1 :       if (_pre_takeoff_sleep_ > 1.0) {
+     552           0 :         ROS_INFO("[AutomaticStart]: sleeping for %.2f secs before takeoff", _pre_takeoff_sleep_);
+     553           0 :         ros::Duration(_pre_takeoff_sleep_).sleep();
+     554             :       }
+     555             : 
+     556           1 :       bool res = takeoff();
+     557             : 
+     558           1 :       if (!res) {
+     559           0 :         return;
+     560             :       }
+     561             : 
+     562           1 :       break;
+     563             :     }
+     564             : 
+     565           1 :     case STATE_FINISHED: {
+     566             : 
+     567           1 :       break;
+     568             :     }
+     569             : 
+     570             :     break;
+     571             :   }
+     572             : 
+     573           2 :   current_state = new_state;
+     574             : }
+     575             : 
+     576             : //}
+     577             : 
+     578             : /* takeoff() //{ */
+     579             : 
+     580           1 : bool AutomaticStart::takeoff() {
+     581             : 
+     582           1 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: taking off");
+     583             : 
+     584           2 :   std_srvs::Trigger srv;
+     585             : 
+     586           1 :   bool res = service_client_takeoff_.call(srv);
+     587             : 
+     588           1 :   if (res) {
+     589             : 
+     590           1 :     if (srv.response.success) {
+     591             : 
+     592           1 :       return true;
+     593             : 
+     594             :     } else {
+     595             : 
+     596           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: taking off failed: %s", srv.response.message.c_str());
+     597             :     }
+     598             : 
+     599             :   } else {
+     600             : 
+     601           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for taking off failed");
+     602             :   }
+     603             : 
+     604           0 :   return false;
+     605             : }
+     606             : 
+     607             : //}
+     608             : 
+     609             : /* elandImpl() //{ */
+     610             : 
+     611           0 : bool AutomaticStart::elandImpl() {
+     612             : 
+     613           0 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: elanding");
+     614             : 
+     615           0 :   std_srvs::Trigger srv;
+     616             : 
+     617           0 :   bool res = service_client_eland_.call(srv);
+     618             : 
+     619           0 :   if (res) {
+     620             : 
+     621           0 :     if (srv.response.success) {
+     622             : 
+     623           0 :       return true;
+     624             : 
+     625             :     } else {
+     626             : 
+     627           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: elanding failed: %s", srv.response.message.c_str());
+     628             :     }
+     629             : 
+     630             :   } else {
+     631             : 
+     632           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for elanding failed");
+     633             :   }
+     634             : 
+     635           0 :   return false;
+     636             : }
+     637             : 
+     638             : //}
+     639             : 
+     640             : /* validateReference() //{ */
+     641             : 
+     642         187 : bool AutomaticStart::validateReference() {
+     643             : 
+     644         374 :   mrs_msgs::ValidateReference srv_out;
+     645             : 
+     646         187 :   srv_out.request.reference.header.frame_id = _body_frame_name_;
+     647             : 
+     648         187 :   bool res = service_client_validate_reference_.call(srv_out);
+     649             : 
+     650         187 :   if (res) {
+     651             : 
+     652         187 :     if (srv_out.response.success) {
+     653             : 
+     654         187 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: current position is valid");
+     655         187 :       return true;
+     656             : 
+     657             :     } else {
+     658             : 
+     659           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position is not valid (safety area, bumper)!");
+     660           0 :       return false;
+     661             :     }
+     662             : 
+     663             :   } else {
+     664             : 
+     665           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position could not be validated");
+     666           0 :     return false;
+     667             :   }
+     668             : }
+     669             : 
+     670             : //}
+     671             : 
+     672             : /* toggleControlOutput() //{ */
+     673             : 
+     674           1 : bool AutomaticStart::toggleControlOutput(const bool& value) {
+     675             : 
+     676           1 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: setting control output %s", value ? "ON" : "OFF");
+     677             : 
+     678           2 :   std_srvs::SetBool srv;
+     679           1 :   srv.request.data = value;
+     680             : 
+     681           1 :   bool res = service_client_toggle_control_output_.call(srv);
+     682             : 
+     683           1 :   if (res) {
+     684             : 
+     685           1 :     if (srv.response.success) {
+     686             : 
+     687           1 :       return true;
+     688             : 
+     689             :     } else {
+     690             : 
+     691           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: setting of control output failed: %s", srv.response.message.c_str());
+     692             :     }
+     693             : 
+     694             :   } else {
+     695             : 
+     696           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for toggling control output failed");
+     697             :   }
+     698             : 
+     699           0 :   return false;
+     700             : }
+     701             : 
+     702             : //}
+     703             : 
+     704             : /* disarm() //{ */
+     705             : 
+     706           0 : bool AutomaticStart::disarm() {
+     707             : 
+     708           0 :   if (!got_hw_api_status_) {
+     709             : 
+     710           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, missing HW API status!");
+     711             : 
+     712           0 :     return false;
+     713             :   }
+     714             : 
+     715           0 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     716             : 
+     717           0 :   if (offboard) {
+     718             : 
+     719           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, already in offboard mode!");
+     720             : 
+     721           0 :     return false;
+     722             :   }
+     723             : 
+     724           0 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: disarming");
+     725             : 
+     726           0 :   std_srvs::SetBool srv;
+     727           0 :   srv.request.data = false;
+     728             : 
+     729           0 :   bool res = service_client_arm_.call(srv);
+     730             : 
+     731           0 :   if (res) {
+     732             : 
+     733           0 :     if (srv.response.success) {
+     734             : 
+     735           0 :       return true;
+     736             : 
+     737             :     } else {
+     738             : 
+     739           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: disarming failed");
+     740             :     }
+     741             : 
+     742             :   } else {
+     743             : 
+     744           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for disarming failed");
+     745             :   }
+     746             : 
+     747           0 :   return false;
+     748             : }
+     749             : 
+     750             : //}
+     751             : 
+     752             : /* isGazeboSimulation() //{ */
+     753             : 
+     754         187 : bool AutomaticStart::isGazeboSimulation(void) {
+     755             : 
+     756         187 :   if (is_gazebo_simulation_) {
+     757           0 :     return true;
+     758             :   }
+     759             : 
+     760         374 :   ros::V_string node_list;
+     761         187 :   ros::master::getNodes(node_list);
+     762             : 
+     763        2618 :   for (auto& node : node_list) {
+     764        2431 :     if (node.find("mrs_drone_spawner") != std::string::npos) {
+     765           0 :       ROS_INFO("[AutomaticStart]: MRS Gazebo Simulation detected");
+     766           0 :       is_gazebo_simulation_ = true;
+     767           0 :       return true;
+     768             :     }
+     769             :   }
+     770             : 
+     771         187 :   return false;
+     772             : }
+     773             : 
+     774             : //}
+     775             : 
+     776             : /* topicCheck() //{ */
+     777             : 
+     778         187 : bool AutomaticStart::topicCheck(void) {
+     779             : 
+     780         187 :   bool got_topics = true;
+     781             : 
+     782         187 :   std::stringstream missing_topics;
+     783             : 
+     784         187 :   if (_topic_check_enabled_) {
+     785             : 
+     786         561 :     for (int i = 0; i < int(topic_check_topics_.size()); i++) {
+     787         374 :       if ((ros::Time::now() - topic_check_topics_[i].getTime()).toSec() > _topic_check_timeout_) {
+     788           0 :         missing_topics << std::endl << "\t" << topic_check_topics_[i].getTopicName();
+     789           0 :         got_topics = false;
+     790             :       }
+     791             :     }
+     792             :   }
+     793             : 
+     794         187 :   if (!got_topics) {
+     795           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[AutomaticStart]: missing data on topics: " << missing_topics.str());
+     796             :   }
+     797             : 
+     798         374 :   return got_topics;
+     799             : }
+     800             : 
+     801             : //}
+     802             : 
+     803             : /* velocityCheck() //{ */
+     804             : 
+     805         187 : bool AutomaticStart::speedCheck(void) {
+     806             : 
+     807         187 :   if (!_velocity_check_enabled_) {
+     808           0 :     return true;
+     809             :   }
+     810             : 
+     811         187 :   if (!sh_estimation_diag_.hasMsg()) {
+     812           0 :     return false;
+     813             :   }
+     814             : 
+     815         374 :   auto estimation_diag = sh_estimation_diag_.getMsg();
+     816             : 
+     817             :   double speed =
+     818         187 :       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));
+     819             : 
+     820         187 :   if (speed > _velocity_check_max_speed_) {
+     821           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: the estimated speed (%.2f ms^-2) is over the limit (%.2f ms^-2)", speed, _velocity_check_max_speed_);
+     822           0 :     return false;
+     823             :   }
+     824             : 
+     825         187 :   return true;
+     826             : }
+     827             : 
+     828             : //}
+     829             : 
+     830             : }  // namespace automatic_start
+     831             : 
+     832             : }  // namespace mrs_uav_autostart
+     833             : 
+     834             : #include <pluginlib/class_list_macros.h>
+     835           1 : 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..d3142cd401 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html @@ -0,0 +1,229 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ 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..ae9712ee8a7c79eb14eea4e48fe3b7830fd849d6 GIT binary patch literal 2767 zcmV;=3NZDFP)36q3bpwZ}y02 zi9oypcOpuR8b&;YN}NqJc60*xcA{uvO;xiP-oU`Xd+?1daJkJu(OW`cpp7n*HM*!vonFlq>6!y=5k`*M|A09V(O?MJ|Qi-C3Fz=9jUHaglxo)u0^If znr3dH3V12eE>{UBV5-=JWlC)PD*C~Q^&Ikq&FNK*Qwj$=-1xfov0hC**t)aMH~7M& zrharU#AdF<;hIWwMGlJ?gjt{@LK}N^;X?MSkxD*%SEnV`U&Tx#W%G_0T3v_8(s>Gt zdu7hlGt)hX;WGCBYw~8YN0l+(TTiEUre{rt7G=0WC!>gsba*G?&v)I<45L(iIZfP~ zz23Gnb6(G|J9%5j07H6I*$?;tq6;w&b4j4lvECE>9T4&n;|y22JjHD)o@i;}ou7(l zH%e-1x>pEwLO8B)MGhs^wUX|7CnwWILcpn~zsMu8XbN3}se94v)w;ffNT9a~9i>>; zK~G|YK~ZCtBJ@X?i_B6S=g0ASh66uzx(8pme7eVP;wj`z;itBWV;%ISD`&$V$rC!A z*JqEW_JfpAJH&#u&%16CI>ho8w?npa*GC6JW&?=MUnn%pBa85COhP`Qt`f4P<`T*$ z_k-(7JFdV!j`1jQAchp~x}n&Ggm5j^hp?sqJRhrwT=fvn2RVht6b=$3^A&|dmQ+O; zw$xZ`LSmy;-G!)6-6>6uobuaP*D8#hWTP@BHPEEeMO7L^->dCdhgec4gsj4yj{q{IW?Qr165n(-+lHq**VsH31|eHhf1; z>SS5z%vcQA9;`Wnkc;TcK9|or)sgPs*tc5P z3JKwsM3Z-H!~AxtWRA{vX?#v0l=W|vjC*Zng()erotGy?SccU((b4t8mUADiHTy%z zTdz0X=P+MtbWS^FTpD%u9V4@#$Sf>hcZQdZjMQ@7HhA#TCfr+h8*W`Qes~JYCaf9z zt$d`xVsIN;DaY_|-FNezSQjL6Hh8A#Yagy6pp1i*EgxKVMM#pl7!rm`Od|~ z3v#nMq5Fee!7HMLm5tb)H=Z!X$M&nWiYhEkguH=ggL# zl4@LD5nsn?Zj@`%XdGeJ5ZPoaHFz_cabdC=hb1Y~{rnKB$hWVOz^YG&5I74dA%A3c zmfPHSQlXl7-G2BM3P(x@Z9XX--L5_R$!csQDIHK-gnkxYm3qS2W*-AxI0)S0(9{mZ zH$WKe2_+9iY!dRM28b$#lJJM#Q>iJ-+Gl&$h#uisSJjiZizhw`8qmVF)OU2wNojN* z#+#(El9SrW-AIR77cFsyz5oAo#WqhL0&pDmv&p(=UKTWc_Iv=kqG68EuSnRCJVSgH z!t9Bu#)rHb;n^_qA@U-`9zAj*nx5$6@yMWvP_hp(hjgP2Ual_%|NHSeA|Y#1aMujG zEtjtKr;R5xSPCt9J~_iD6C^D*GZNB!1l0z21zq;%I(LQW^uR(pf$gKZWd`*?gIqY%G+)c*D3S1yPRN2K{ffoqwF7o`oeavrSf>%6az}1(1{gw4 z*ky#4lh{eUH?DkmVWgA$$X~B_&1)y9Jx*Cx+LS=43nc@Hm2Q&)huXsJp}^=GVsVtZ z)FZoO8dCczEw$fOnD+6BD;HFh)R^5vf^)Tn$m;rI^=3;r`W#HMX6^sUHD?Cs4Fy$l z$>vf)l}aq6M3tS{4o@RtejCZ{$<-=&m%8+DI_S=Dyk_eiu?V>*x!fkdyKZRH)I|po zMVc`@vG;*a1wY+uIbnE1u1A)Bdej4wI3lGS8!L$u)zcK{E~$UlrpY3_p)Q5_goGOk zFT(PxO3$xMWw*O_SHVNlbmuD0G=ZHfIR+U(EMEldM@$paq+RPg(`+RG`VdjbCGuPa zpPUinQ9MBMwOlw)_j22yOI^E^owNraF3VJwCEWErclp;%g+R?B(i4%{b4S;=1r>sJ zFHPxJp03PPPbJU%#Dv|5&t_R8AMr3HPc`SU6Y*$y|GkLlJfmD_*BeyOgirtC=WO_&*{dSHq9?e*_8PdwR=vSy3E7Aoh4=CJQH9g8ym0>DjxNAK=c9=5?;gf~8Srgh>=q?)cLU3^TZ>-x#^ zJv%CCj_*q>#VZ^eg{AiNdW3|>`$j*ytJ$}!ry`(3euWLHc?F{YrTb>ga zD#5QK-tZjF;!`gb&?&CvqMy%auOU3*GCbVIci_7?wnajQx5uq&;?CUro^H}eAycu#trHtj#9z{tv{) V;T*i + + + + + + 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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.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 +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
<unnamed>70.9 %197 / 27880.0 %16 / 20
+
+
+ + + + +
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..65ca3cd69b --- /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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.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 +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
<unnamed>70.9 %197 / 27880.0 %16 / 20
+
+
+ + + + +
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..645a2a669a --- /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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.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 +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
<unnamed>70.9 %197 / 27880.0 %16 / 20
+
+
+ + + + +
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..2670c5df66 --- /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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.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 +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
+
+
+ + + + +
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..8708584dcc --- /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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.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 +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
+
+
+ + + + +
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..e1bc143094 --- /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:19727870.9 %
Date:2023-11-30 22:31:42Functions:162080.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 +
70.9%70.9%
+ 		70.9 %197 / 27880.0 %16 / 20
+
+
+ + + + +
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..cbba31282b --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2023-11-30 22:31:42Functions: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::HwApiActuatorCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<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::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)3
+
+
+ + + +
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..1b92b60883 --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2023-11-30 22:31:42Functions: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&)3
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&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<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::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)3
+
+
+ + + +
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..0e555b4708 --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.html @@ -0,0 +1,189 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2023-11-30 22:31:42Functions: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           3 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      77           3 :     return msg.throttle;
+      78             :   }
+      79           3 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      80           3 :     return msg.throttle;
+      81             :   }
+      82           3 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      83           3 :     return std::nullopt;
+      84             :   }
+      85           3 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      86           3 :     return std::nullopt;
+      87             :   }
+      88           3 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      89           3 :     return std::nullopt;
+      90             :   }
+      91           3 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      92           3 :     return std::nullopt;
+      93             :   }
+      94           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+      95           2 :     return std::nullopt;
+      96             :   }
+      97             : };
+      98             : 
+      99             : //}
+     100             : 
+     101             : }  // namespace common
+     102             : 
+     103             : }  // namespace mrs_uav_controllers
+     104             : 
+     105             : #endif  // MRS_UAV_CONTROLLERS_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.gcov.overview.html b/mrs_uav_controllers/include/common.h.gcov.overview.html new file mode 100644 index 0000000000..42538a882f --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.overview.html @@ -0,0 +1,47 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::reset()312
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)312
mrs_uav_controllers::PIDController::PIDController()312
mrs_uav_controllers::PIDController::setSaturation(double)3419
mrs_uav_controllers::PIDController::update(double const&, double const&)3419
+
+
+ + + +
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..6f4e84cdad --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:313393.9 %
Date:2023-11-30 22:31:42Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()0
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
(anonymous namespace)::ProxyExec0::ProxyExec0()26
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>)26
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()56
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)104
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)1005
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)16694
+
+
+ + + +
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..d4280972b4 --- /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:16219483.5 %
Date:2023-11-30 22:31:42Functions:81172.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()26
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()0
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>)26
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)1005
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)104
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)16694
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()56
+
+
+ + + +
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..ce59ee7cef --- /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:16219483.5 %
Date:2023-11-30 22:31:42Functions:81172.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 <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          26 : 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          26 :   nh_ = nh;
+     117             : 
+     118          26 :   common_handlers_  = common_handlers;
+     119          26 :   private_handlers_ = private_handlers;
+     120             : 
+     121          26 :   _uav_mass_ = common_handlers->getMass();
+     122             : 
+     123          26 :   ros::Time::waitForValid();
+     124             : 
+     125             :   // | ---------- loading params using the parent's nh ---------- |
+     126             : 
+     127          52 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     128             : 
+     129          26 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     130             : 
+     131          26 :   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          26 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/failsafe_controller.yaml");
+     139          26 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/failsafe_controller.yaml");
+     140             : 
+     141          52 :   const std::string yaml_namespace = "mrs_uav_controllers/failsafe_controller/";
+     142             : 
+     143          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/throttle_decrease_rate", _throttle_decrease_rate_);
+     144          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/initial_throttle_percentage", _initial_throttle_percentage_);
+     145             : 
+     146          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "attitude_controller/gains/kp", _kq_);
+     147             : 
+     148          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "rate_controller/gains/kp", _kw_);
+     149             : 
+     150          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "velocity_output/descend_speed", _descend_speed_);
+     151          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "acceleration_output/descend_acceleration", _descend_acceleration_);
+     152             : 
+     153          26 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     154           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     155           0 :     return false;
+     156             :   }
+     157             : 
+     158          26 :   _descend_speed_        = std::abs(_descend_speed_);
+     159          26 :   _descend_acceleration_ = std::abs(_descend_acceleration_);
+     160             : 
+     161          26 :   uav_mass_difference_ = 0;
+     162             : 
+     163             :   // | ----------------------- subscribers ---------------------- |
+     164             : 
+     165          26 :   mrs_lib::SubscribeHandlerOptions shopts;
+     166          26 :   shopts.nh                 = nh_;
+     167          26 :   shopts.node_name          = "FailsafeController";
+     168          26 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     169          26 :   shopts.threadsafe         = true;
+     170          26 :   shopts.autostart          = true;
+     171          26 :   shopts.queue_size         = 10;
+     172          26 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     173             : 
+     174          26 :   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          26 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     179             : 
+     180             :   // | ------------------------ profiler ------------------------ |
+     181             : 
+     182          26 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "FailsafeController", _profiler_enabled_);
+     183             : 
+     184             :   // | ----------------------- finish init ---------------------- |
+     185             : 
+     186          26 :   ROS_INFO("[FailsafeController]: initialized");
+     187             : 
+     188          26 :   is_initialized_ = true;
+     189             : 
+     190          26 :   return true;
+     191             : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* activate() //{ */
+     196             : 
+     197           8 : bool FailsafeController::activate(const ControlOutput &last_control_output) {
+     198             : 
+     199          16 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     200             : 
+     201           8 :   if (!last_control_output.control_output) {
+     202             : 
+     203           0 :     ROS_WARN("[FailsafeController]: activated without getting the last controller's command");
+     204             : 
+     205           0 :     return false;
+     206             : 
+     207             :   } else {
+     208             : 
+     209             :     // | -------------- calculate the initial heading ------------- |
+     210             : 
+     211           8 :     if (sh_hw_api_orientation_.getMsg()) {
+     212             : 
+     213          16 :       auto hw_api_orientation = sh_hw_api_orientation_.getMsg();
+     214             : 
+     215           8 :       heading_setpoint_ = getHeadingSafely(hw_api_orientation->quaternion);
+     216             : 
+     217           8 :       ROS_INFO("[FailsafeController]: activated with heading = %.2f rad", heading_setpoint_);
+     218             : 
+     219             :     } else {
+     220             : 
+     221           0 :       ROS_ERROR("[FailsafeController]: missing orientation from HW API, activated with heading = 0 rad");
+     222             : 
+     223           0 :       heading_setpoint_ = 0;
+     224             :     }
+     225             : 
+     226           8 :     activation_control_output_ = last_control_output;
+     227             : 
+     228           8 :     if (last_control_output.diagnostics.mass_estimator) {
+     229           4 :       uav_mass_difference_ = last_control_output.diagnostics.mass_difference;
+     230             :     } else {
+     231           4 :       uav_mass_difference_ = 0;
+     232             :     }
+     233             : 
+     234           8 :     activation_control_output_.diagnostics.controller_enforcing_constraints = false;
+     235             : 
+     236           8 :     hover_throttle_ = _initial_throttle_percentage_ * mrs_lib::quadratic_throttle_model::forceToThrottle(
+     237           8 :                                                           common_handlers_->throttle_model, (_uav_mass_ + uav_mass_difference_) * common_handlers_->g);
+     238             : 
+     239           8 :     ROS_INFO("[FailsafeController]: activated with uav_mass_difference %.2f kg.", uav_mass_difference_);
+     240             :   }
+     241             : 
+     242           8 :   first_iteration_ = true;
+     243             : 
+     244           8 :   is_active_ = true;
+     245             : 
+     246           8 :   return true;
+     247             : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* deactivate() //{ */
+     252             : 
+     253           0 : void FailsafeController::deactivate(void) {
+     254             : 
+     255           0 :   is_active_           = false;
+     256           0 :   first_iteration_     = false;
+     257           0 :   uav_mass_difference_ = 0;
+     258             : 
+     259           0 :   ROS_INFO("[FailsafeController]: deactivated");
+     260           0 : }
+     261             : 
+     262             : //}
+     263             : 
+     264             : /* updateInactive() //{ */
+     265             : 
+     266       16694 : void FailsafeController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     267             : 
+     268       16694 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     269             : 
+     270       16694 :   last_update_time_ = ros::Time::now();
+     271             : 
+     272       16694 :   first_iteration_ = false;
+     273       16694 : }
+     274             : 
+     275             : //}
+     276             : 
+     277             : /* //{ updateWhenAcctive() */
+     278             : 
+     279        1005 : FailsafeController::ControlOutput FailsafeController::updateActive(const mrs_msgs::UavState &                       uav_state,
+     280             :                                                                    [[maybe_unused]] const mrs_msgs::TrackerCommand &tracker_command) {
+     281             : 
+     282        3015 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     283        3015 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("FailsafeController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     284             : 
+     285             :   {
+     286        2010 :     std::scoped_lock lock(mutex_uav_state_);
+     287             : 
+     288        1005 :     uav_state_ = uav_state;
+     289             :   }
+     290             : 
+     291        1005 :   if (!is_active_) {
+     292           0 :     return ControlOutput();
+     293             :   }
+     294             : 
+     295        1005 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     296             : 
+     297             :   // | -------------------- calculate the dt -------------------- |
+     298             : 
+     299             :   double dt;
+     300             : 
+     301        1005 :   if (first_iteration_) {
+     302           8 :     dt               = 0.01;
+     303           8 :     first_iteration_ = false;
+     304             :   } else {
+     305         997 :     dt = (ros::Time::now() - last_update_time_).toSec();
+     306             :   }
+     307             : 
+     308        1005 :   last_update_time_ = ros::Time::now();
+     309             : 
+     310        1005 :   hover_throttle_ -= _throttle_decrease_rate_ * dt;
+     311             : 
+     312        1005 :   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        1005 :   } else if (hover_throttle_ > 1.0) {
+     316           0 :     hover_throttle_ = 1.0;
+     317        1005 :   } else if (hover_throttle_ < 0.0) {
+     318           0 :     hover_throttle_ = 0.0;
+     319             :   }
+     320             : 
+     321             :   // | --------------- prepare the control output --------------- |
+     322             : 
+     323        2010 :   FailsafeController::ControlOutput control_output;
+     324             : 
+     325        1005 :   control_output.diagnostics.controller = "FailsafeController";
+     326             : 
+     327        1005 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     328             : 
+     329        1005 :   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        1005 :   control_output.diagnostics.controller = "FailsafeController";
+     335             : 
+     336             :   // --------------------------------------------------------------
+     337             :   // |                       position output                      |
+     338             :   // --------------------------------------------------------------
+     339             : 
+     340        1005 :   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        1005 :   if (highest_modality.value() == common::VELOCITY_HDG) {
+     350             : 
+     351         248 :     mrs_msgs::HwApiVelocityHdgCmd vel_cmd;
+     352             : 
+     353         124 :     vel_cmd.header.stamp = ros::Time::now();
+     354             : 
+     355         124 :     vel_cmd.velocity.x = 0;
+     356         124 :     vel_cmd.velocity.y = 0;
+     357         124 :     vel_cmd.velocity.z = -_descend_speed_;
+     358         124 :     vel_cmd.heading    = heading_setpoint_;
+     359             : 
+     360         124 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning velocity+hdg output");
+     361         124 :     control_output.control_output = vel_cmd;
+     362         124 :     return control_output;
+     363             :   }
+     364             : 
+     365         881 :   if (highest_modality.value() == common::VELOCITY_HDG_RATE) {
+     366             : 
+     367         252 :     mrs_msgs::HwApiVelocityHdgRateCmd vel_cmd;
+     368             : 
+     369         126 :     vel_cmd.header.stamp = ros::Time::now();
+     370             : 
+     371         126 :     vel_cmd.velocity.x   = 0;
+     372         126 :     vel_cmd.velocity.y   = 0;
+     373         126 :     vel_cmd.velocity.z   = -_descend_speed_;
+     374         126 :     vel_cmd.heading_rate = 0.0;
+     375             : 
+     376         126 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning velocity+hdg rate output");
+     377         126 :     control_output.control_output = vel_cmd;
+     378         126 :     return control_output;
+     379             :   }
+     380             : 
+     381             :   // --------------------------------------------------------------
+     382             :   // |                     acceleration output                    |
+     383             :   // --------------------------------------------------------------
+     384             : 
+     385         755 :   if (highest_modality.value() == common::ACCELERATION_HDG) {
+     386             : 
+     387         248 :     mrs_msgs::HwApiAccelerationHdgCmd acc_cmd;
+     388             : 
+     389         124 :     acc_cmd.header.stamp = ros::Time::now();
+     390             : 
+     391         124 :     acc_cmd.acceleration.x = 0;
+     392         124 :     acc_cmd.acceleration.y = 0;
+     393         124 :     acc_cmd.acceleration.z = -_descend_acceleration_;
+     394         124 :     acc_cmd.heading        = heading_setpoint_;
+     395             : 
+     396         124 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning acceleration+hdg output");
+     397         124 :     control_output.control_output = acc_cmd;
+     398         124 :     return control_output;
+     399             :   }
+     400             : 
+     401         631 :   if (highest_modality.value() == common::ACCELERATION_HDG_RATE) {
+     402             : 
+     403         262 :     mrs_msgs::HwApiAccelerationHdgRateCmd acc_cmd;
+     404             : 
+     405         131 :     acc_cmd.header.stamp = ros::Time::now();
+     406             : 
+     407         131 :     acc_cmd.acceleration.x = 0;
+     408         131 :     acc_cmd.acceleration.y = 0;
+     409         131 :     acc_cmd.acceleration.z = -_descend_acceleration_;
+     410         131 :     acc_cmd.heading_rate   = 0.0;
+     411             : 
+     412         131 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning acceleration+hdg rate output");
+     413         131 :     control_output.control_output = acc_cmd;
+     414         131 :     return control_output;
+     415             :   }
+     416             : 
+     417             :   // --------------------------------------------------------------
+     418             :   // |                       attitude output                      |
+     419             :   // --------------------------------------------------------------
+     420             : 
+     421         500 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+     422             : 
+     423         500 :   attitude_cmd.stamp       = ros::Time::now();
+     424         500 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(0, 0, heading_setpoint_);
+     425         500 :   attitude_cmd.throttle    = hover_throttle_;
+     426             : 
+     427         500 :   if (highest_modality.value() == common::ATTITUDE) {
+     428         128 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude output");
+     429         128 :     control_output.control_output = attitude_cmd;
+     430         128 :     return control_output;
+     431             :   }
+     432             : 
+     433             :   // --------------------------------------------------------------
+     434             :   // |                      attitude control                      |
+     435             :   // --------------------------------------------------------------
+     436             : 
+     437         372 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+     438         372 :   Eigen::Vector3d rate_ff(0, 0, 0);
+     439         372 :   Eigen::Vector3d Kq(_kq_, _kq_, _kq_);
+     440             : 
+     441         372 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, rate_ff, attitude_rate_saturation, Kq, false);
+     442             : 
+     443         372 :   if (highest_modality.value() == common::ATTITUDE_RATE) {
+     444         124 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude rate output");
+     445         124 :     control_output.control_output = attitude_rate_command;
+     446         124 :     return control_output;
+     447             :   }
+     448             : 
+     449             :   // --------------------------------------------------------------
+     450             :   // |                    attitude rate control                   |
+     451             :   // --------------------------------------------------------------
+     452             : 
+     453         248 :   Eigen::Vector3d Kw = common_handlers_->detailed_model_params->inertia.diagonal() * _kw_;
+     454             : 
+     455         248 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+     456             : 
+     457         248 :   if (highest_modality.value() == common::CONTROL_GROUP) {
+     458         124 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning control group output");
+     459         124 :     control_output.control_output = control_group_command;
+     460         124 :     return control_output;
+     461             :   }
+     462             : 
+     463             :   // --------------------------------------------------------------
+     464             :   // |                            mixer                           |
+     465             :   // --------------------------------------------------------------
+     466             : 
+     467         248 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+     468             : 
+     469         124 :   ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning actuators output");
+     470         124 :   control_output.control_output = actuator_cmd;
+     471         124 :   return control_output;
+     472             : }
+     473             : 
+     474             : //}
+     475             : 
+     476             : /* getStatus() //{ */
+     477             : 
+     478          56 : const mrs_msgs::ControllerStatus FailsafeController::getStatus() {
+     479             : 
+     480          56 :   mrs_msgs::ControllerStatus controller_status;
+     481             : 
+     482          56 :   controller_status.active = is_active_;
+     483             : 
+     484          56 :   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         104 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr FailsafeController::setConstraints([
+     506             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     507             : 
+     508         104 :   if (!is_initialized_) {
+     509           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     510             :   }
+     511             : 
+     512         104 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     513             : 
+     514         104 :   ROS_INFO("[FailsafeController]: updating constraints");
+     515             : 
+     516         208 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     517         104 :   res.success = true;
+     518         104 :   res.message = "constraints updated";
+     519             : 
+     520         104 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     521             : }
+     522             : 
+     523             : //}
+     524             : 
+     525             : /* getHeadingSafely() //{ */
+     526             : 
+     527           8 : double FailsafeController::getHeadingSafely(const geometry_msgs::Quaternion &quaternion) {
+     528             : 
+     529             :   try {
+     530           8 :     return mrs_lib::AttitudeConverter(quaternion).getHeading();
+     531             :   }
+     532           0 :   catch (...) {
+     533             :   }
+     534             : 
+     535             :   try {
+     536           0 :     return mrs_lib::AttitudeConverter(quaternion).getYaw();
+     537             :   }
+     538           0 :   catch (...) {
+     539             :   }
+     540             : 
+     541           0 :   return 0;
+     542             : }
+     543             : 
+     544             : //}
+     545             : 
+     546             : }  // namespace failsafe_controller
+     547             : 
+     548             : }  // namespace mrs_uav_controllers
+     549             : 
+     550             : #include <pluginlib/class_list_macros.h>
+     551          26 : 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..e0c0514d928c2ed397120ae8169dc1e40831243a GIT binary patch literal 1815 zcmV+y2k7{TP)ybYz%kc+WrWS^>`t znT>ELO2OakqZUN2x8qpm_&uFPkbBu%%_I%|%vga8hB;+gx<=h9|Jo4g%2?si?0yg- zVjft;9zG|I&6O2{7pT&qpGBNg#N4JLMbQT_`YcDYfXq?GZe%@Bh<+?bpg0wLOPmM_ zx;|^pGUtNgsh}XuRO-iBKD>oX`UO*5+))j2q#(a*U`L-z!ez9{j=(8|vf=Gvp&XGy zIZ6Op$VbB%Mj|+h6MR0lAktc- zu-i-l;mi`5$y9I5!t9wBOrD+~HJ`aOycv4yT|5gbSN~K<%~Mt}8ElmpLsuA!IDV&@ z??|v2qG0%piUx}M%wj~Tjsxp6+ord)nfN`tIjvdCK4%5lZQYF7OpcqjLf17xuf%G? zPqd&3f<;B7U2AO20f^){gor*;_#{U8g>!cKu9%QO$DBssdF!ZCoaiYDVp%nv3O*jM zq&+|Xp0EA=W!vM^tl;~-{Xw_saVFsf`3(m9*}JYMy{;NOX#zokukG~nlTepEHGn+{ zx-aA6D-?1VYmT;RYcwax2F3_V%{DU^LtK3xdK zW+^MeoXH2=t7BXZJ9lv_rJxz0qe_oH`X`rHD<=h4N9Y+U9hV_$DrfbAjO^bEvfEYr z!jVVfg=E=e95Y5{@B^|m`3>Ml=X#x{%p$~WVc-3W{l6cp`Rg_DR%=lPz(t(qrlU}@ zg$OWv9-%U_JdhS#Q*eyL*Xs%>na96I#2Q?9rx6kI+Lb8Yyf^lVAT$qm_M@1)T2>zk z?9*~y!tj8&?=Mo2m*k3Qh8ULYzmaFSF0AeWjKP8^%dOI9Iy9K0XwpB%lzt z1o1J}1D;eOFoiJ67JMuFBx0&m$0Y6XT0=o*nL^O@g@+`=Xi=^l$4I8#YhRU11JEca zMjU)x;KC18#$iVacGs7X0^Io+0bi#BcIo-39l7{jgL)72QKAo`nIW@WK}O0o9Z(JQ z)I*9Do?nq=#S0d8H;9u&E^PN~6s6+h6$M{ip~41BFpY(wurmaY8ZWSjKShojYznEt zyxHS^q_e4MWyZaUnb_M+g@&M&32|jcI($_yXkO>12r3bVMR?g?apoq|7P}B^5q*Jpch7(6 z!dn}>P+()u!i2d8Vp*^OF)r7G5YJA;TLkMW!b%*t22en5G&qc;I=uKYcjB3fYpkGG zj_mrn%s%JihVFc{nJ3z)B-nsBoR2HTvsOV;M>H6!%q<@b#YcS$KDNd1c*i$B`$-H( zQT)%&nX~hmHbFT0iXgtsl_8IvGoLqZF=yx)9;0{e zIBdFVrj{N1x>6|s?m8`2g2@X7LGqfrtGU!v-|n6B(Vi+-_?T;XLq2}qQQzictuo`E zCxW_qhZPIz<6nR26VRtrX4g(fK4N|4(nyfQM_t`>xt1Z`iI0|!Ga}bF_()w=7XD$w z346Hbg%NQWd-Pvtk7W?1!FN~JQqj4s9$wCmj+d-(UR#gCMW8be-s&Qd8gsP&Y3Wjw z&ew)bMH#PW7l#0K)s^DgVHM4Q;EhOOr|^)wK&9JZ_*}rgf3JH|!z@IZ2VSDPmw5(W zBdeb~!tes>J;X8T8~>%upaSn`v>lrKq)~D002ovPDHLk FV1g`|Uq%1` 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..cecdc5e9b0 --- /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:1740217280.1 %
Date:2023-11-30 22:31:42Functions:556683.3 %
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 +
83.5%83.5%
+ 		83.5 %162 / 19472.7 %8 / 11
<unnamed>83.5 %162 / 19472.7 %8 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
se3_controller.cpp +
77.8%77.8%
+ 		77.8 %602 / 77482.4 %14 / 17
<unnamed>77.8 %602 / 77482.4 %14 / 17
mpc_controller.cpp +
79.2%79.2%
+ 		79.2 %708 / 89483.3 %15 / 18
<unnamed>79.2 %708 / 89483.3 %15 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
+
+
+ + + + +
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..f07f4e2ce5 --- /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:1740217280.1 %
Date:2023-11-30 22:31:42Functions:556683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
se3_controller.cpp +
77.8%77.8%
+ 		77.8 %602 / 77482.4 %14 / 17
<unnamed>77.8 %602 / 77482.4 %14 / 17
mpc_controller.cpp +
79.2%79.2%
+ 		79.2 %708 / 89483.3 %15 / 18
<unnamed>79.2 %708 / 89483.3 %15 / 18
failsafe_controller.cpp +
83.5%83.5%
+ 		83.5 %162 / 19472.7 %8 / 11
<unnamed>83.5 %162 / 19472.7 %8 / 11
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-detail.html b/mrs_uav_controllers/src/index-detail.html new file mode 100644 index 0000000000..ba6b87ed44 --- /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:1740217280.1 %
Date:2023-11-30 22:31:42Functions:556683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
83.5%83.5%
+ 		83.5 %162 / 19472.7 %8 / 11
<unnamed>83.5 %162 / 19472.7 %8 / 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 +
79.2%79.2%
+ 		79.2 %708 / 89483.3 %15 / 18
<unnamed>79.2 %708 / 89483.3 %15 / 18
se3_controller.cpp +
77.8%77.8%
+ 		77.8 %602 / 77482.4 %14 / 17
<unnamed>77.8 %602 / 77482.4 %14 / 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..afa0dcfd6a --- /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:1740217280.1 %
Date:2023-11-30 22:31:42Functions:556683.3 %
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 +
83.5%83.5%
+ 		83.5 %162 / 19472.7 %8 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
se3_controller.cpp +
77.8%77.8%
+ 		77.8 %602 / 77482.4 %14 / 17
mpc_controller.cpp +
79.2%79.2%
+ 		79.2 %708 / 89483.3 %15 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-sort-l.html b/mrs_uav_controllers/src/index-sort-l.html new file mode 100644 index 0000000000..d36e569a08 --- /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:1740217280.1 %
Date:2023-11-30 22:31:42Functions:556683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
se3_controller.cpp +
77.8%77.8%
+ 		77.8 %602 / 77482.4 %14 / 17
mpc_controller.cpp +
79.2%79.2%
+ 		79.2 %708 / 89483.3 %15 / 18
failsafe_controller.cpp +
83.5%83.5%
+ 		83.5 %162 / 19472.7 %8 / 11
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index.html b/mrs_uav_controllers/src/index.html new file mode 100644 index 0000000000..d836a197da --- /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:1740217280.1 %
Date:2023-11-30 22:31:42Functions:556683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
83.5%83.5%
+ 		83.5 %162 / 19472.7 %8 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
79.2%79.2%
+ 		79.2 %708 / 89483.3 %15 / 18
se3_controller.cpp +
77.8%77.8%
+ 		77.8 %602 / 77482.4 %14 / 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..5488034b17 --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2023-11-30 22:31:42Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::midair_activation_controller::MidairActivationController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::midair_activation_controller::MidairActivationController::resetDisturbanceEstimators()0
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)13
(anonymous namespace)::ProxyExec0::ProxyExec0()26
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()26
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>)26
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()32
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)52
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)104
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)143
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)17556
+
+
+ + + +
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..a9e81c9d06 --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2023-11-30 22:31:42Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::mpc_controller::MpcController::callbackSetIntegralTerms(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_controllers::mpc_controller::MpcController::resetDisturbanceEstimators()0
(anonymous namespace)::ProxyExec0::ProxyExec0()26
mrs_uav_controllers::mpc_controller::MpcController::deactivate()34
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)43
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>)52
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)52
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)180
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)208
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&)401
mrs_uav_controllers::mpc_controller::MpcController::getStatus()1034
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)2129
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&)6522
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)6923
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)7103
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)17032
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)28295
+
+
+ + + +
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..9112da1404 --- /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:70889479.2 %
Date:2023-11-30 22:31:42Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()26
mrs_uav_controllers::mpc_controller::MpcController::deactivate()34
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>)52
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)2129
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)52
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)7103
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)208
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)28295
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)6923
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&)401
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)17032
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)180
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::mpc_controller::MpcController::callbackSetIntegralTerms(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_controllers::mpc_controller::MpcController::resetDisturbanceEstimators()0
mrs_uav_controllers::mpc_controller::MpcController::MPC(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&)6522
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)43
mrs_uav_controllers::mpc_controller::MpcController::getStatus()1034
+
+
+ + + +
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..9f0721614a --- /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:70889479.2 %
Date:2023-11-30 22:31:42Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <common.h>
+       7             : #include <pid.hpp>
+       8             : 
+       9             : #include <mrs_uav_managers/controller.h>
+      10             : 
+      11             : #include <mpc_controller.h>
+      12             : 
+      13             : #include <dynamic_reconfigure/server.h>
+      14             : #include <mrs_uav_controllers/mpc_controllerConfig.h>
+      15             : 
+      16             : #include <std_srvs/SetBool.h>
+      17             : 
+      18             : #include <mrs_lib/profiler.h>
+      19             : #include <mrs_lib/utils.h>
+      20             : #include <mrs_lib/mutex.h>
+      21             : #include <mrs_lib/attitude_converter.h>
+      22             : #include <mrs_lib/geometry/cyclic.h>
+      23             : 
+      24             : #include <geometry_msgs/Vector3Stamped.h>
+      25             : 
+      26             : //}
+      27             : 
+      28             : #define OUTPUT_ACTUATORS 0
+      29             : #define OUTPUT_CONTROL_GROUP 1
+      30             : #define OUTPUT_ATTITUDE_RATE 2
+      31             : #define OUTPUT_ATTITUDE 3
+      32             : 
+      33             : namespace mrs_uav_controllers
+      34             : {
+      35             : 
+      36             : namespace mpc_controller
+      37             : {
+      38             : 
+      39             : /* structs //{ */
+      40             : 
+      41             : typedef struct
+      42             : {
+      43             :   double kiwxy;          // world xy integral gain
+      44             :   double kibxy;          // body xy integral gain
+      45             :   double kiwxy_lim;      // world xy integral limit
+      46             :   double kibxy_lim;      // body xy integral limit
+      47             :   double km;             // mass estimator gain
+      48             :   double km_lim;         // mass estimator limit
+      49             :   double kq_roll_pitch;  // pitch/roll attitude gain
+      50             :   double kq_yaw;         // yaw attitude gain
+      51             :   double kw_rp;          // attitude rate gain
+      52             :   double kw_y;           // attitude rate gain
+      53             : } Gains_t;
+      54             : 
+      55             : //}
+      56             : 
+      57             : /* //{ class MpcController */
+      58             : 
+      59             : class MpcController : public mrs_uav_managers::Controller {
+      60             : 
+      61             : public:
+      62             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      63             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      64             : 
+      65             :   bool activate(const ControlOutput &last_control_output);
+      66             : 
+      67             :   void deactivate(void);
+      68             : 
+      69             :   void updateInactive(const mrs_msgs::UavState &uav_state, const std::optional<mrs_msgs::TrackerCommand> &tracker_command);
+      70             : 
+      71             :   ControlOutput updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+      72             : 
+      73             :   const mrs_msgs::ControllerStatus getStatus();
+      74             : 
+      75             :   void switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      76             : 
+      77             :   void resetDisturbanceEstimators(void);
+      78             : 
+      79             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      80             : 
+      81             : private:
+      82             :   ros::NodeHandle nh_;
+      83             : 
+      84             :   bool is_initialized_ = false;
+      85             :   bool is_active_      = false;
+      86             : 
+      87             :   std::string name_;
+      88             : 
+      89             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      90             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      91             : 
+      92             :   // | ------------------------ uav state ----------------------- |
+      93             : 
+      94             :   mrs_msgs::UavState uav_state_;
+      95             :   std::mutex         mutex_uav_state_;
+      96             : 
+      97             :   // | --------------- dynamic reconfigure server --------------- |
+      98             : 
+      99             :   boost::recursive_mutex                            mutex_drs_;
+     100             :   typedef mrs_uav_controllers::mpc_controllerConfig DrsConfig_t;
+     101             :   typedef dynamic_reconfigure::Server<DrsConfig_t>  Drs_t;
+     102             :   boost::shared_ptr<Drs_t>                          drs_;
+     103             :   void                                              callbackDrs(mrs_uav_controllers::mpc_controllerConfig &config, uint32_t level);
+     104             :   DrsConfig_t                                       drs_params_;
+     105             : 
+     106             :   // | ----------------------- controllers ---------------------- |
+     107             : 
+     108             :   void positionPassthrough(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+     109             : 
+     110             :   void PIDVelocityOutput(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command, const common::CONTROL_OUTPUT &control_output,
+     111             :                          const double &dt);
+     112             : 
+     113             :   void MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command, const double &dt,
+     114             :            const common::CONTROL_OUTPUT &output_modality);
+     115             : 
+     116             :   // | ----------------------- constraints ---------------------- |
+     117             : 
+     118             :   mrs_msgs::DynamicsConstraints constraints_;
+     119             :   std::mutex                    mutex_constraints_;
+     120             : 
+     121             :   // | -------- throttle generation and mass estimation --------- |
+     122             : 
+     123             :   double _uav_mass_;
+     124             :   double uav_mass_difference_;
+     125             : 
+     126             :   Gains_t gains_;
+     127             : 
+     128             :   // | ------------------- configurable gains ------------------- |
+     129             : 
+     130             :   std::mutex mutex_gains_;       // locks the gains the are used and filtered
+     131             :   std::mutex mutex_drs_params_;  // locks the gains that came from the drs
+     132             : 
+     133             :   ros::Timer timer_gains_;
+     134             :   void       timerGains(const ros::TimerEvent &event);
+     135             : 
+     136             :   double _gain_filtering_rate_;
+     137             : 
+     138             :   // | --------------------- gain filtering --------------------- |
+     139             : 
+     140             :   double calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name, bool &updated);
+     141             : 
+     142             :   double getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+     143             : 
+     144             :   double _gains_filter_change_rate_;
+     145             :   double _gains_filter_min_change_rate_;
+     146             : 
+     147             :   // | ----------------------- gain muting ---------------------- |
+     148             : 
+     149             :   std::atomic<bool> mute_gains_            = false;
+     150             :   std::atomic<bool> mute_gains_by_tracker_ = false;
+     151             :   double            _gain_mute_coefficient_;
+     152             : 
+     153             :   // | ------------ controller limits and saturations ----------- |
+     154             : 
+     155             :   bool   _tilt_angle_failsafe_enabled_;
+     156             :   double _tilt_angle_failsafe_;
+     157             : 
+     158             :   double _throttle_saturation_;
+     159             : 
+     160             :   // | ------------------ activation and output ----------------- |
+     161             : 
+     162             :   ControlOutput last_control_output_;
+     163             :   ControlOutput activation_control_output_;
+     164             : 
+     165             :   ros::Time         last_update_time_;
+     166             :   std::atomic<bool> first_iteration_ = true;
+     167             : 
+     168             :   // | ----------------- integral terms enabler ----------------- |
+     169             : 
+     170             :   ros::ServiceServer service_set_integral_terms_;
+     171             :   bool               callbackSetIntegralTerms(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+     172             :   bool               integral_terms_enabled_ = true;
+     173             : 
+     174             :   // | --------------------- MPC controller --------------------- |
+     175             : 
+     176             :   // number of states
+     177             :   int _n_states_;
+     178             : 
+     179             :   // time steps
+     180             :   double _dt1_;  // the first time step
+     181             :   double _dt2_;  // all the other steps
+     182             : 
+     183             :   // the last control input
+     184             :   double mpc_solver_x_u_ = 0;
+     185             :   double mpc_solver_y_u_ = 0;
+     186             :   double mpc_solver_z_u_ = 0;
+     187             : 
+     188             :   int _horizon_length_;
+     189             : 
+     190             :   // constraints
+     191             :   double _max_speed_horizontal_, _max_acceleration_horizontal_, _max_jerk_;
+     192             :   double _max_speed_vertical_, _max_acceleration_vertical_, _max_u_vertical_;
+     193             : 
+     194             :   // Q and S matrix diagonals for horizontal
+     195             :   std::vector<double> _mat_Q_, _mat_S_;
+     196             : 
+     197             :   // Q and S matrix diagonals for vertical
+     198             :   std::vector<double> _mat_Q_z_, _mat_S_z_;
+     199             : 
+     200             :   // MPC solver handlers
+     201             :   std::shared_ptr<mrs_mpc_solvers::mpc_controller::Solver> mpc_solver_x_;
+     202             :   std::shared_ptr<mrs_mpc_solvers::mpc_controller::Solver> mpc_solver_y_;
+     203             :   std::shared_ptr<mrs_mpc_solvers::mpc_controller::Solver> mpc_solver_z_;
+     204             : 
+     205             :   // MPC solver params
+     206             :   bool _mpc_solver_verbose_ = false;
+     207             :   int  _mpc_solver_max_iterations_;
+     208             : 
+     209             :   // | ------------------------ profiler ------------------------ |
+     210             : 
+     211             :   mrs_lib::Profiler profiler_;
+     212             :   bool              _profiler_enabled_ = false;
+     213             : 
+     214             :   // | ------------------------ integrals ----------------------- |
+     215             : 
+     216             :   Eigen::Vector2d Ib_b_;  // body error integral in the body frame
+     217             :   Eigen::Vector2d Iw_w_;  // world error integral in the world_frame
+     218             :   std::mutex      mutex_integrals_;
+     219             : 
+     220             :   // | ------------------------- rampup ------------------------- |
+     221             : 
+     222             :   bool   _rampup_enabled_ = false;
+     223             :   double _rampup_speed_;
+     224             : 
+     225             :   bool      rampup_active_ = false;
+     226             :   double    rampup_throttle_;
+     227             :   int       rampup_direction_;
+     228             :   double    rampup_duration_;
+     229             :   ros::Time rampup_start_time_;
+     230             :   ros::Time rampup_last_time_;
+     231             : 
+     232             :   // | ---------------------- position pid ---------------------- |
+     233             : 
+     234             :   double _pos_pid_p_;
+     235             :   double _pos_pid_i_;
+     236             :   double _pos_pid_d_;
+     237             : 
+     238             :   double _hdg_pid_p_;
+     239             :   double _hdg_pid_i_;
+     240             :   double _hdg_pid_d_;
+     241             : 
+     242             :   PIDController position_pid_x_;
+     243             :   PIDController position_pid_y_;
+     244             :   PIDController position_pid_z_;
+     245             :   PIDController position_pid_heading_;
+     246             : };
+     247             : 
+     248             : //}
+     249             : 
+     250             : // --------------------------------------------------------------
+     251             : // |                   controller's interface                   |
+     252             : // --------------------------------------------------------------
+     253             : 
+     254             : /* //{ initialize() */
+     255             : 
+     256          52 : 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          52 :   nh_ = nh;
+     260             : 
+     261          52 :   common_handlers_  = common_handlers;
+     262          52 :   private_handlers_ = private_handlers;
+     263             : 
+     264          52 :   _uav_mass_ = common_handlers_->getMass();
+     265          52 :   name_      = private_handlers_->runtime_name;
+     266             : 
+     267          52 :   ros::Time::waitForValid();
+     268             : 
+     269             :   // | ---------- loading params using the parent's nh ---------- |
+     270             : 
+     271         104 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     272             : 
+     273          52 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     274             : 
+     275          52 :   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          52 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/mpc_controller.yaml");
+     283          52 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/mpc_controller.yaml");
+     284          52 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/" + private_handlers->name_space + ".yaml");
+     285          52 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/" + private_handlers->name_space + ".yaml");
+     286             : 
+     287         104 :   const std::string yaml_namespace = "mrs_uav_controllers/" + private_handlers_->name_space + "/";
+     288             : 
+     289             :   // load the dynamicall model parameters
+     290          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/number_of_states", _n_states_);
+     291          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt1", _dt1_);
+     292          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt2", _dt2_);
+     293             : 
+     294          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizon_length", _horizon_length_);
+     295             : 
+     296          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_speed", _max_speed_horizontal_);
+     297          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_acceleration", _max_acceleration_horizontal_);
+     298          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_jerk", _max_jerk_);
+     299             : 
+     300          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/Q", _mat_Q_);
+     301          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/S", _mat_S_);
+     302             : 
+     303          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_speed", _max_speed_vertical_);
+     304          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_acceleration", _max_acceleration_vertical_);
+     305          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_u", _max_u_vertical_);
+     306             : 
+     307          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/Q", _mat_Q_z_);
+     308          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/S", _mat_S_z_);
+     309             : 
+     310          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/verbose", _mpc_solver_verbose_);
+     311          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/max_iterations", _mpc_solver_max_iterations_);
+     312             : 
+     313             :   // | ------------------------- rampup ------------------------- |
+     314             : 
+     315          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/enabled", _rampup_enabled_);
+     316          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/speed", _rampup_speed_);
+     317             : 
+     318             :   // | --------------------- integral gains --------------------- |
+     319             : 
+     320          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw", gains_.kiwxy);
+     321          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kib", gains_.kibxy);
+     322             : 
+     323             :   // integrator limits
+     324          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw_lim", gains_.kiwxy_lim);
+     325          52 :   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          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/roll_pitch", gains_.kq_roll_pitch);
+     331          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/yaw", gains_.kq_yaw);
+     332             : 
+     333             :   // attitude rate gains
+     334          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/roll_pitch", gains_.kw_rp);
+     335          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/yaw", gains_.kw_y);
+     336             : 
+     337             :   // mass estimator
+     338          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km", gains_.km);
+     339          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km_lim", gains_.km_lim);
+     340             : 
+     341             :   // constraints
+     342          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     343          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     344             : 
+     345          52 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     346             : 
+     347          52 :   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          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/throttle_saturation", _throttle_saturation_);
+     353             : 
+     354             :   // gain filtering
+     355          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     356          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     357          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/rate", _gain_filtering_rate_);
+     358          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     359             : 
+     360             :   // angular rate feed forward
+     361          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     362             : 
+     363             :   // output mode
+     364          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/preferred_output", drs_params_.preferred_output_mode);
+     365             : 
+     366             :   // | ------------------- position pid params ------------------ |
+     367             : 
+     368          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     369          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     370          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     371             : 
+     372          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     373          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     374          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     375             : 
+     376          52 :   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          52 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     384          52 :         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          52 :   uav_mass_difference_ = 0;
+     390          52 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     391          52 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     392             : 
+     393             :   // | ----------------- prepare the MPC solver ----------------- |
+     394             : 
+     395          52 :   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          52 :                                                                             _dt2_, 0, 1.0);
+     397          52 :   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          52 :                                                                             _dt2_, 0, 1.0);
+     399          52 :   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          52 :                                                                             _dt1_, _dt2_, 0.5, 0.5);
+     401             : 
+     402             :   // | --------------- dynamic reconfigure server --------------- |
+     403             : 
+     404          52 :   drs_params_.kiwxy         = gains_.kiwxy;
+     405          52 :   drs_params_.kibxy         = gains_.kibxy;
+     406          52 :   drs_params_.kq_roll_pitch = gains_.kq_roll_pitch;
+     407          52 :   drs_params_.kq_yaw        = gains_.kq_yaw;
+     408          52 :   drs_params_.km            = gains_.km;
+     409          52 :   drs_params_.km_lim        = gains_.km_lim;
+     410          52 :   drs_params_.kiwxy_lim     = gains_.kiwxy_lim;
+     411          52 :   drs_params_.kibxy_lim     = gains_.kibxy_lim;
+     412             : 
+     413          52 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     414          52 :   drs_->updateConfig(drs_params_);
+     415          52 :   Drs_t::CallbackType f = boost::bind(&MpcController::callbackDrs, this, _1, _2);
+     416          52 :   drs_->setCallback(f);
+     417             : 
+     418             :   // | --------------------- service servers -------------------- |
+     419             : 
+     420          52 :   service_set_integral_terms_ = nh_.advertiseService("set_integral_terms_in", &MpcController::callbackSetIntegralTerms, this);
+     421             : 
+     422             :   // | ------------------------- timers ------------------------- |
+     423             : 
+     424          52 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &MpcController::timerGains, this, false, false);
+     425             : 
+     426             :   // | ---------------------- position pid ---------------------- |
+     427             : 
+     428          52 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     429          52 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     430          52 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     431          52 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     432             : 
+     433             :   // | ------------------------ profiler ------------------------ |
+     434             : 
+     435          52 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MpcController", _profiler_enabled_);
+     436             : 
+     437             :   // | ----------------------- finish init ---------------------- |
+     438             : 
+     439          52 :   ROS_INFO("[%s]: initialized", this->name_.c_str());
+     440             : 
+     441          52 :   is_initialized_ = true;
+     442             : 
+     443          52 :   return true;
+     444             : }
+     445             : 
+     446             : //}
+     447             : 
+     448             : /* //{ activate() */
+     449             : 
+     450          43 : bool MpcController::activate(const ControlOutput &last_control_output) {
+     451             : 
+     452          43 :   activation_control_output_ = last_control_output;
+     453             : 
+     454          43 :   double activation_mass = _uav_mass_;
+     455             : 
+     456          43 :   if (activation_control_output_.diagnostics.mass_estimator) {
+     457           0 :     uav_mass_difference_ = activation_control_output_.diagnostics.mass_difference;
+     458           0 :     activation_mass += uav_mass_difference_;
+     459           0 :     ROS_INFO("[%s]: setting mass difference from the last control output: %.2f kg", this->name_.c_str(), uav_mass_difference_);
+     460             :   }
+     461             : 
+     462          43 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     463             : 
+     464          43 :   if (activation_control_output_.diagnostics.disturbance_estimator) {
+     465           0 :     Ib_b_[0] = -activation_control_output_.diagnostics.disturbance_bx_b;
+     466           0 :     Ib_b_[1] = -activation_control_output_.diagnostics.disturbance_by_b;
+     467             : 
+     468           0 :     Iw_w_[0] = -activation_control_output_.diagnostics.disturbance_wx_w;
+     469           0 :     Iw_w_[1] = -activation_control_output_.diagnostics.disturbance_wy_w;
+     470             : 
+     471           0 :     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          43 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     479             : 
+     480             :   // rampup check
+     481          43 :   if (_rampup_enabled_ && throttle_last_controller) {
+     482             : 
+     483           8 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     484             : 
+     485           8 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     486             : 
+     487           8 :     if (throttle_difference > 0) {
+     488           4 :       rampup_direction_ = 1;
+     489           4 :     } else if (throttle_difference < 0) {
+     490           0 :       rampup_direction_ = -1;
+     491             :     } else {
+     492           4 :       rampup_direction_ = 0;
+     493             :     }
+     494             : 
+     495           8 :     ROS_INFO("[%s]: activating rampup with initial throttle: %.4f, target: %.4f", name_.c_str(), throttle_last_controller.value(), hover_throttle);
+     496             : 
+     497           8 :     rampup_active_     = true;
+     498           8 :     rampup_start_time_ = ros::Time::now();
+     499           8 :     rampup_last_time_  = ros::Time::now();
+     500           8 :     rampup_throttle_   = throttle_last_controller.value();
+     501             : 
+     502           8 :     rampup_duration_ = fabs(throttle_difference) / _rampup_speed_;
+     503             :   }
+     504             : 
+     505          43 :   first_iteration_ = true;
+     506          43 :   mute_gains_      = true;
+     507             : 
+     508          43 :   timer_gains_.start();
+     509             : 
+     510          43 :   ROS_INFO("[%s]: activated", this->name_.c_str());
+     511             : 
+     512          43 :   is_active_ = true;
+     513             : 
+     514          43 :   return true;
+     515             : }
+     516             : 
+     517             : //}
+     518             : 
+     519             : /* //{ deactivate() */
+     520             : 
+     521          34 : void MpcController::deactivate(void) {
+     522             : 
+     523          34 :   is_active_           = false;
+     524          34 :   first_iteration_     = false;
+     525          34 :   uav_mass_difference_ = 0;
+     526             : 
+     527          34 :   timer_gains_.stop();
+     528             : 
+     529          34 :   ROS_INFO("[%s]: deactivated", this->name_.c_str());
+     530          34 : }
+     531             : 
+     532             : //}
+     533             : 
+     534             : /* updateInactive() //{ */
+     535             : 
+     536       28295 : void MpcController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     537             : 
+     538       28295 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     539             : 
+     540       28295 :   last_update_time_ = uav_state.header.stamp;
+     541             : 
+     542       28295 :   first_iteration_ = false;
+     543       28295 : }
+     544             : 
+     545             : //}
+     546             : 
+     547             : /* //{ updateWhenAcctive() */
+     548             : 
+     549        7103 : MpcController::ControlOutput MpcController::updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     550             : 
+     551       21309 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     552       21309 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     553             : 
+     554       14206 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     555             : 
+     556        7103 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     557             : 
+     558        7103 :   last_control_output_.desired_heading_rate          = {};
+     559        7103 :   last_control_output_.desired_orientation           = {};
+     560        7103 :   last_control_output_.desired_unbiased_acceleration = {};
+     561        7103 :   last_control_output_.control_output                = {};
+     562             : 
+     563        7103 :   if (!is_active_) {
+     564           0 :     return last_control_output_;
+     565             :   }
+     566             : 
+     567             :   // | -------------------- calculate the dt -------------------- |
+     568             : 
+     569             :   double dt;
+     570             : 
+     571        7103 :   if (first_iteration_) {
+     572          17 :     dt               = 0.01;
+     573          17 :     first_iteration_ = false;
+     574             :   } else {
+     575        7086 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     576             :   }
+     577             : 
+     578        7103 :   last_update_time_ = uav_state.header.stamp;
+     579             : 
+     580        7103 :   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        7103 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     590             : 
+     591        7103 :   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        7103 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     601         962 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing attitude rate output", name_.c_str());
+     602         962 :     lowest_modality = common::ATTITUDE_RATE;
+     603        6141 :   } 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        6141 :   } 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        6141 :   } 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        7103 :   switch (lowest_modality.value()) {
+     615             : 
+     616         180 :     case common::POSITION: {
+     617         180 :       positionPassthrough(uav_state, tracker_command);
+     618         180 :       break;
+     619             :     }
+     620             : 
+     621         202 :     case common::VELOCITY_HDG: {
+     622         202 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     623         202 :       break;
+     624             :     }
+     625             : 
+     626         199 :     case common::VELOCITY_HDG_RATE: {
+     627         199 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     628         199 :       break;
+     629             :     }
+     630             : 
+     631         403 :     case common::ACCELERATION_HDG: {
+     632         403 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     633         403 :       break;
+     634             :     }
+     635             : 
+     636         403 :     case common::ACCELERATION_HDG_RATE: {
+     637         403 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     638         403 :       break;
+     639             :     }
+     640             : 
+     641         836 :     case common::ATTITUDE: {
+     642         836 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE);
+     643         836 :       break;
+     644             :     }
+     645             : 
+     646         962 :     case common::ATTITUDE_RATE: {
+     647         962 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     648         962 :       break;
+     649             :     }
+     650             : 
+     651        1913 :     case common::CONTROL_GROUP: {
+     652        1913 :       MPC(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     653        1913 :       break;
+     654             :     }
+     655             : 
+     656        2005 :     case common::ACTUATORS_CMD: {
+     657        2005 :       MPC(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     658        2005 :       break;
+     659             :     }
+     660             : 
+     661           0 :     default: {
+     662           0 :       break;
+     663             :     }
+     664             :   }
+     665             : 
+     666        7103 :   return last_control_output_;
+     667             : }
+     668             : 
+     669             : //}
+     670             : 
+     671             : /* //{ getStatus() */
+     672             : 
+     673        1034 : const mrs_msgs::ControllerStatus MpcController::getStatus() {
+     674             : 
+     675        1034 :   mrs_msgs::ControllerStatus controller_status;
+     676             : 
+     677        1034 :   controller_status.active = is_active_;
+     678             : 
+     679        1034 :   return controller_status;
+     680             : }
+     681             : 
+     682             : //}
+     683             : 
+     684             : /* switchOdometrySource() //{ */
+     685             : 
+     686           0 : void MpcController::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     687             : 
+     688           0 :   ROS_INFO("[%s]: switching the odometry source", this->name_.c_str());
+     689             : 
+     690           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     691             : 
+     692             :   // | ----- transform world disturabances to the new frame ----- |
+     693             : 
+     694           0 :   geometry_msgs::Vector3Stamped world_integrals;
+     695             : 
+     696           0 :   world_integrals.header.stamp    = ros::Time::now();
+     697           0 :   world_integrals.header.frame_id = uav_state.header.frame_id;
+     698             : 
+     699           0 :   world_integrals.vector.x = Iw_w_[0];
+     700           0 :   world_integrals.vector.y = Iw_w_[1];
+     701           0 :   world_integrals.vector.z = 0;
+     702             : 
+     703           0 :   auto res = common_handlers_->transformer->transformSingle(world_integrals, new_uav_state.header.frame_id);
+     704             : 
+     705           0 :   if (res) {
+     706             : 
+     707           0 :     std::scoped_lock lock(mutex_integrals_);
+     708             : 
+     709           0 :     Iw_w_[0] = res.value().vector.x;
+     710           0 :     Iw_w_[1] = res.value().vector.y;
+     711             : 
+     712             :   } else {
+     713             : 
+     714           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: could not transform world integral to the new frame", this->name_.c_str());
+     715             : 
+     716           0 :     std::scoped_lock lock(mutex_integrals_);
+     717             : 
+     718           0 :     Iw_w_[0] = 0;
+     719           0 :     Iw_w_[1] = 0;
+     720             :   }
+     721           0 : }
+     722             : 
+     723             : //}
+     724             : 
+     725             : /* resetDisturbanceEstimators() //{ */
+     726             : 
+     727           0 : void MpcController::resetDisturbanceEstimators(void) {
+     728             : 
+     729           0 :   std::scoped_lock lock(mutex_integrals_);
+     730             : 
+     731           0 :   Iw_w_ = Eigen::Vector2d::Zero(2);
+     732           0 :   Ib_b_ = Eigen::Vector2d::Zero(2);
+     733           0 : }
+     734             : 
+     735             : //}
+     736             : 
+     737             : /* setConstraints() //{ */
+     738             : 
+     739         208 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcController::setConstraints([
+     740             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     741             : 
+     742         208 :   if (!is_initialized_) {
+     743           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     744             :   }
+     745             : 
+     746         208 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     747             : 
+     748         208 :   ROS_INFO("[%s]: updating constraints", this->name_.c_str());
+     749             : 
+     750         416 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     751         208 :   res.success = true;
+     752         208 :   res.message = "constraints updated";
+     753             : 
+     754         208 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     755             : }
+     756             : 
+     757             : //}
+     758             : 
+     759             : // --------------------------------------------------------------
+     760             : // |                         controllers                        |
+     761             : // --------------------------------------------------------------
+     762             : 
+     763             : /* Mpc() //{ */
+     764             : 
+     765        6522 : 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       13044 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     769        6522 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     770        6522 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     771             : 
+     772             :   // | ----------------- get the current heading ---------------- |
+     773             : 
+     774        6522 :   double uav_heading = getHeadingSafely(uav_state, tracker_command);
+     775             : 
+     776             :   // | ------------------- prepare constraints ------------------ |
+     777             : 
+     778        6522 :   double max_speed_horizontal        = _max_speed_horizontal_;
+     779        6522 :   double max_speed_vertical          = _max_speed_vertical_;
+     780        6522 :   double max_acceleration_horizontal = _max_acceleration_horizontal_;
+     781        6522 :   double max_acceleration_vertical   = _max_acceleration_vertical_;
+     782        6522 :   double max_jerk                    = _max_jerk_;
+     783        6522 :   double max_u_vertical              = _max_u_vertical_;
+     784             : 
+     785        6522 :   if (tracker_command.use_full_state_prediction) {
+     786             : 
+     787        6506 :     max_speed_horizontal = 1.5 * (constraints.horizontal_speed);
+     788        6506 :     max_speed_vertical   = 1.5 * (constraints.vertical_ascending_speed < constraints.vertical_descending_speed ? constraints.vertical_ascending_speed
+     789             :                                                                                                              : constraints.vertical_descending_speed);
+     790             : 
+     791        6506 :     max_acceleration_horizontal = 1.5 * (constraints.horizontal_acceleration);
+     792        6506 :     max_acceleration_vertical =
+     793        6506 :         1.5 * (constraints.vertical_ascending_acceleration < constraints.vertical_descending_acceleration ? constraints.vertical_ascending_acceleration
+     794             :                                                                                                           : constraints.vertical_descending_acceleration);
+     795             : 
+     796        6506 :     max_jerk       = 1.5 * constraints.horizontal_jerk;
+     797        6506 :     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        6522 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     810        6522 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     811        6522 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     812             : 
+     813        6522 :   Rp << tracker_command.position.x, tracker_command.position.y, tracker_command.position.z;  // fill the desired position
+     814        6522 :   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        6522 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     821        6522 :   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        6522 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     825             : 
+     826             :   // Ow - UAV angular rate
+     827        6522 :   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        6522 :   Eigen::Vector3d Ep = Rp - Op;
+     836        6522 :   Eigen::Vector3d Ev = Rv - Ov;
+     837             : 
+     838             :   // | ------------------- initial conditions ------------------- |
+     839             : 
+     840       13044 :   Eigen::MatrixXd initial_x = Eigen::MatrixXd::Zero(3, 1);
+     841       13044 :   Eigen::MatrixXd initial_y = Eigen::MatrixXd::Zero(3, 1);
+     842       13044 :   Eigen::MatrixXd initial_z = Eigen::MatrixXd::Zero(3, 1);
+     843             : 
+     844             :   /* initial x //{ */
+     845             : 
+     846             :   {
+     847             :     double acceleration;
+     848             :     double velocity;
+     849        6522 :     double coef = 1.5;
+     850             : 
+     851        6522 :     if (fabs(uav_state.acceleration.linear.x) < coef * max_acceleration_horizontal) {
+     852        6522 :       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        6522 :     if (fabs(uav_state.velocity.linear.x) < coef * max_speed_horizontal) {
+     861        6522 :       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        6522 :     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        6522 :     double coef = 1.5;
+     880             : 
+     881        6522 :     if (fabs(uav_state.acceleration.linear.y) < coef * max_acceleration_horizontal) {
+     882        6522 :       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        6522 :     if (fabs(uav_state.velocity.linear.y) < coef * max_speed_horizontal) {
+     891        6522 :       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        6522 :     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        6522 :     double coef = 1.5;
+     910             : 
+     911        6522 :     if (fabs(uav_state.acceleration.linear.z) < coef * max_acceleration_horizontal) {
+     912        6522 :       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        6522 :     if (fabs(uav_state.velocity.linear.z) < coef * max_speed_vertical) {
+     921        6522 :       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        6522 :     initial_z << uav_state.pose.position.z, velocity, acceleration;
+     930             :   }
+     931             : 
+     932             :   //}
+     933             : 
+     934             :   // | ---------------------- set reference --------------------- |
+     935             : 
+     936       13044 :   Eigen::MatrixXd mpc_reference_x = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     937       13044 :   Eigen::MatrixXd mpc_reference_y = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     938       13044 :   Eigen::MatrixXd mpc_reference_z = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     939             : 
+     940             :   // prepare the full reference vector
+     941        6522 :   if (tracker_command.use_full_state_prediction) {
+     942             : 
+     943        6506 :     mpc_reference_x(0, 0) = tracker_command.position.x;
+     944        6506 :     mpc_reference_y(0, 0) = tracker_command.position.y;
+     945        6506 :     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      169156 :     for (int i = 1; i < _horizon_length_; i++) {
+     951      162650 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].x;
+     952      162650 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].y;
+     953      162650 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].z;
+     954             :     }
+     955             : 
+     956      169156 :     for (int i = 1; i < _horizon_length_; i++) {
+     957      162650 :       mpc_reference_x((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].x;
+     958      162650 :       mpc_reference_y((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].y;
+     959      162650 :       mpc_reference_z((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].z;
+     960             :     }
+     961             : 
+     962      169156 :     for (int i = 1; i < _horizon_length_; i++) {
+     963      162650 :       mpc_reference_x((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].x;
+     964      162650 :       mpc_reference_y((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].y;
+     965      162650 :       mpc_reference_z((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].z;
+     966             :     }
+     967             : 
+     968             :   } else {
+     969             : 
+     970         432 :     for (int i = 0; i < _horizon_length_; i++) {
+     971         416 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.position.x;
+     972         416 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.position.y;
+     973         416 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.position.z;
+     974             :     }
+     975             :   }
+     976             : 
+     977             :   // | ------------------ set the penalizations ----------------- |
+     978             : 
+     979       13044 :   std::vector<double> temp_Q_horizontal = _mat_Q_;
+     980       13044 :   std::vector<double> temp_Q_vertical   = _mat_Q_z_;
+     981             : 
+     982       13044 :   std::vector<double> temp_S_horizontal = _mat_S_;
+     983       13044 :   std::vector<double> temp_S_vertical   = _mat_S_z_;
+     984             : 
+     985        6522 :   if (!tracker_command.use_position_horizontal) {
+     986           0 :     temp_Q_horizontal[0] = 0;
+     987           0 :     temp_S_horizontal[0] = 0;
+     988             :   }
+     989             : 
+     990        6522 :   if (!tracker_command.use_velocity_horizontal) {
+     991           0 :     temp_Q_horizontal[1] = 0;
+     992           0 :     temp_S_horizontal[1] = 0;
+     993             :   }
+     994             : 
+     995        6522 :   if (!tracker_command.use_position_vertical) {
+     996           0 :     temp_Q_vertical[0] = 0;
+     997           0 :     temp_S_vertical[0] = 0;
+     998             :   }
+     999             : 
+    1000        6522 :   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        6522 :   mpc_solver_x_->setQ(temp_Q_horizontal);
+    1008        6522 :   mpc_solver_x_->setS(temp_S_horizontal);
+    1009        6522 :   mpc_solver_x_->setParams();
+    1010        6522 :   mpc_solver_x_->setLastInput(mpc_solver_x_u_);
+    1011        6522 :   mpc_solver_x_->loadReference(mpc_reference_x);
+    1012        6522 :   mpc_solver_x_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1013        6522 :   mpc_solver_x_->setInitialState(initial_x);
+    1014        6522 :   [[maybe_unused]] int iters_x = mpc_solver_x_->solveMPC();
+    1015        6522 :   mpc_solver_x_u_              = mpc_solver_x_->getFirstControlInput();
+    1016             : 
+    1017        6522 :   mpc_solver_y_->setQ(temp_Q_horizontal);
+    1018        6522 :   mpc_solver_y_->setS(temp_S_horizontal);
+    1019        6522 :   mpc_solver_y_->setParams();
+    1020        6522 :   mpc_solver_y_->setLastInput(mpc_solver_y_u_);
+    1021        6522 :   mpc_solver_y_->loadReference(mpc_reference_y);
+    1022        6522 :   mpc_solver_y_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1023        6522 :   mpc_solver_y_->setInitialState(initial_y);
+    1024        6522 :   [[maybe_unused]] int iters_y = mpc_solver_y_->solveMPC();
+    1025        6522 :   mpc_solver_y_u_              = mpc_solver_y_->getFirstControlInput();
+    1026             : 
+    1027        6522 :   mpc_solver_z_->setQ(temp_Q_vertical);
+    1028        6522 :   mpc_solver_z_->setS(temp_S_vertical);
+    1029        6522 :   mpc_solver_z_->setParams();
+    1030        6522 :   mpc_solver_z_->setLastInput(mpc_solver_z_u_);
+    1031        6522 :   mpc_solver_z_->loadReference(mpc_reference_z);
+    1032        6522 :   mpc_solver_z_->setLimits(max_speed_vertical, max_acceleration_vertical, max_u_vertical, 999.0, _dt1_, _dt2_);
+    1033        6522 :   mpc_solver_z_->setInitialState(initial_z);
+    1034        6522 :   [[maybe_unused]] int iters_z = mpc_solver_z_->solveMPC();
+    1035        6522 :   mpc_solver_z_u_              = mpc_solver_z_->getFirstControlInput();
+    1036             : 
+    1037             :   // | ----------- disable lateral feedback if needed ----------- |
+    1038             : 
+    1039        6522 :   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        6522 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+    1047             : 
+    1048        6522 :   Eigen::Array3d Kw(0, 0, 0);
+    1049        6522 :   Eigen::Array3d Kq;
+    1050             : 
+    1051             :   {
+    1052        6522 :     std::scoped_lock lock(mutex_gains_);
+    1053             : 
+    1054        6522 :     Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+    1055             : 
+    1056        6522 :     Kw[0] = gains.kw_rp;
+    1057        6522 :     Kw[1] = gains.kw_rp;
+    1058        6522 :     Kw[2] = gains.kw_y;
+    1059             :   }
+    1060             : 
+    1061             :   // | ---------- desired orientation matrix and force ---------- |
+    1062             : 
+    1063             :   // get body integral in the world frame
+    1064             : 
+    1065        6522 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+    1066             : 
+    1067             :   {
+    1068             : 
+    1069       13044 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+    1070             : 
+    1071        6522 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+    1072        6522 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+    1073        6522 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+    1074        6522 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+    1075        6522 :     Ib_b_stamped.vector.z        = 0;
+    1076             : 
+    1077       13044 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+    1078             : 
+    1079        6522 :     if (res) {
+    1080        6522 :       Ib_w[0] = res.value().vector.x;
+    1081        6522 :       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        6522 :   if (tracker_command.use_acceleration && !tracker_command.use_full_state_prediction) {
+    1090          15 :     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        6507 :     Ra << mpc_solver_x_u_, mpc_solver_y_u_, mpc_solver_z_u_;
+    1093             :   }
+    1094             : 
+    1095        6522 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+    1096             : 
+    1097        6522 :   Eigen::Vector3d feed_forward = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+    1098             : 
+    1099        6522 :   Eigen::Vector3d integral_feedback;
+    1100             :   {
+    1101        6522 :     std::scoped_lock lock(mutex_integrals_);
+    1102             : 
+    1103        6522 :     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       13044 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+    1118             : 
+    1119        6522 :     Eigen::Vector3d integration_switch(1, 1, 0);
+    1120             : 
+    1121             :     // integrate the world error
+    1122             : 
+    1123             :     // antiwindup
+    1124        6522 :     double temp_gain = gains.kiwxy;
+    1125        6522 :     if (!tracker_command.disable_antiwindups) {
+    1126        6522 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1127        4342 :         temp_gain = 0;
+    1128        4342 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for world integral kicks in", this->name_.c_str());
+    1129             :       }
+    1130             :     }
+    1131             : 
+    1132        6522 :     if (integral_terms_enabled_) {
+    1133        6522 :       if (tracker_command.use_position_horizontal) {
+    1134        6522 :         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        6522 :     bool world_integral_saturated = false;
+    1142        6522 :     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        6522 :     } else if (Iw_w_[0] > gains.kiwxy_lim) {
+    1146           0 :       Iw_w_[0]                 = gains.kiwxy_lim;
+    1147           0 :       world_integral_saturated = true;
+    1148        6522 :     } 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        6522 :     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        6522 :     world_integral_saturated = false;
+    1159        6522 :     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        6522 :     } else if (Iw_w_[1] > gains.kiwxy_lim) {
+    1163           0 :       Iw_w_[1]                 = gains.kiwxy_lim;
+    1164           0 :       world_integral_saturated = true;
+    1165        6522 :     } 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        6522 :     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       13044 :     std::scoped_lock lock(mutex_gains_);
+    1181             : 
+    1182        6522 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+    1183        6522 :     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       13044 :       geometry_msgs::Vector3Stamped Ep_stamped;
+    1189             : 
+    1190        6522 :       Ep_stamped.header.stamp    = ros::Time::now();
+    1191        6522 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+    1192        6522 :       Ep_stamped.vector.x        = Ep(0);
+    1193        6522 :       Ep_stamped.vector.y        = Ep(1);
+    1194        6522 :       Ep_stamped.vector.z        = Ep(2);
+    1195             : 
+    1196       19566 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+    1197             : 
+    1198        6522 :       if (res) {
+    1199        6522 :         Ep_fcu_untilted[0] = res.value().vector.x;
+    1200        6522 :         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       13044 :       geometry_msgs::Vector3Stamped Ev_stamped;
+    1209             : 
+    1210        6522 :       Ev_stamped.header.stamp    = ros::Time::now();
+    1211        6522 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+    1212        6522 :       Ev_stamped.vector.x        = Ev(0);
+    1213        6522 :       Ev_stamped.vector.y        = Ev(1);
+    1214        6522 :       Ev_stamped.vector.z        = Ev(2);
+    1215             : 
+    1216       19566 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+    1217             : 
+    1218        6522 :       if (res) {
+    1219        6522 :         Ev_fcu_untilted[0] = res.value().vector.x;
+    1220        6522 :         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        6522 :     double temp_gain = gains.kibxy;
+    1230        6522 :     if (!tracker_command.disable_antiwindups) {
+    1231        6522 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1232        4342 :         temp_gain = 0;
+    1233        4342 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for body integral kicks in", this->name_.c_str());
+    1234             :       }
+    1235             :     }
+    1236             : 
+    1237        6522 :     if (integral_terms_enabled_) {
+    1238        6522 :       if (tracker_command.use_position_horizontal) {
+    1239        6522 :         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        6522 :     bool body_integral_saturated = false;
+    1247        6522 :     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        6522 :     } else if (Ib_b_[0] > gains.kibxy_lim) {
+    1251           0 :       Ib_b_[0]                = gains.kibxy_lim;
+    1252           0 :       body_integral_saturated = true;
+    1253        6522 :     } 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        6522 :     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        6522 :     body_integral_saturated = false;
+    1264        6522 :     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        6522 :     } else if (Ib_b_[1] > gains.kibxy_lim) {
+    1268           0 :       Ib_b_[1]                = gains.kibxy_lim;
+    1269           0 :       body_integral_saturated = true;
+    1270        6522 :     } 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        6522 :     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        6522 :   Eigen::Vector3d des_acc = integral_feedback / total_mass + Ra;
+    1283             : 
+    1284        6522 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1285             : 
+    1286         806 :     if (output_modality == common::ACCELERATION_HDG) {
+    1287             : 
+    1288         806 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1289             : 
+    1290         403 :       cmd.acceleration.x = des_acc[0];
+    1291         403 :       cmd.acceleration.y = des_acc[1];
+    1292         403 :       cmd.acceleration.z = des_acc[2];
+    1293             : 
+    1294         403 :       cmd.heading = tracker_command.heading;
+    1295             : 
+    1296         403 :       last_control_output_.control_output = cmd;
+    1297             : 
+    1298             :     } else {
+    1299             : 
+    1300         403 :       double des_hdg_ff = 0;
+    1301             : 
+    1302         403 :       if (tracker_command.use_heading_rate) {
+    1303         403 :         des_hdg_ff = tracker_command.heading_rate;
+    1304             :       }
+    1305             : 
+    1306         806 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1307             : 
+    1308         403 :       cmd.acceleration.x = des_acc[0];
+    1309         403 :       cmd.acceleration.y = des_acc[1];
+    1310         403 :       cmd.acceleration.z = des_acc[2];
+    1311             : 
+    1312         403 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1313             : 
+    1314         403 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1315             : 
+    1316         403 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1317             : 
+    1318         403 :       cmd.heading_rate = des_hdg_rate;
+    1319             : 
+    1320         403 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1321             : 
+    1322         403 :       last_control_output_.control_output = cmd;
+    1323             :     }
+    1324             : 
+    1325             :     // | -------------- unbiased desired acceleration ------------- |
+    1326             : 
+    1327         806 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1328             : 
+    1329             :     {
+    1330             : 
+    1331        1612 :       geometry_msgs::Vector3Stamped world_accel;
+    1332             : 
+    1333         806 :       world_accel.header.stamp    = ros::Time::now();
+    1334         806 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1335         806 :       world_accel.vector.x        = Ra[0];
+    1336         806 :       world_accel.vector.y        = Ra[1];
+    1337         806 :       world_accel.vector.z        = Ra[2];
+    1338             : 
+    1339        2418 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1340             : 
+    1341         806 :       if (res) {
+    1342         806 :         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         806 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1348             : 
+    1349             :     // | ----------------- fill in the diagnostics ---------------- |
+    1350             : 
+    1351         806 :     last_control_output_.diagnostics.ramping_up = false;
+    1352             : 
+    1353         806 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1354         806 :     last_control_output_.diagnostics.mass_difference = 0;
+    1355         806 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1356             : 
+    1357         806 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1358             : 
+    1359         806 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1360         806 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1361             : 
+    1362         806 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1363         806 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1364             : 
+    1365         806 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1366         806 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1367             : 
+    1368         806 :     last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1369             : 
+    1370         806 :     last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * max_speed_horizontal;
+    1371         806 :     last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * max_acceleration_horizontal;
+    1372             : 
+    1373         806 :     last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * max_speed_vertical;
+    1374         806 :     last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1375             : 
+    1376         806 :     last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * max_speed_vertical;
+    1377         806 :     last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1378             : 
+    1379         806 :     last_control_output_.diagnostics.controller = name_;
+    1380             : 
+    1381         806 :     return;
+    1382             :   }
+    1383             : 
+    1384             :   /* mass estimatior //{ */
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                integrate the mass difference               |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390             :   {
+    1391       11432 :     std::scoped_lock lock(mutex_gains_);
+    1392             : 
+    1393             :     // antiwindup
+    1394        5716 :     double temp_gain = gains.km;
+    1395       11424 :     if (rampup_active_ ||
+    1396        5708 :         (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        2064 :       temp_gain = 0;
+    1398        2064 :       ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for the mass kicks in", this->name_.c_str());
+    1399             :     }
+    1400             : 
+    1401        5716 :     if (tracker_command.use_position_vertical) {
+    1402        5716 :       uav_mass_difference_ += temp_gain * Ep[2] * dt;
+    1403             :     }
+    1404             : 
+    1405             :     // saturate the mass estimator
+    1406        5716 :     bool uav_mass_saturated = false;
+    1407        5716 :     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        5716 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1411           0 :       uav_mass_difference_ = gains.km_lim;
+    1412           0 :       uav_mass_saturated   = true;
+    1413        5716 :     } 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        5716 :     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        5716 :   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        5716 :   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        5716 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1441             : 
+    1442        5716 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "MpcController");
+    1443             : 
+    1444        5716 :   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        5716 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1458             : 
+    1459             :   // --------------------------------------------------------------
+    1460             :   // |               desired orientation + throttle               |
+    1461             :   // --------------------------------------------------------------
+    1462             : 
+    1463             :   // | ------------------- desired orientation ------------------ |
+    1464             : 
+    1465        5716 :   Eigen::Matrix3d Rd;
+    1466             : 
+    1467        5716 :   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        5716 :     Eigen::Vector3d bxd;  // desired heading vector
+    1484             : 
+    1485        5716 :     if (tracker_command.use_heading) {
+    1486        5716 :       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        5716 :     Rd = common::so3transform(f_normed, bxd, false);
+    1493             :   }
+    1494             : 
+    1495             :   // | -------------------- desired throttle -------------------- |
+    1496             : 
+    1497        5716 :   double desired_thrust_force = f.dot(R.col(2));
+    1498        5716 :   double throttle             = 0;
+    1499             : 
+    1500        5716 :   if (rampup_active_) {
+    1501             : 
+    1502             :     // deactivate the rampup when the times up
+    1503           8 :     if (fabs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1504             : 
+    1505           8 :       rampup_active_ = false;
+    1506             : 
+    1507           8 :       ROS_INFO("[%s]: rampup finished", name_.c_str());
+    1508             : 
+    1509             :     } else {
+    1510             : 
+    1511           0 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1512             : 
+    1513           0 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1514             : 
+    1515           0 :       rampup_last_time_ = ros::Time::now();
+    1516             : 
+    1517           0 :       throttle = rampup_throttle_;
+    1518             : 
+    1519           0 :       ROS_INFO_THROTTLE(0.1, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
+    1520             :     }
+    1521             : 
+    1522             :   } else {
+    1523             : 
+    1524        5708 :     if (desired_thrust_force >= 0) {
+    1525        5708 :       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        5716 :   bool throttle_saturated = false;
+    1534             : 
+    1535        5716 :   if (!std::isfinite(throttle)) {
+    1536             : 
+    1537           0 :     ROS_ERROR("[%s]: NaN detected in variable 'throttle'!!!", name_.c_str());
+    1538           0 :     return;
+    1539             : 
+    1540        5716 :   } 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        5716 :   } 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        5716 :   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        5716 :   double desired_x_accel = 0;
+    1569        5716 :   double desired_y_accel = 0;
+    1570        5716 :   double desired_z_accel = 0;
+    1571             : 
+    1572             :   {
+    1573        5716 :     Eigen::Vector3d thrust_vector = desired_thrust_force * Rd.col(2);
+    1574             : 
+    1575        5716 :     double world_accel_x = (thrust_vector[0] / total_mass) - (Iw_w_[0] / total_mass) - (Ib_w[0] / total_mass);
+    1576        5716 :     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        5716 :     double world_accel_z = tracker_command.acceleration.z;
+    1580             : 
+    1581       11432 :     geometry_msgs::Vector3Stamped world_accel;
+    1582             : 
+    1583        5716 :     world_accel.header.stamp    = ros::Time::now();
+    1584        5716 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1585        5716 :     world_accel.vector.x        = world_accel_x;
+    1586        5716 :     world_accel.vector.y        = world_accel_y;
+    1587        5716 :     world_accel.vector.z        = world_accel_z;
+    1588             : 
+    1589       17148 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1590             : 
+    1591        5716 :     if (res) {
+    1592             : 
+    1593        5716 :       desired_x_accel = res.value().vector.x;
+    1594        5716 :       desired_y_accel = res.value().vector.y;
+    1595        5716 :       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        5716 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1603             : 
+    1604             :   // fill the unbiased desired accelerations
+    1605        5716 :   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        5716 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1610             : 
+    1611        5716 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1612        5716 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1613        5716 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1614             : 
+    1615        5716 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1616             : 
+    1617        5716 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1618        5716 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1619             : 
+    1620        5716 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1621        5716 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1622             : 
+    1623        5716 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1624        5716 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1625             : 
+    1626        5716 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1627             : 
+    1628        5716 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1629        5716 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1630             : 
+    1631        5716 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1632        5716 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1633             : 
+    1634        5716 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1635        5716 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1636             : 
+    1637        5716 :   last_control_output_.diagnostics.controller = name_;
+    1638             : 
+    1639             :   // | ------------ construct the attitude reference ------------ |
+    1640             : 
+    1641        5716 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1642             : 
+    1643        5716 :   attitude_cmd.stamp       = ros::Time::now();
+    1644        5716 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1645        5716 :   attitude_cmd.throttle    = throttle;
+    1646             : 
+    1647        5716 :   if (output_modality == common::ATTITUDE) {
+    1648             : 
+    1649         836 :     last_control_output_.control_output = attitude_cmd;
+    1650             : 
+    1651         836 :     return;
+    1652             :   }
+    1653             : 
+    1654             :   // --------------------------------------------------------------
+    1655             :   // |                      attitude control                      |
+    1656             :   // --------------------------------------------------------------
+    1657             : 
+    1658        4880 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1659             : 
+    1660        4880 :   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        4880 :   } else if (tracker_command.use_heading_rate) {
+    1665             : 
+    1666             :     // to fill in the feed forward yaw rate
+    1667        4879 :     double desired_yaw_rate = 0;
+    1668             : 
+    1669             :     try {
+    1670        4879 :       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        4879 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1677             :   }
+    1678             : 
+    1679             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1680             : 
+    1681        4880 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1682             : 
+    1683        4880 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1684             : 
+    1685        4868 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: using jerk feedforward", name_.c_str());
+    1686             : 
+    1687        4868 :     Eigen::Matrix3d I;
+    1688        4868 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1689        4868 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1690        4868 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1691             :   }
+    1692             : 
+    1693             :   // | --------------- run the attitude controller -------------- |
+    1694             : 
+    1695        4880 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1696             : 
+    1697        4880 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq, false);
+    1698             : 
+    1699        4880 :   if (!attitude_rate_command) {
+    1700           0 :     return;
+    1701             :   }
+    1702             : 
+    1703             :   // | --------- fill in the already known attitude rate -------- |
+    1704             : 
+    1705             :   {
+    1706             :     try {
+    1707        4880 :       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        4880 :   if (output_modality == common::ATTITUDE_RATE) {
+    1716             : 
+    1717         962 :     last_control_output_.control_output = attitude_rate_command;
+    1718             : 
+    1719         962 :     return;
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                    Attitude rate control                   |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726        3918 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1727             : 
+    1728        3918 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1729             : 
+    1730        3918 :   if (!control_group_command) {
+    1731           0 :     return;
+    1732             :   }
+    1733             : 
+    1734        3918 :   if (output_modality == common::CONTROL_GROUP) {
+    1735             : 
+    1736        1913 :     last_control_output_.control_output = control_group_command;
+    1737             : 
+    1738        1913 :     return;
+    1739             :   }
+    1740             : 
+    1741             :   // --------------------------------------------------------------
+    1742             :   // |                        output mixer                        |
+    1743             :   // --------------------------------------------------------------
+    1744             : 
+    1745        4010 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1746             : 
+    1747        2005 :   last_control_output_.control_output = actuator_cmd;
+    1748             : 
+    1749        2005 :   return;
+    1750             : }
+    1751             : 
+    1752             : //}
+    1753             : 
+    1754             : /* positionPassthrough() //{ */
+    1755             : 
+    1756         180 : void MpcController::positionPassthrough(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    1757             : 
+    1758         180 :   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         360 :   mrs_msgs::HwApiPositionCmd cmd;
+    1764             : 
+    1765         180 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1766         180 :   cmd.header.stamp    = ros::Time::now();
+    1767             : 
+    1768         180 :   cmd.position = tracker_command.position;
+    1769         180 :   cmd.heading  = tracker_command.heading;
+    1770             : 
+    1771         180 :   last_control_output_.control_output = cmd;
+    1772             : 
+    1773             :   // fill the unbiased desired accelerations
+    1774         180 :   last_control_output_.desired_unbiased_acceleration = {};
+    1775         180 :   last_control_output_.desired_orientation           = {};
+    1776         180 :   last_control_output_.desired_heading_rate          = {};
+    1777             : 
+    1778             :   // | ----------------- fill in the diagnostics ---------------- |
+    1779             : 
+    1780         180 :   last_control_output_.diagnostics.ramping_up = false;
+    1781             : 
+    1782         180 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1783         180 :   last_control_output_.diagnostics.mass_difference = 0;
+    1784             : 
+    1785         180 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1786             : 
+    1787         180 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1788         180 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1789             : 
+    1790         180 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1791         180 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1792             : 
+    1793         180 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1794         180 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1795             : 
+    1796         180 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1797             : 
+    1798         180 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1799         180 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1800             : 
+    1801         180 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1802         180 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1803             : 
+    1804         180 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1805         180 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1806             : 
+    1807         180 :   last_control_output_.diagnostics.controller = name_;
+    1808             : }
+    1809             : 
+    1810             : //}
+    1811             : 
+    1812             : /* PIDVelocityOutput() //{ */
+    1813             : 
+    1814         401 : 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         401 :   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         401 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1823             : 
+    1824         401 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1825         401 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1826             : 
+    1827         401 :   double hdg_ref = tracker_command.heading;
+    1828         401 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1829             : 
+    1830             :   // | ------------------ velocity feedforward ------------------ |
+    1831             : 
+    1832         401 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1833             : 
+    1834         401 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1835         401 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1836             :   }
+    1837             : 
+    1838             :   // | --------------------- control errors --------------------- |
+    1839             : 
+    1840         401 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1841             : 
+    1842             :   // | --------------------------- pid -------------------------- |
+    1843             : 
+    1844         401 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1845         401 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1846         401 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1847             : 
+    1848         401 :   double des_vel_x = position_pid_x_.update(Ep[0], dt);
+    1849         401 :   double des_vel_y = position_pid_y_.update(Ep[1], dt);
+    1850         401 :   double des_vel_z = position_pid_z_.update(Ep[2], dt);
+    1851             : 
+    1852             :   // | -------------------- position feedback ------------------- |
+    1853             : 
+    1854         401 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1855             : 
+    1856         401 :   if (control_output == common::VELOCITY_HDG) {
+    1857             : 
+    1858             :     // | --------------------- fill the output -------------------- |
+    1859             : 
+    1860         404 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1861             : 
+    1862         202 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1863         202 :     cmd.header.stamp    = ros::Time::now();
+    1864             : 
+    1865         202 :     cmd.velocity.x = des_vel[0];
+    1866         202 :     cmd.velocity.y = des_vel[1];
+    1867         202 :     cmd.velocity.z = des_vel[2];
+    1868             : 
+    1869         202 :     cmd.heading = tracker_command.heading;
+    1870             : 
+    1871         202 :     last_control_output_.control_output = cmd;
+    1872             : 
+    1873         199 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1874             : 
+    1875         199 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1876             : 
+    1877         199 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1878             : 
+    1879         199 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1880             : 
+    1881             :     // | --------------------------- ff --------------------------- |
+    1882             : 
+    1883         199 :     double des_hdg_ff = 0;
+    1884             : 
+    1885         199 :     if (tracker_command.use_heading_rate) {
+    1886         199 :       des_hdg_ff = tracker_command.heading_rate;
+    1887             :     }
+    1888             : 
+    1889             :     // | --------------------- fill the output -------------------- |
+    1890             : 
+    1891         398 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1892             : 
+    1893         199 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1894         199 :     cmd.header.stamp    = ros::Time::now();
+    1895             : 
+    1896         199 :     cmd.velocity.x = des_vel[0];
+    1897         199 :     cmd.velocity.y = des_vel[1];
+    1898         199 :     cmd.velocity.z = des_vel[2];
+    1899             : 
+    1900         199 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1901             : 
+    1902         199 :     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         401 :   last_control_output_.desired_unbiased_acceleration = {};
+    1911         401 :   last_control_output_.desired_orientation           = {};
+    1912         401 :   last_control_output_.desired_heading_rate          = {};
+    1913             : 
+    1914             :   // | ----------------- fill in the diagnostics ---------------- |
+    1915             : 
+    1916         401 :   last_control_output_.diagnostics.ramping_up = false;
+    1917             : 
+    1918         401 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1919         401 :   last_control_output_.diagnostics.mass_difference = 0;
+    1920             : 
+    1921         401 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1922             : 
+    1923         401 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1924         401 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1925             : 
+    1926         401 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1927         401 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1928             : 
+    1929         401 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1930         401 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1931             : 
+    1932         401 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1933             : 
+    1934         401 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1935         401 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1936             : 
+    1937         401 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1938         401 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1939             : 
+    1940         401 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1941         401 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1942             : 
+    1943         401 :   last_control_output_.diagnostics.controller = name_;
+    1944             : }
+    1945             : 
+    1946             : //}
+    1947             : 
+    1948             : // --------------------------------------------------------------
+    1949             : // |                          callbacks                         |
+    1950             : // --------------------------------------------------------------
+    1951             : 
+    1952             : /* //{ callbackDrs() */
+    1953             : 
+    1954          52 : void MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig &config, [[maybe_unused]] uint32_t level) {
+    1955             : 
+    1956          52 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1957             : 
+    1958          52 :   ROS_INFO("[%s]: DRS updated gains", this->name_.c_str());
+    1959          52 : }
+    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        2129 : void MpcController::timerGains(const ros::TimerEvent &event) {
+    1993             : 
+    1994        6387 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1995             :   mrs_lib::ScopeTimer timer =
+    1996        6387 :       mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1997             : 
+    1998        4258 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1999        2129 :   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        2129 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    2004        2129 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    2005             : 
+    2006        2129 :   mute_gains_ = false;
+    2007             : 
+    2008        2129 :   const double dt = (event.current_real - event.last_real).toSec();
+    2009             : 
+    2010        2129 :   bool updated = false;
+    2011             : 
+    2012        2129 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    2013        2129 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    2014        2129 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    2015        2129 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    2016        2129 :   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        2129 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    2020        2129 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    2021        2129 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    2022             : 
+    2023        2129 :   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        2129 :   if (updated) {
+    2028             : 
+    2029         387 :     drs_params.kiwxy         = gains.kiwxy;
+    2030         387 :     drs_params.kibxy         = gains.kibxy;
+    2031         387 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    2032         387 :     drs_params.kq_yaw        = gains.kq_yaw;
+    2033         387 :     drs_params.km            = gains.km;
+    2034         387 :     drs_params.km_lim        = gains.km_lim;
+    2035         387 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    2036         387 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    2037             : 
+    2038         387 :     drs_->updateConfig(drs_params);
+    2039             : 
+    2040         387 :     ROS_INFO_THROTTLE(10.0, "[%s]: gains have been updated", name_.c_str());
+    2041             :   }
+    2042        2129 : }
+    2043             : 
+    2044             : //}
+    2045             : 
+    2046             : // --------------------------------------------------------------
+    2047             : // |                       other routines                       |
+    2048             : // --------------------------------------------------------------
+    2049             : 
+    2050             : /* calculateGainChange() //{ */
+    2051             : 
+    2052       17032 : 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       17032 :   double change = desired_value - current_value;
+    2056             : 
+    2057       17032 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    2058       17032 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    2059             : 
+    2060       17032 :   if (!bypass_rate) {
+    2061             : 
+    2062             :     // if current value is near 0...
+    2063             :     double change_in_perc;
+    2064             :     double saturated_change;
+    2065             : 
+    2066       16817 :     if (fabs(current_value) < 1e-6) {
+    2067           0 :       change *= gains_filter_max_change;
+    2068             :     } else {
+    2069             : 
+    2070       16817 :       saturated_change = change;
+    2071             : 
+    2072       16817 :       change_in_perc = (current_value + saturated_change) / current_value - 1.0;
+    2073             : 
+    2074       16817 :       if (change_in_perc > gains_filter_max_change) {
+    2075        1505 :         saturated_change = current_value * gains_filter_max_change;
+    2076       15312 :       } else if (change_in_perc < -gains_filter_max_change) {
+    2077           0 :         saturated_change = current_value * -gains_filter_max_change;
+    2078             :       }
+    2079             : 
+    2080       16817 :       if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
+    2081           0 :         change *= gains_filter_min_change;
+    2082             :       } else {
+    2083       16817 :         change = saturated_change;
+    2084             :       }
+    2085             :     }
+    2086             :   }
+    2087             : 
+    2088       17032 :   if (fabs(change) > 1e-3) {
+    2089        1935 :     ROS_DEBUG("[%s]: changing gain '%s' from %.2f to %.2f", name_.c_str(), name.c_str(), current_value, desired_value);
+    2090        1935 :     updated = true;
+    2091             :   }
+    2092             : 
+    2093       17032 :   return current_value + change;
+    2094             : }
+    2095             : 
+    2096             : //}
+    2097             : 
+    2098             : /* getHeadingSafely() //{ */
+    2099             : 
+    2100        6923 : double MpcController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    2101             : 
+    2102             :   try {
+    2103        6923 :     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          26 : 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..4d840357449e667d2e3436617f60ffaad963ac05 GIT binary patch literal 6660 zcmV+f8vEsmP)e90{{R30vG`D0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpDB(>JCI&$C=*~6)Y|o|1?5#LjA7)QW9Z=t; zTM7sKH1Fu$qb%=(08X=8MR`xx^!Pv+Ge`m$FbZ9}mj|HKbHGRd8Ga=NFuQIgvN1-H z=8QcD7&}v7NJYT8ho-|l_g7NWPK)Pc7delMb)GXN-KLE6x;^zbr{DP#9HA~X(;$O5 z;sLbFfbuEah7pW!3O^f+P##zOX!b8m7$N5Z#xsIRBp@HG895_UC&uS6&J<(LQ_Dd0 zV@hmk8#;$E40)Q0g9cBJGN7UBtM&nF!B}jb*L6ViQkW+6?HNl+gEA#DRRP>zbW=fz znCS|1&IvOWPC@>M8NFv$*G#`G0bXbq=#TghGkISh{549CV3o9SA4|^!ezr3lsXam0 zH$60q2r$kRT>`wMN5>3* zo5@XG!zI#+24aM(aPra$x;P=YX};q_J8cduc&~!qAtw8YF^b>K0MFaFMD3C7;R&@j z1EF9o$()0NZ5pZUMt7h(`Y7h~sA;A8LS{z5aqYimMy|rZAz&4@3&mU?W-b!vZ3>T} zCOVlsPOOj1(sklvaMoRW@bOC30Ou|AFZfW2M==S2rzj^nJeeKsR8QXDSq+^*rPHN3 zm8d}I7&=ejXWZcJw6#|RB;Tm{A?sb|#PspR9dqIp|1r?8eU<=&(LaaB-4y`>qI*vvvdfyO zPP@XQ_yTD;LjmqO@}?*bSLn3$7)J!Ui)~JT@jGDWp2jnqFRU|9|{E|NVC~|Lf;@-PAde{9oxo<-Zhy#2mG2!9%&SRW_+`ue|MuuPFw7vVx5I@x$j*)EK-g1oB zs(8P4i~!?qH)VFz76J7bi54Yjz^#DmrRnY4+303+jbj)3m%CV;hIbfc2}_69&@ix}eyT={rG{+y{{ z+_}xl<2PpK&f0sU^TCbkA^hcmniBefa$dA02Oh z+dC6h-K`%V4~&qHB@Q}fSpb#nd3o3Mc|0)wI9YfE;UEHW?D?D$aGs&Ma=~^F{Sm%D zq)bf6i-!kTvXtosD<5&YVs~CuX##?~qHee5Cl}CCrHxk`##_>)8FCXwppLgJ*8>2; zoS`Dd4Rdf^H+HXZPnz`kEG&tCfV=D~7MEp=I`*jBf(@Vbc(I1rTH|ZuT<^_!J`{lP z(OL%Fw19b@)(rz%i6I-1_$~q}Z7##iz`lxObgZecyV36Hw0Ff)R`OHWFjh!5WBhaG z_0WXV4h;AFUIoVt1dJ+00a*3p+QukpoHH1!J59Bisk=abH4zEjHKfwst zu36EjcFiOL@+2y)G*Kpwx2AzNbnw2#VsOoxzw~&U@bi^cS03vdG zCt^eets-Xs^sR^~2yR9NrUL=K2Qk9jD2Pd0skb0TrvNx&GQigpQ!r_&GqYj9FEL+Y zK=bkI`h)#`Kq@SNyV6z|5JV<6h0LChiYs)xIVF3H!Z$#So6~lvV4uZU$lGa41nT4P z)lcEj3M_pB0sP6Z^dZDmS6DutsQaIY1%Rwj(@L<2L_C)l&-G5X!GcVL9`r=Z58 zdFJlyuyT!Q=VL@9>`W;j-pqj!GPsloOrc&WfI(nE{jBukv)ES9^<3x zL$UxWyWa0&D&E0nZk(Uu5f3jpo$;_u;V_2R&q}EQwZ0T3oPn6(CBZs8lY|G-4_%rK z+N>-SHjG%w3R9&A{HVF^n1UcDJwodgCC`ap*=(iNl;HyEv!^W?NWYVNz2x2nv)Fb_i=~FdN={U2 zh#}?kV?0<#8JfDzeE=)#Ct@@OouW+uq;t6uj7hcdUJZzGLx2SK{6U+nTB9B#O8d;^ zoC91qz`$68^r(d*J8R3Pe=P)1HUeA$1~DNmkv5E&Wnq|z#+!)YxgAGu7~_yLKBh<4 z6=<^H4n6)b^wBk^yq3wQy8I5Hwk0F3Sny&Q0jQOpg=Q)U?+k!y!43Cg%;}Z^S9rA5 zx0j}{HhWsYg(Wuk&r5*x{c2-RsA^JNASoHAR1##fSpf4K>gSaBEm2J$XvRZ+7Tt5S zq<+$8Lv;@RE#eyfM8=!&e>v1`8l4c=;RtMXiO|uK`zH}@b zVdNuN?mMQ*)-{gdl-=u+auur8`OOR`?B@A+p5ckzj|#(|z-2dt;WwNP8ubfe>42>G zQNE_HHWLBA+U%>%zS``o%@loqwVB-YSDOj!j#ae2+N=-5|CBZ(xqxVUeZDXs@9|#O z+$YrmP}yRQ^C{6Q5i>CMu@XWASA0i6Eqh$$`{UmfT%hryiR6U|fc}!?Yd8>iLP+-z z$v&AYa2Img2%Yg+Lxhn!o^S9u(-CmMS%5Fq{}ZbF=RbDl;~+pSA4kLsC-{I17XqDs z#DU1dhB7W9!!y8*t| zGb3C=-3Is$Gn08EK4NBIMWFZ(T%g?NKBV&FYTsetb+Z_o&a~gUv6FbFH01 za5#M}-7*gVYZ$d%tM>|^Y*}F;Sl!&PN>P$DyliZap|F4J4p^xQJit+j@3jx!p^$~t zb{ngA*G(}6&x`~%;8sj4fsx=>=DN~CHblA_#z@kYh3xPQ5b&skF(uDex;6sJg?{u@ z^ZDbLxz;#WJxbzTB!#QK8UU>UF&IsWCu8^5u8QC*i)~!;B*l|`R0_()Ms+xr@(lu7 zidn`lW|$!0kYbkZe9?vpx#1<&c*rjx1mYR%#I<6)NzX4md$(?#-GUxX}CQO@V2B>hu)DDFp%mKh9)cQ4f2Qz@#QwOF}in6)|iSDa}Ymz!-Xn={s~Jq-(isG~YEty4W?vtM6KGx@sGk!s8r^ zG@WFOk64b|Wh4e{}^T(Jdr{l zOSDrgfXc4pC5zF$H-pQo(6sfpdsu(iDR&q$_Db$B+_ij91po|Qu1YQBj(`TLYyL2! zJ51*f!{)W*q_6_2{4?REQ~X3i{xGKqUv80hU0lm|%~SvlRDajZyRMmQ%;)Zc{{Q08 z*xZZ#)t`50xEB=T{e(lqy`U2Q`lWE}3yVp{)S*$(j~}-$EEY{hvnv(SY~B}&xBlq6 z9=k8xHsZ%(h@tyJP((N0H8j$pQ6mNp|0c(fzJmJ|T;kFATMiAlvxk#SpL8uL{)F4a zB(bJRFeu4-Xx)7T2JnD(jD@=*1_9c}h!6Pnz=#;haMI!!>V33nN%nQo%r)=1DA_zl zIDZn)EmpIM>bi{oO$nFV76C07^OR=VS}@X;7$#{m>&N68_Jb4w%^1kPd@JEO5?&9vmIhXh%Lj3oh1eK z;KXHdjI@|{hcSk^>4L|ox0u{06&>(jJY4iP}{o(xB$Rj#CVidz$F3p>e`oZ zuLADp=lH_P`L~W@Reryu7mb7NU}?Fol?jRZtn1n z3=sK9D_18{XKHk#!YizxuNjOp@11td`?zV>cQJJiySZ`K)UC*N&CsszdZ?RQ4pU^r zgqeBzIZ5G}#~*cbXFl%c`eyLdIj(s}PN#-_$&^l}hwp$dSSa?VS6wZ&IVAYFn;ybo z_86GkXRgId4{ z*HG6NoDdB{@qEVI9;oD>DZeLul4Qn5)d+S?4w`*TSMT z?iIpQC{jGd$3{?uyF3;zwGL8fy1|$rvA^=Urwg!?%4aIaH`R*A*}L(o^hd~6v|~)y ztqaZq6Grj4U^7Pi;Ce!X-2Guhj^>C565P;ilHv~%UWw#7w_?lV!*R}+!%173$E#BY z^G)t)8&~61Y`1Ym{oLZG$kS`V@4I2orEC{2dG^M+kn@W##H84KwSsi&v|Mr5vIM8D z>o#t&>oE4wHGE4?Y+;Ovso^&Z7&5ToHXqJ4iz>W(M~oC_0i)McjLS~Mwm*D+k>S+4 z(t$nLTHwdX6Eo2N*eNd!nb%UmJEutd11eiXlr%keG)9_G*(X5(Y$}Wkoo6SYumrU*oXJ7tOyedeU{;v}x&c~z zd*xx;_JuKvD_$tdF&ys13puo=u5}7)$(?A0o5 zamU78=a`@|!*}N|)J3;dtR0psT@`BrRAXeazT;Iy|Gx~r$! zJC)7-rfToI6w0k_yLY)}&gk*my(FUiafCD88Fs^N^n7S0=c1fJ!xRpwGag%?GqhQ% ztY49MQf=CdwSF_P$g*)B@plTbKYv1a|FtI|0yEC%ASH}q+>-#xk<_-X1)od?xFNtU zT|;~)ORMz>h?U0}*rNg5I6wn2zZYU_WVo_sNioFM>U9;T5NA5~i6J!te!Fh!8erbr zH*$8a>vxEpeV8THsTc`sN#%wV%W&*O3#BpC?XQ_!Fr!XjXp8Q_)ogRL_?#vW*q|v^ zN(-|-JRpqCrtTcQ0Z`jDv?;&|tuDt{ZzMoKiP5u%pJV!|k8|;UmlXkyJ?<84%{hH? zhL5Y4jkA|f<1t?2wXNsJC^R^QeE{QZ+80+|Q-Ox}`vW%3Ok9R@hnZot891M}n)n!V zxl!Gl98gU)T{Ph^WHZ@Jy0Sownrh`v90l!4JMo6DOZAOM0ouk`s|tSJJ$nWtOuQgZ z4RkHxnK6$9Yhwu?q4sp=iw*LapLdOwNcK-cs?_tv*$-bouH3$U+%}B= zPiN}akK42BuOC;5QLpQVM#Q-$WJz4m-}W}No?O7$sp_>5wI>sLS}3+#^tYZkrnN;p z{7F&0n5nxSB4*g3R=!G5(P+&9ro|#-I+F`j(JNYMmryTBlV`8(8jSiWkDvkLvr3E_ zW(THQ2 zlLXETSmvTJ;gq)3qoRNY_P90eSF~TY41e6gOm|o_9Sq_4=)#J1V$@Wlqhvt`^Z`7E zn&wQd-?Jr*d@?6QARiL9V|0IxW8^}WYCv7f{kYPt-)$J-(Z-H z>q~6GPk)ZunQ*c>eodbIGRpyA7vrAX<0DcZY9XV-2ExAZDV5o8+{G5m z@1i$frw4Lf3PFf+_5#A9947^~uQ zj8P*#fsciXGk}k(DbyeH?f&G7T-?bi-;oo!db_y?y8p2zBXu@@Uy(Dx%lmrh6oRp3Uqady+L@(+R+WiYZvO%CoffNmkw%RG O0000 + + + + + + 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:60277477.8 %
Date:2023-11-30 22:31:42Functions:141782.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()0
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&)9
(anonymous namespace)::ProxyExec0::ProxyExec0()26
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>)26
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)52
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)104
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)181
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&)363
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()393
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)797
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&)6124
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)6487
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)6668
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)11031
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)11158
+
+
+ + + +
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..2bc0f6fc52 --- /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:60277477.8 %
Date:2023-11-30 22:31:42Functions:141782.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()26
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()0
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>)26
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)797
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)52
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)6668
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&)6124
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)104
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)11031
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)6487
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&)363
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)11158
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)181
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&)9
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()393
+
+
+ + + +
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..d39b29f032 --- /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:60277477.8 %
Date:2023-11-30 22:31:42Functions:141782.4 %
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          26 : 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          26 :   nh_ = nh;
+     217             : 
+     218          26 :   common_handlers_  = common_handlers;
+     219          26 :   private_handlers_ = private_handlers;
+     220             : 
+     221          26 :   _uav_mass_ = common_handlers->getMass();
+     222             : 
+     223          26 :   ros::Time::waitForValid();
+     224             : 
+     225             :   // | ---------- loading params using the parent's nh ---------- |
+     226             : 
+     227          52 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     228             : 
+     229          26 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     230             : 
+     231          26 :   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          26 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/se3_controller.yaml");
+     239          26 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/se3_controller.yaml");
+     240             : 
+     241          52 :   const std::string yaml_namespace = "mrs_uav_controllers/se3_controller/";
+     242             : 
+     243             :   // lateral gains
+     244          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kp", gains_.kpxy);
+     245          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kv", gains_.kvxy);
+     246          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/ka", gains_.kaxy);
+     247             : 
+     248          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw", gains_.kiwxy);
+     249          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib", gains_.kibxy);
+     250             : 
+     251             :   // | ------------------------- rampup ------------------------- |
+     252             : 
+     253          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/enabled", _rampup_enabled_);
+     254          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/speed", _rampup_speed_);
+     255             : 
+     256             :   // height gains
+     257          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kp", gains_.kpz);
+     258          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kv", gains_.kvz);
+     259          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/ka", gains_.kaz);
+     260             : 
+     261             :   // attitude gains
+     262          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_roll_pitch", gains_.kq_roll_pitch);
+     263          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_yaw", gains_.kq_yaw);
+     264             : 
+     265             :   // attitude rate gains
+     266          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_roll_pitch", gains_.kw_roll_pitch);
+     267          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_yaw", gains_.kw_yaw);
+     268             : 
+     269             :   // mass estimator
+     270          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km", gains_.km);
+     271          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km_lim", gains_.km_lim);
+     272             : 
+     273             :   // integrator limits
+     274          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw_lim", gains_.kiwxy_lim);
+     275          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib_lim", gains_.kibxy_lim);
+     276             : 
+     277             :   // constraints
+     278          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     279          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     280             : 
+     281          26 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     282             : 
+     283          26 :   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          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/throttle_saturation", _throttle_saturation_);
+     289             : 
+     290             :   // gain filtering
+     291          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     292          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     293          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/rate", _gain_filtering_rate_);
+     294          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     295             : 
+     296             :   // output mode
+     297          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/preferred_output", drs_params_.preferred_output_mode);
+     298             : 
+     299          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rotation_matrix", drs_params_.rotation_type);
+     300             : 
+     301             :   // angular rate feed forward
+     302          52 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/parasitic_pitch_roll",
+     303          26 :                                             drs_params_.pitch_roll_heading_rate_compensation);
+     304          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     305             : 
+     306             :   // | ------------------- position pid params ------------------ |
+     307             : 
+     308          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     309          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     310          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     311             : 
+     312          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     313          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     314          26 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     315             : 
+     316             :   // | ------------------ finish loading params ----------------- |
+     317             : 
+     318          26 :   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          26 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     326          26 :         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          26 :   uav_mass_difference_ = 0;
+     333          26 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     334          26 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     335             : 
+     336             :   // | --------------- dynamic reconfigure server --------------- |
+     337             : 
+     338          26 :   drs_params_.kpxy             = gains_.kpxy;
+     339          26 :   drs_params_.kvxy             = gains_.kvxy;
+     340          26 :   drs_params_.kaxy             = gains_.kaxy;
+     341          26 :   drs_params_.kiwxy            = gains_.kiwxy;
+     342          26 :   drs_params_.kibxy            = gains_.kibxy;
+     343          26 :   drs_params_.kpz              = gains_.kpz;
+     344          26 :   drs_params_.kvz              = gains_.kvz;
+     345          26 :   drs_params_.kaz              = gains_.kaz;
+     346          26 :   drs_params_.kq_roll_pitch    = gains_.kq_roll_pitch;
+     347          26 :   drs_params_.kq_yaw           = gains_.kq_yaw;
+     348          26 :   drs_params_.kiwxy_lim        = gains_.kiwxy_lim;
+     349          26 :   drs_params_.kibxy_lim        = gains_.kibxy_lim;
+     350          26 :   drs_params_.km               = gains_.km;
+     351          26 :   drs_params_.km_lim           = gains_.km_lim;
+     352          26 :   drs_params_.jerk_feedforward = true;
+     353             : 
+     354          26 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     355          26 :   drs_->updateConfig(drs_params_);
+     356          26 :   Drs_t::CallbackType f = boost::bind(&Se3Controller::callbackDrs, this, _1, _2);
+     357          26 :   drs_->setCallback(f);
+     358             : 
+     359             :   // | ------------------------- timers ------------------------- |
+     360             : 
+     361          26 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &Se3Controller::timerGains, this, false, false);
+     362             : 
+     363             :   // | ---------------------- position pid ---------------------- |
+     364             : 
+     365          26 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     366          26 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     367          26 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     368          26 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     369             : 
+     370             :   // | ------------------------ profiler ------------------------ |
+     371             : 
+     372          26 :   profiler_ = mrs_lib::Profiler(common_handlers_->parent_nh, "Se3Controller", _profiler_enabled_);
+     373             : 
+     374             :   // | ----------------------- finish init ---------------------- |
+     375             : 
+     376          26 :   ROS_INFO("[Se3Controller]: initialized");
+     377             : 
+     378          26 :   is_initialized_ = true;
+     379             : 
+     380          26 :   return true;
+     381             : }
+     382             : 
+     383             : //}
+     384             : 
+     385             : /* //{ activate() */
+     386             : 
+     387           9 : bool Se3Controller::activate(const ControlOutput& last_control_output) {
+     388             : 
+     389           9 :   activation_control_output_ = last_control_output;
+     390             : 
+     391           9 :   double activation_mass = _uav_mass_;
+     392             : 
+     393           9 :   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           9 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     400             : 
+     401           9 :   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           9 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     416             : 
+     417             :   // rampup check
+     418           9 :   if (_rampup_enabled_ && throttle_last_controller) {
+     419             : 
+     420           4 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     421             : 
+     422           4 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     423             : 
+     424           4 :     if (throttle_difference > 0) {
+     425           2 :       rampup_direction_ = 1;
+     426           2 :     } else if (throttle_difference < 0) {
+     427           0 :       rampup_direction_ = -1;
+     428             :     } else {
+     429           2 :       rampup_direction_ = 0;
+     430             :     }
+     431             : 
+     432           4 :     ROS_INFO("[Se3Controller]: activating rampup with initial throttle: %.4f, target: %.4f", throttle_last_controller.value(), hover_throttle);
+     433             : 
+     434           4 :     rampup_active_     = true;
+     435           4 :     rampup_start_time_ = ros::Time::now();
+     436           4 :     rampup_last_time_  = ros::Time::now();
+     437           4 :     rampup_throttle_   = throttle_last_controller.value();
+     438             : 
+     439           4 :     rampup_duration_ = fabs(throttle_difference) / _rampup_speed_;
+     440             :   }
+     441             : 
+     442           9 :   first_iteration_ = true;
+     443           9 :   mute_gains_      = true;
+     444             : 
+     445           9 :   timer_gains_.start();
+     446             : 
+     447             :   // | ------------------ finish the activation ----------------- |
+     448             : 
+     449           9 :   ROS_INFO("[Se3Controller]: activated");
+     450             : 
+     451           9 :   is_active_ = true;
+     452             : 
+     453           9 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* //{ deactivate() */
+     459             : 
+     460           0 : void Se3Controller::deactivate(void) {
+     461             : 
+     462           0 :   is_active_           = false;
+     463           0 :   first_iteration_     = false;
+     464           0 :   uav_mass_difference_ = 0;
+     465             : 
+     466           0 :   timer_gains_.stop();
+     467             : 
+     468           0 :   ROS_INFO("[Se3Controller]: deactivated");
+     469           0 : }
+     470             : 
+     471             : //}
+     472             : 
+     473             : /* updateInactive() //{ */
+     474             : 
+     475       11031 : void Se3Controller::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command) {
+     476             : 
+     477       11031 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     478             : 
+     479       11031 :   last_update_time_ = uav_state.header.stamp;
+     480             : 
+     481       11031 :   first_iteration_ = false;
+     482       11031 : }
+     483             : 
+     484             : //}
+     485             : 
+     486             : /* //{ updateWhenActive() */
+     487             : 
+     488        6668 : Se3Controller::ControlOutput Se3Controller::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+     489             : 
+     490       20004 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     491       20004 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     492             : 
+     493       13336 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     494             : 
+     495        6668 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     496             : 
+     497        6668 :   last_control_output_.desired_heading_rate          = {};
+     498        6668 :   last_control_output_.desired_orientation           = {};
+     499        6668 :   last_control_output_.desired_unbiased_acceleration = {};
+     500        6668 :   last_control_output_.control_output                = {};
+     501             : 
+     502             :   // | -------------------- calculate the dt -------------------- |
+     503             : 
+     504             :   double dt;
+     505             : 
+     506        6668 :   if (first_iteration_) {
+     507           9 :     dt               = 0.01;
+     508           9 :     first_iteration_ = false;
+     509             :   } else {
+     510        6659 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     511             :   }
+     512             : 
+     513        6668 :   last_update_time_ = uav_state.header.stamp;
+     514             : 
+     515        6668 :   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        6668 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     525             : 
+     526        6668 :   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        6668 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     536         843 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing attitude rate output");
+     537         843 :     lowest_modality = common::ATTITUDE_RATE;
+     538        5825 :   } 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        5825 :   } 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        5825 :   } 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        6668 :   switch (lowest_modality.value()) {
+     550             : 
+     551         181 :     case common::POSITION: {
+     552         181 :       positionPassthrough(uav_state, tracker_command);
+     553         181 :       break;
+     554             :     }
+     555             : 
+     556         182 :     case common::VELOCITY_HDG: {
+     557         182 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     558         182 :       break;
+     559             :     }
+     560             : 
+     561         181 :     case common::VELOCITY_HDG_RATE: {
+     562         181 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     563         181 :       break;
+     564             :     }
+     565             : 
+     566         361 :     case common::ACCELERATION_HDG: {
+     567         361 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     568         361 :       break;
+     569             :     }
+     570             : 
+     571         344 :     case common::ACCELERATION_HDG_RATE: {
+     572         344 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     573         344 :       break;
+     574             :     }
+     575             : 
+     576         870 :     case common::ATTITUDE: {
+     577         870 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE);
+     578         870 :       break;
+     579             :     }
+     580             : 
+     581         843 :     case common::ATTITUDE_RATE: {
+     582         843 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     583         843 :       break;
+     584             :     }
+     585             : 
+     586        1901 :     case common::CONTROL_GROUP: {
+     587        1901 :       SE3Controller(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     588        1901 :       break;
+     589             :     }
+     590             : 
+     591        1805 :     case common::ACTUATORS_CMD: {
+     592        1805 :       SE3Controller(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     593        1805 :       break;
+     594             :     }
+     595             : 
+     596        6668 :     default: {
+     597             :     }
+     598             :   }
+     599             : 
+     600        6668 :   return last_control_output_;
+     601             : }
+     602             : 
+     603             : //}
+     604             : 
+     605             : /* //{ getStatus() */
+     606             : 
+     607         393 : const mrs_msgs::ControllerStatus Se3Controller::getStatus() {
+     608             : 
+     609         393 :   mrs_msgs::ControllerStatus controller_status;
+     610             : 
+     611         393 :   controller_status.active = is_active_;
+     612             : 
+     613         393 :   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         104 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr Se3Controller::setConstraints([
+     674             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+     675             : 
+     676         104 :   if (!is_initialized_) {
+     677           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     678             :   }
+     679             : 
+     680         104 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     681             : 
+     682         104 :   ROS_INFO("[Se3Controller]: updating constraints");
+     683             : 
+     684         208 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     685         104 :   res.success = true;
+     686         104 :   res.message = "constraints updated";
+     687             : 
+     688         104 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     689             : }
+     690             : 
+     691             : //}
+     692             : 
+     693             : // --------------------------------------------------------------
+     694             : // |                         controllers                        |
+     695             : // --------------------------------------------------------------
+     696             : 
+     697             : /* SE3Controller() //{ */
+     698             : 
+     699        6124 : 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       12248 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     703        6124 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     704        6124 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     705             : 
+     706             :   // | ----------------- get the current heading ---------------- |
+     707             : 
+     708        6124 :   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        6124 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     719        6124 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     720        6124 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     721             : 
+     722        6124 :   if (tracker_command.use_position_vertical || tracker_command.use_position_horizontal) {
+     723             : 
+     724        6124 :     if (tracker_command.use_position_horizontal) {
+     725        6124 :       Rp[0] = tracker_command.position.x;
+     726        6124 :       Rp[1] = tracker_command.position.y;
+     727             :     } else {
+     728           0 :       Rv[0] = 0;
+     729           0 :       Rv[1] = 0;
+     730             :     }
+     731             : 
+     732        6124 :     if (tracker_command.use_position_vertical) {
+     733        6124 :       Rp[2] = tracker_command.position.z;
+     734             :     } else {
+     735           0 :       Rv[2] = 0;
+     736             :     }
+     737             :   }
+     738             : 
+     739        6124 :   if (tracker_command.use_velocity_horizontal) {
+     740        6124 :     Rv[0] = tracker_command.velocity.x;
+     741        6124 :     Rv[1] = tracker_command.velocity.y;
+     742             :   } else {
+     743           0 :     Rv[0] = 0;
+     744           0 :     Rv[1] = 0;
+     745             :   }
+     746             : 
+     747        6124 :   if (tracker_command.use_velocity_vertical) {
+     748        6124 :     Rv[2] = tracker_command.velocity.z;
+     749             :   } else {
+     750           0 :     Rv[2] = 0;
+     751             :   }
+     752             : 
+     753        6124 :   if (tracker_command.use_acceleration) {
+     754        6124 :     Ra << tracker_command.acceleration.x, tracker_command.acceleration.y, tracker_command.acceleration.z;
+     755             :   } else {
+     756           0 :     Ra << 0, 0, 0;
+     757             :   }
+     758             : 
+     759             :   // | ------ store the estimated values from the uav state ----- |
+     760             : 
+     761             :   // Op - position in global frame
+     762             :   // Ov - velocity in global frame
+     763        6124 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     764        6124 :   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        6124 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     768             : 
+     769             :   // Ow - UAV angular rate
+     770        6124 :   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        6124 :   Eigen::Vector3d Ep(0, 0, 0);
+     776             : 
+     777        6124 :   if (tracker_command.use_position_horizontal || tracker_command.use_position_vertical) {
+     778        6124 :     Ep = Rp - Op;
+     779             :   }
+     780             : 
+     781             :   // velocity control error
+     782        6124 :   Eigen::Vector3d Ev(0, 0, 0);
+     783             : 
+     784        6124 :   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        6124 :     Ev = Rv - Ov;
+     787             :   }
+     788             : 
+     789             :   // | --------------------- load the gains --------------------- |
+     790             : 
+     791        6124 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+     792             : 
+     793        6124 :   Eigen::Vector3d Ka(0, 0, 0);
+     794        6124 :   Eigen::Array3d  Kp(0, 0, 0);
+     795        6124 :   Eigen::Array3d  Kv(0, 0, 0);
+     796        6124 :   Eigen::Array3d  Kq(0, 0, 0);
+     797        6124 :   Eigen::Array3d  Kw(0, 0, 0);
+     798             : 
+     799             :   {
+     800        6124 :     std::scoped_lock lock(mutex_gains_);
+     801             : 
+     802        6124 :     if (tracker_command.use_position_horizontal) {
+     803        6124 :       Kp[0] = gains.kpxy;
+     804        6124 :       Kp[1] = gains.kpxy;
+     805             :     } else {
+     806           0 :       Kp[0] = 0;
+     807           0 :       Kp[1] = 0;
+     808             :     }
+     809             : 
+     810        6124 :     if (tracker_command.use_position_vertical) {
+     811        6124 :       Kp[2] = gains.kpz;
+     812             :     } else {
+     813           0 :       Kp[2] = 0;
+     814             :     }
+     815             : 
+     816        6124 :     if (tracker_command.use_velocity_horizontal) {
+     817        6124 :       Kv[0] = gains.kvxy;
+     818        6124 :       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        6124 :     if (tracker_command.use_velocity_vertical || tracker_command.use_position_vertical) {
+     826        6124 :       Kv[2] = gains.kvz;
+     827             :     } else {
+     828           0 :       Kv[2] = 0;
+     829             :     }
+     830             : 
+     831        6124 :     if (tracker_command.use_acceleration) {
+     832        6124 :       Ka << gains.kaxy, gains.kaxy, gains.kaz;
+     833             :     } else {
+     834           0 :       Ka << 0, 0, 0;
+     835             :     }
+     836             : 
+     837        6124 :     if (!tracker_command.use_attitude_rate) {
+     838        6124 :       Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+     839             :     }
+     840             : 
+     841        6124 :     Kw[0] = gains.kw_roll_pitch;
+     842        6124 :     Kw[1] = gains.kw_roll_pitch;
+     843        6124 :     Kw[2] = gains.kw_yaw;
+     844             :   }
+     845             : 
+     846        6124 :   Kp = Kp * (_uav_mass_ + uav_mass_difference_);
+     847        6124 :   Kv = Kv * (_uav_mass_ + uav_mass_difference_);
+     848             : 
+     849             :   // | --------------- desired orientation matrix --------------- |
+     850             : 
+     851             :   // get body integral in the world frame
+     852             : 
+     853        6124 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+     854             : 
+     855             :   {
+     856       12248 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+     857             : 
+     858        6124 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+     859        6124 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+     860        6124 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+     861        6124 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+     862        6124 :     Ib_b_stamped.vector.z        = 0;
+     863             : 
+     864       12248 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+     865             : 
+     866        6124 :     if (res) {
+     867        6124 :       Ib_w[0] = res.value().vector.x;
+     868        6124 :       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        6124 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+     877             : 
+     878        6124 :   Eigen::Vector3d feed_forward      = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+     879        6124 :   Eigen::Vector3d position_feedback = Kp * Ep.array();
+     880        6124 :   Eigen::Vector3d velocity_feedback = Kv * Ev.array();
+     881        6124 :   Eigen::Vector3d integral_feedback;
+     882             :   {
+     883        6124 :     std::scoped_lock lock(mutex_integrals_);
+     884             : 
+     885        6124 :     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       12248 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+     900             : 
+     901        6124 :     Eigen::Vector3d integration_switch(1, 1, 0);
+     902             : 
+     903             :     // integrate the world error
+     904        6124 :     if (tracker_command.use_position_horizontal) {
+     905        6124 :       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        6124 :     bool world_integral_saturated = false;
+     912        6124 :     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        6124 :     } else if (Iw_w_[0] > gains.kiwxy_lim) {
+     916           0 :       Iw_w_[0]                 = gains.kiwxy_lim;
+     917           0 :       world_integral_saturated = true;
+     918        6124 :     } 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        6124 :     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        6124 :     world_integral_saturated = false;
+     929        6124 :     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        6124 :     } else if (Iw_w_[1] > gains.kiwxy_lim) {
+     933           0 :       Iw_w_[1]                 = gains.kiwxy_lim;
+     934           0 :       world_integral_saturated = true;
+     935        6124 :     } 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        6124 :     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       12248 :     std::scoped_lock lock(mutex_gains_);
+     955             : 
+     956        6124 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+     957        6124 :     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       12248 :       geometry_msgs::Vector3Stamped Ep_stamped;
+     963             : 
+     964        6124 :       Ep_stamped.header.stamp    = ros::Time::now();
+     965        6124 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+     966        6124 :       Ep_stamped.vector.x        = Ep(0);
+     967        6124 :       Ep_stamped.vector.y        = Ep(1);
+     968        6124 :       Ep_stamped.vector.z        = Ep(2);
+     969             : 
+     970       18372 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+     971             : 
+     972        6124 :       if (res) {
+     973        6124 :         Ep_fcu_untilted[0] = res.value().vector.x;
+     974        6124 :         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       12248 :       geometry_msgs::Vector3Stamped Ev_stamped;
+     983             : 
+     984        6124 :       Ev_stamped.header.stamp    = ros::Time::now();
+     985        6124 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+     986        6124 :       Ev_stamped.vector.x        = Ev(0);
+     987        6124 :       Ev_stamped.vector.y        = Ev(1);
+     988        6124 :       Ev_stamped.vector.z        = Ev(2);
+     989             : 
+     990       18372 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+     991             : 
+     992        6124 :       if (res) {
+     993        6124 :         Ev_fcu_untilted[0] = res.value().vector.x;
+     994        6124 :         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        6124 :     if (tracker_command.use_position_horizontal) {
+    1002        6124 :       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        6124 :     bool body_integral_saturated = false;
+    1009        6124 :     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        6124 :     } else if (Ib_b_[0] > gains.kibxy_lim) {
+    1013           0 :       Ib_b_[0]                = gains.kibxy_lim;
+    1014           0 :       body_integral_saturated = true;
+    1015        6124 :     } 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        6124 :     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        6124 :     body_integral_saturated = false;
+    1026        6124 :     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        6124 :     } else if (Ib_b_[1] > gains.kibxy_lim) {
+    1030           0 :       Ib_b_[1]                = gains.kibxy_lim;
+    1031           0 :       body_integral_saturated = true;
+    1032        6124 :     } 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        6124 :     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        6124 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1045             : 
+    1046         705 :     Eigen::Vector3d des_acc = (position_feedback + velocity_feedback + integral_feedback) / total_mass + Ra;
+    1047             : 
+    1048         705 :     if (output_modality == common::ACCELERATION_HDG) {
+    1049             : 
+    1050         722 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1051             : 
+    1052         361 :       cmd.acceleration.x = des_acc[0];
+    1053         361 :       cmd.acceleration.y = des_acc[1];
+    1054         361 :       cmd.acceleration.z = des_acc[2];
+    1055             : 
+    1056         361 :       cmd.heading = tracker_command.heading;
+    1057             : 
+    1058         361 :       last_control_output_.control_output = cmd;
+    1059             : 
+    1060             :     } else {
+    1061             : 
+    1062         344 :       double des_hdg_ff = 0;
+    1063             : 
+    1064         344 :       if (tracker_command.use_heading_rate) {
+    1065         344 :         des_hdg_ff = tracker_command.heading_rate;
+    1066             :       }
+    1067             : 
+    1068         688 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1069             : 
+    1070         344 :       cmd.acceleration.x = des_acc[0];
+    1071         344 :       cmd.acceleration.y = des_acc[1];
+    1072         344 :       cmd.acceleration.z = des_acc[2];
+    1073             : 
+    1074         344 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1075             : 
+    1076         344 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1077             : 
+    1078         344 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1079             : 
+    1080         344 :       cmd.heading_rate = des_hdg_rate;
+    1081             : 
+    1082         344 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1083             : 
+    1084         344 :       last_control_output_.control_output = cmd;
+    1085             :     }
+    1086             : 
+    1087             :     // | -------------- unbiased desired acceleration ------------- |
+    1088             : 
+    1089         705 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1090             : 
+    1091             :     {
+    1092             : 
+    1093         705 :       Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1094             : 
+    1095        1410 :       geometry_msgs::Vector3Stamped world_accel;
+    1096             : 
+    1097         705 :       world_accel.header.stamp    = ros::Time::now();
+    1098         705 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1099         705 :       world_accel.vector.x        = unbiased_des_acc_world[0];
+    1100         705 :       world_accel.vector.y        = unbiased_des_acc_world[1];
+    1101         705 :       world_accel.vector.z        = unbiased_des_acc_world[2];
+    1102             : 
+    1103        2115 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1104             : 
+    1105         705 :       if (res) {
+    1106         705 :         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         705 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1112             : 
+    1113             :     // | ----------------- fill in the diagnostics ---------------- |
+    1114             : 
+    1115         705 :     last_control_output_.diagnostics.ramping_up = false;
+    1116             : 
+    1117         705 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1118         705 :     last_control_output_.diagnostics.mass_difference = 0;
+    1119         705 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1120             : 
+    1121         705 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1122             : 
+    1123         705 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1124         705 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1125             : 
+    1126         705 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1127         705 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1128             : 
+    1129         705 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1130         705 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1131             : 
+    1132         705 :     last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1133             : 
+    1134         705 :     last_control_output_.diagnostics.controller = "Se3Controller";
+    1135             : 
+    1136         705 :     return;
+    1137             :   }
+    1138             : 
+    1139             :   /* mass estimatior //{ */
+    1140             : 
+    1141             :   // --------------------------------------------------------------
+    1142             :   // |                integrate the mass difference               |
+    1143             :   // --------------------------------------------------------------
+    1144             : 
+    1145             :   {
+    1146       10838 :     std::scoped_lock lock(mutex_gains_);
+    1147             : 
+    1148        5419 :     if (tracker_command.use_position_vertical && !rampup_active_) {
+    1149        5415 :       uav_mass_difference_ += gains.km * Ep[2] * dt;
+    1150             :     }
+    1151             : 
+    1152             :     // saturate the mass estimator
+    1153        5419 :     bool uav_mass_saturated = false;
+    1154        5419 :     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        5419 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1158           0 :       uav_mass_difference_ = gains.km_lim;
+    1159           0 :       uav_mass_saturated   = true;
+    1160        5419 :     } 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        5419 :     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        5419 :   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        5419 :   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        5419 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1188             : 
+    1189        5419 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "Se3Controller");
+    1190             : 
+    1191        5419 :   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        5419 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1205             : 
+    1206             :   // --------------------------------------------------------------
+    1207             :   // |               desired orientation + throttle               |
+    1208             :   // --------------------------------------------------------------
+    1209             : 
+    1210             :   // | ------------------- desired orientation ------------------ |
+    1211             : 
+    1212        5419 :   Eigen::Matrix3d Rd;
+    1213             : 
+    1214        5419 :   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        5419 :     Eigen::Vector3d bxd;  // desired heading vector
+    1231             : 
+    1232        5419 :     if (tracker_command.use_heading) {
+    1233        5419 :       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        5419 :     Rd = common::so3transform(f_normed, bxd, drs_params.rotation_type == 1);
+    1240             :   }
+    1241             : 
+    1242             :   // | -------------------- desired throttle -------------------- |
+    1243             : 
+    1244        5419 :   double desired_thrust_force = f.dot(R.col(2));
+    1245        5419 :   double throttle             = 0;
+    1246             : 
+    1247        5419 :   if (tracker_command.use_throttle) {
+    1248             : 
+    1249             :     // the throttle is overriden from the tracker command
+    1250           0 :     throttle = tracker_command.throttle;
+    1251             : 
+    1252        5419 :   } else if (rampup_active_) {
+    1253             : 
+    1254             :     // deactivate the rampup when the times up
+    1255           4 :     if (fabs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1256             : 
+    1257           4 :       rampup_active_ = false;
+    1258             : 
+    1259           4 :       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        5415 :     if (desired_thrust_force >= 0) {
+    1277        5415 :       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        5419 :   bool throttle_saturated = false;
+    1286             : 
+    1287        5419 :   if (!std::isfinite(throttle)) {
+    1288             : 
+    1289           0 :     ROS_ERROR("[Se3Controller]: NaN detected in variable 'throttle'!!!");
+    1290           0 :     return;
+    1291             : 
+    1292        5419 :   } 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        5419 :   } 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        5419 :   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        5419 :   Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1321             : 
+    1322             :   {
+    1323        5419 :     Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1324             : 
+    1325       10838 :     geometry_msgs::Vector3Stamped world_accel;
+    1326             : 
+    1327        5419 :     world_accel.header.stamp    = ros::Time::now();
+    1328        5419 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1329        5419 :     world_accel.vector.x        = unbiased_des_acc_world[0];
+    1330        5419 :     world_accel.vector.y        = unbiased_des_acc_world[1];
+    1331        5419 :     world_accel.vector.z        = unbiased_des_acc_world[2];
+    1332             : 
+    1333       16257 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1334             : 
+    1335        5419 :     if (res) {
+    1336        5419 :       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        5419 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1344             : 
+    1345             :   // fill the unbiased desired accelerations
+    1346        5419 :   last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1347             : 
+    1348             :   // | ----------------- fill in the diagnostics ---------------- |
+    1349             : 
+    1350        5419 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1351             : 
+    1352        5419 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1353        5419 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1354        5419 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1355             : 
+    1356        5419 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1357             : 
+    1358        5419 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1359        5419 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1360             : 
+    1361        5419 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1362        5419 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1363             : 
+    1364        5419 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1365        5419 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1366             : 
+    1367        5419 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1368             : 
+    1369        5419 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1370             : 
+    1371             :   // | ------------ construct the attitude reference ------------ |
+    1372             : 
+    1373        5419 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1374             : 
+    1375        5419 :   attitude_cmd.stamp       = ros::Time::now();
+    1376        5419 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1377        5419 :   attitude_cmd.throttle    = throttle;
+    1378             : 
+    1379        5419 :   if (output_modality == common::ATTITUDE) {
+    1380             : 
+    1381         870 :     last_control_output_.control_output = attitude_cmd;
+    1382             : 
+    1383         870 :     return;
+    1384             :   }
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                      attitude control                      |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390        4549 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1391             : 
+    1392        4549 :   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        4549 :   } else if (tracker_command.use_heading_rate) {
+    1397             : 
+    1398             :     // to fill in the feed forward yaw rate
+    1399        4549 :     double desired_yaw_rate = 0;
+    1400             : 
+    1401             :     try {
+    1402        4549 :       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        4549 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1409             :   }
+    1410             : 
+    1411             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1412             : 
+    1413        4549 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1414             : 
+    1415        4549 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1416             : 
+    1417        4544 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: using jerk feedforward");
+    1418             : 
+    1419        4544 :     Eigen::Matrix3d I;
+    1420        4544 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1421        4544 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1422        4544 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1423             :   }
+    1424             : 
+    1425             :   // | --------------- run the attitude controller -------------- |
+    1426             : 
+    1427        4549 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1428             : 
+    1429        4549 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq,
+    1430        9098 :                                                           drs_params.pitch_roll_heading_rate_compensation);
+    1431             : 
+    1432        4549 :   if (!attitude_rate_command) {
+    1433           0 :     return;
+    1434             :   }
+    1435             : 
+    1436             :   // | --------- fill in the already known attitude rate -------- |
+    1437             : 
+    1438             :   {
+    1439             :     try {
+    1440        4549 :       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        4549 :   if (output_modality == common::ATTITUDE_RATE) {
+    1449             : 
+    1450         843 :     last_control_output_.control_output = attitude_rate_command;
+    1451             : 
+    1452         843 :     return;
+    1453             :   }
+    1454             : 
+    1455             :   // --------------------------------------------------------------
+    1456             :   // |                    Attitude rate control                   |
+    1457             :   // --------------------------------------------------------------
+    1458             : 
+    1459        3706 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1460             : 
+    1461        3706 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1462             : 
+    1463        3706 :   if (!control_group_command) {
+    1464           0 :     return;
+    1465             :   }
+    1466             : 
+    1467        3706 :   if (output_modality == common::CONTROL_GROUP) {
+    1468             : 
+    1469        1901 :     last_control_output_.control_output = control_group_command;
+    1470             : 
+    1471        1901 :     return;
+    1472             :   }
+    1473             : 
+    1474             :   // --------------------------------------------------------------
+    1475             :   // |                        output mixer                        |
+    1476             :   // --------------------------------------------------------------
+    1477             : 
+    1478        3610 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1479             : 
+    1480        1805 :   last_control_output_.control_output = actuator_cmd;
+    1481             : 
+    1482        1805 :   return;
+    1483             : }
+    1484             : 
+    1485             : //}
+    1486             : 
+    1487             : /* positionPassthrough() //{ */
+    1488             : 
+    1489         181 : void Se3Controller::positionPassthrough(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1490             : 
+    1491         181 :   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         362 :   mrs_msgs::HwApiPositionCmd cmd;
+    1497             : 
+    1498         181 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1499         181 :   cmd.header.stamp    = ros::Time::now();
+    1500             : 
+    1501         181 :   cmd.position = tracker_command.position;
+    1502         181 :   cmd.heading  = tracker_command.heading;
+    1503             : 
+    1504         181 :   last_control_output_.control_output = cmd;
+    1505             : 
+    1506             :   // fill the unbiased desired accelerations
+    1507         181 :   last_control_output_.desired_unbiased_acceleration = {};
+    1508         181 :   last_control_output_.desired_orientation           = {};
+    1509         181 :   last_control_output_.desired_heading_rate          = {};
+    1510             : 
+    1511             :   // | ----------------- fill in the diagnostics ---------------- |
+    1512             : 
+    1513         181 :   last_control_output_.diagnostics.ramping_up = false;
+    1514             : 
+    1515         181 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1516         181 :   last_control_output_.diagnostics.mass_difference = 0;
+    1517             : 
+    1518         181 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1519             : 
+    1520         181 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1521         181 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1522             : 
+    1523         181 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1524         181 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1525             : 
+    1526         181 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1527         181 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1528             : 
+    1529         181 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1530             : 
+    1531         181 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1532             : }
+    1533             : 
+    1534             : //}
+    1535             : 
+    1536             : /* PIDVelocityOutput() //{ */
+    1537             : 
+    1538         363 : 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         363 :   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         363 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1547         363 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1548             : 
+    1549         363 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1550         363 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1551             : 
+    1552         363 :   double hdg_ref = tracker_command.heading;
+    1553         363 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1554             : 
+    1555             :   // | ------------------ velocity feedforward ------------------ |
+    1556             : 
+    1557         363 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1558             : 
+    1559         363 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1560         363 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1561             :   }
+    1562             : 
+    1563             :   // | -------------------------- gains ------------------------- |
+    1564             : 
+    1565         363 :   Eigen::Vector3d Kp;
+    1566             : 
+    1567             :   {
+    1568         363 :     std::scoped_lock lock(mutex_gains_);
+    1569             : 
+    1570         363 :     Kp << gains.kpxy, gains.kpxy, gains.kpz;
+    1571             :   }
+    1572             : 
+    1573             :   // | --------------------- control errors --------------------- |
+    1574             : 
+    1575         363 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1576             : 
+    1577             :   // | --------------------------- pid -------------------------- |
+    1578             : 
+    1579         363 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1580         363 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1581         363 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1582             : 
+    1583         363 :   double des_vel_x = position_pid_x_.update(Ep[0], dt);
+    1584         363 :   double des_vel_y = position_pid_y_.update(Ep[1], dt);
+    1585         363 :   double des_vel_z = position_pid_z_.update(Ep[2], dt);
+    1586             : 
+    1587             :   // | -------------------- position feedback ------------------- |
+    1588             : 
+    1589         363 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1590             : 
+    1591         363 :   if (control_output == common::VELOCITY_HDG) {
+    1592             : 
+    1593             :     // | --------------------- fill the output -------------------- |
+    1594             : 
+    1595         364 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1596             : 
+    1597         182 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1598         182 :     cmd.header.stamp    = ros::Time::now();
+    1599             : 
+    1600         182 :     cmd.velocity.x = des_vel[0];
+    1601         182 :     cmd.velocity.y = des_vel[1];
+    1602         182 :     cmd.velocity.z = des_vel[2];
+    1603             : 
+    1604         182 :     cmd.heading = tracker_command.heading;
+    1605             : 
+    1606         182 :     last_control_output_.control_output = cmd;
+    1607             : 
+    1608         181 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1609             : 
+    1610         181 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1611             : 
+    1612         181 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1613             : 
+    1614         181 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1615             : 
+    1616             :     // | --------------------------- ff --------------------------- |
+    1617             : 
+    1618         181 :     double des_hdg_ff = 0;
+    1619             : 
+    1620         181 :     if (tracker_command.use_heading_rate) {
+    1621         181 :       des_hdg_ff = tracker_command.heading_rate;
+    1622             :     }
+    1623             : 
+    1624             :     // | --------------------- fill the output -------------------- |
+    1625             : 
+    1626         362 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1627             : 
+    1628         181 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1629         181 :     cmd.header.stamp    = ros::Time::now();
+    1630             : 
+    1631         181 :     cmd.velocity.x = des_vel[0];
+    1632         181 :     cmd.velocity.y = des_vel[1];
+    1633         181 :     cmd.velocity.z = des_vel[2];
+    1634             : 
+    1635         181 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1636             : 
+    1637         181 :     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         363 :   last_control_output_.desired_unbiased_acceleration = {};
+    1646         363 :   last_control_output_.desired_orientation           = {};
+    1647         363 :   last_control_output_.desired_heading_rate          = {};
+    1648             : 
+    1649             :   // | ----------------- fill in the diagnostics ---------------- |
+    1650             : 
+    1651         363 :   last_control_output_.diagnostics.ramping_up = false;
+    1652             : 
+    1653         363 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1654         363 :   last_control_output_.diagnostics.mass_difference = 0;
+    1655             : 
+    1656         363 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1657             : 
+    1658         363 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1659         363 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1660             : 
+    1661         363 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1662         363 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1663             : 
+    1664         363 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1665         363 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1666             : 
+    1667         363 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1668             : 
+    1669         363 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1670             : }
+    1671             : 
+    1672             : //}
+    1673             : 
+    1674             : // --------------------------------------------------------------
+    1675             : // |                          callbacks                         |
+    1676             : 
+    1677             : 
+    1678             : /* //{ callbackDrs() */
+    1679             : 
+    1680          52 : void Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig& config, [[maybe_unused]] uint32_t level) {
+    1681             : 
+    1682          52 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1683             : 
+    1684          52 :   ROS_INFO("[Se3Controller]: DRS updated gains");
+    1685          52 : }
+    1686             : 
+    1687             : //}
+    1688             : 
+    1689             : // --------------------------------------------------------------
+    1690             : // |                           timers                           |
+    1691             : // --------------------------------------------------------------
+    1692             : 
+    1693             : /* timerGains() //{ */
+    1694             : 
+    1695         797 : void Se3Controller::timerGains(const ros::TimerEvent& event) {
+    1696             : 
+    1697        2391 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1698             :   mrs_lib::ScopeTimer timer =
+    1699        2391 :       mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1700             : 
+    1701        1594 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1702         797 :   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         797 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    1707         797 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    1708             : 
+    1709         797 :   mute_gains_ = false;
+    1710             : 
+    1711         797 :   const double dt = (event.current_real - event.last_real).toSec();
+    1712             : 
+    1713         797 :   bool updated = false;
+    1714             : 
+    1715         797 :   gains.kpxy          = calculateGainChange(dt, gains.kpxy, drs_params.kpxy * gain_coeff, bypass_filter, "kpxy", updated);
+    1716         797 :   gains.kvxy          = calculateGainChange(dt, gains.kvxy, drs_params.kvxy * gain_coeff, bypass_filter, "kvxy", updated);
+    1717         797 :   gains.kaxy          = calculateGainChange(dt, gains.kaxy, drs_params.kaxy * gain_coeff, bypass_filter, "kaxy", updated);
+    1718         797 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    1719         797 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    1720         797 :   gains.kpz           = calculateGainChange(dt, gains.kpz, drs_params.kpz * gain_coeff, bypass_filter, "kpz", updated);
+    1721         797 :   gains.kvz           = calculateGainChange(dt, gains.kvz, drs_params.kvz * gain_coeff, bypass_filter, "kvz", updated);
+    1722         797 :   gains.kaz           = calculateGainChange(dt, gains.kaz, drs_params.kaz * gain_coeff, bypass_filter, "kaz", updated);
+    1723         797 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    1724         797 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    1725         797 :   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         797 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    1729         797 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    1730         797 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    1731             : 
+    1732         797 :   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         797 :   if (updated) {
+    1737             : 
+    1738          81 :     drs_params.kpxy          = gains.kpxy;
+    1739          81 :     drs_params.kvxy          = gains.kvxy;
+    1740          81 :     drs_params.kaxy          = gains.kaxy;
+    1741          81 :     drs_params.kiwxy         = gains.kiwxy;
+    1742          81 :     drs_params.kibxy         = gains.kibxy;
+    1743          81 :     drs_params.kpz           = gains.kpz;
+    1744          81 :     drs_params.kvz           = gains.kvz;
+    1745          81 :     drs_params.kaz           = gains.kaz;
+    1746          81 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    1747          81 :     drs_params.kq_yaw        = gains.kq_yaw;
+    1748          81 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    1749          81 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    1750          81 :     drs_params.km            = gains.km;
+    1751          81 :     drs_params.km_lim        = gains.km_lim;
+    1752             : 
+    1753          81 :     drs_->updateConfig(drs_params);
+    1754             : 
+    1755          81 :     ROS_INFO_THROTTLE(10.0, "[Se3Controller]: gains have been updated");
+    1756             :   }
+    1757         797 : }
+    1758             : 
+    1759             : //}
+    1760             : 
+    1761             : // --------------------------------------------------------------
+    1762             : // |                       other routines                       |
+    1763             : // --------------------------------------------------------------
+    1764             : 
+    1765             : /* calculateGainChange() //{ */
+    1766             : 
+    1767       11158 : 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       11158 :   double change = desired_value - current_value;
+    1771             : 
+    1772       11158 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    1773       11158 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    1774             : 
+    1775       11158 :   if (!bypass_rate) {
+    1776             : 
+    1777             :     // if current value is near 0...
+    1778             :     double change_in_perc;
+    1779             :     double saturated_change;
+    1780             : 
+    1781       11059 :     if (fabs(current_value) < 1e-6) {
+    1782           0 :       change *= gains_filter_max_change;
+    1783             :     } else {
+    1784             : 
+    1785       11059 :       saturated_change = change;
+    1786             : 
+    1787       11059 :       change_in_perc = (current_value + saturated_change) / current_value - 1.0;
+    1788             : 
+    1789       11059 :       if (change_in_perc > gains_filter_max_change) {
+    1790         693 :         saturated_change = current_value * gains_filter_max_change;
+    1791       10366 :       } else if (change_in_perc < -gains_filter_max_change) {
+    1792          22 :         saturated_change = current_value * -gains_filter_max_change;
+    1793             :       }
+    1794             : 
+    1795       11059 :       if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
+    1796           9 :         change *= gains_filter_min_change;
+    1797             :       } else {
+    1798       11050 :         change = saturated_change;
+    1799             :       }
+    1800             :     }
+    1801             :   }
+    1802             : 
+    1803       11158 :   if (fabs(change) > 1e-3) {
+    1804         913 :     ROS_DEBUG("[Se3Controller]: changing gain '%s' from %.2f to %.2f", name.c_str(), current_value, desired_value);
+    1805         913 :     updated = true;
+    1806             :   }
+    1807             : 
+    1808       11158 :   return current_value + change;
+    1809             : }
+    1810             : 
+    1811             : //}
+    1812             : 
+    1813             : /* getHeadingSafely() //{ */
+    1814             : 
+    1815        6487 : double Se3Controller::getHeadingSafely(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1816             : 
+    1817             :   try {
+    1818        6487 :     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          26 : 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..f23e995b0b92509904fb3fafa9935b65284faea0 GIT binary patch literal 5841 zcmV;?7B1Wh~&aLO&d1*>f5ETLOa-FW%>-_WY z(yRR7JCI&$C=*~6)LQkd1?*Gr1ru}aLKK6MCyT4P1aGanfCT}nG3u)}QLP}XSU5hm za?oQu_j3;8K3IL3w17p>7Qnd=T~}0h0cUqMqNfBH` z9*>3|&Fu^5K&NY1fiU140B8Swj+6lU>KUG1)2e_lW{?ChU=*sJA=O;HrvM$Om(uBq z0WT47l86q(5J0AH(zG}Gss)Z?j3T=kj1${|SsO*bxjIg*r8mA~Tu+hismI8UQ`9^s zj-Hx=^;1r&FReF>mVr>0im8!7cw)sZXjo!;bvn|I$WxL68`T)sgrCApV8RF%_K5L} zU=ksugf}2(G}jaDa~Nlek>RPKq53u@X0$b(!x)-8O~poog{ur`sJd1>XX=1v!JQ`b z>lycH!_-G4paQr*bek{|F;lF6I;Bd(F37)_(Pwrn%=E_+V75x4FYy;M`CM=OHA;`5 zL$q-pOV0%Uwlf?}Qo_vKw8Rw=VvJLepp$!2S{+w0s(KPyYp;4h&%hG2B?*xET{|y5 zF;lE}a>{FX*F=oUZU)rrrf%Rd_S=pIjIouMu_%wxz09T_&b|YCep)S780~OXgJqOjgv1%sSyi zAIZt7M#G?)>a++K?bz2Q!%C&<$eW@#T;Wc)L}Jous`9ACh~%o`jGHx8s|)IDZ=XG_ zeCW(j{Q>{AHp1S1BcKS|+rEAo;ZDsh+NdO_R9cyUqLkV*n=xA27^-Zh6PMdmN^`>Q4)xilO8SA6i@0Y3r4|14OD|>i~uyS=Ur9z*X-X*jLx9* z7lh&>2CRG-!|68l1KUXSwvTM1pXNWkjdgYOGbYkq--D~nq5o|WP>+$UR2zA**Rke0 z#WPbce%|db->FmlTZ5}VpoMBxz{fjP1rssB>sF%aWYzg^MyoS!Z9=}Yk5Fg2Q~ej( zNWCc&Q8rt8Ml?;_;HHn6nvXr+zS(mx;*9$d+daQ&pHD^%aqQWE%_lrvxWNfmx$w*` zanFNeJ3k-(&wH1+s7cjUC1}uC3%03a@9LlNuC*PoVa#+2BU^Brd^>aUM*?n$MI8V) zR5ioLaey8g_O}Fxh{^baRM!Pc&}qO062nM(QycfP1Lo+K7Sm@WV&(0#`GQo#xNH0y z{gJO>{rN}vSZJe`m|?aQ&RT}}*4oA%-aSrL@RLCJaLCCo0dla-T^No6on;R%iK+j7 z8r$5)DX0p)x=XS=ci9obj4mIS=G7N8@zRu6{) z{wxfMAHZGW6@$ytMjd<9WnmPb?RePy)BeYzn=NV6CGL(eAY59@fSVR@$@dEw&`J#1 zfW%J`P^ohndIr{2Y@=gMh24#M&wzd5oHksiOrM$D3fpEI)fFb1G5&K3dq0i+^`Z(2 zKUo^Il&IOmYCIs)L$X5w8zgog1E_}n zkuqRnaA8il96KouANO4x93xUl`AGG;23!m`e5xaCZju|a+MZ@giE}Y`&@%Yzg0P7N=flM&r z8QUo~^02zc6#=4g&2=F$_XLblr2KbF2-_ZDk4R2VOp*p5BDYT> zMx3Bk#O!ZBikO1n@}yv5KC5ShzR?nM`xs&j697jH1bja+1(inEGuxM#FEJ7^nhl?! z>h2aKqAJK$|0z>^mu$Ox$yP`B0Z3ZMxp%f%`5vj2kFO)pz#}gL z;yoH@L1Wd9>wN(WzBof0AC>5*mkUhOV|?gBIrJFssU1v*G-G^JO+ywyWz~D0dV319 z;+T=l56^h$=J@|lV0itEj~Y;GBKcFE+-_@s;@hG z?d8ZrsGS0XZ}}(ss?}ZqYKo^NbnkBkK(G0SGYWg)CIxK67!gZ{s%i%%5MZQG0JOBR z;5&)}_2;$#T;a01T|#0v_-1TYH?LCn+PyLzxRIyhz*{a9c*011tu8U}g7rV9Q=7?E z$pWT<<^dOael|kcB^<^oOx|*kj?%V7l=Kvidu57fRJDfIldSwxpVSXPu_w3 zJAm4fmcruk>tzI>wy)WPHZ-Not@$hf6i23wS&)?Msn7xHLS5XUfggQOqr_Nn&t|zk<6>PENuUSBy8Z?B+}Vu8 zk2|!l>?Z3s^AYSSrYpUn@7yUic5B$caBUZc8a^3NS@hZ-h+(HZZmha#_fZQj)&l@zTAZ1@2!83}@Hte_YJC?Z-j&3KP`}46sqH*?#=i z;9|&zRJ+j!1OpDFLBVrSsTVOe0hT9#0f(v%)y%Q^Cb!i9s4N2#MP5?smnrU7%}6kN zqEIDekI`g@>OGuXw^WTWZekY3tZJG&g9S`^9V=BE0p&vjdaBp*akGQDR{c>GJ-d+M zQak~n0<^4LnOhr$;~dZLG|yh3Gss2movb7JxjZYS6sJv_Gmj+0ygJ(+20WU;g}j~s z6c!Nus=#ffgi&b5==3p12zZLi@E{Y3#$T9x=V6VR(D#FV=OHGwPoPap#6YMWt%n1t zKC!iDg}p%@(K)!>U}0FV;Ve}$UK{s?$RE!d$9y(2?oy7QJqgys?PX z%~jU`J#*&MJ+fV>IW-uUxN4B5ycQmhj={&7T#!&Mt;Ni08!(0%Vqh+93AK@mC;9oR zxs8uhHN~s1dgv~0U)#hShop@U@bK`0{0!+5-YU3YG>dsOzJmJ|+%tkZK)oIjt{D9% zeWC$RsWWgn{!De|yr&{aBUm*<9ZQJ$-7J90s$(WyaJNg!Cy2P%Tg(>T^l{4afs&Dw zEFUDOT1;;Q0E0oJdW60kpn>X|>BDIG(3w6Cj4Tt+0IEzMD4q37HhmoDkW_Q?0159u zxR&pjsQ?=7U-j7i%(fBx3qwrW#>9t5Jh;2}&$|T|G(Vs3zJmJ|Tq3w}C;o43#F3c- z@`mI>Y}{$W&EtxnnxgDTtjN~{F}`6omfbY%8QV~ABX@WQgr03}f{P@Tm9=J! zAtDA!7+*CRHv-`P=2rm?R9EB3EZ4R!ErucJlcJB71!;(NW~M`x+0Yf{dSqd=c`87@ zgk(u=&X*vxG23&t#7R+{*!)sp4~~Qs+ej|}@6g83HyttJD18fHr>eIGz`sm2!h9ZM zn#$YSsFjFZ{o2PvAUM@sC$ikOY+ z+!u$8SCu@Pyp=23w|s8J>`K(A{BOmodE+Ck`d+w+My{ILd)TVE%kR`x4?c%fNGVt9 znRv7IZaDqd?2mynTh`m~$)Lo6yM|!uiJ->KaDIfTe&^^%pWzQI6#K(MM}^_skUy3f z8?!qnoVw26F-#@^CDo;29kZsoQyRK*xGImcdsV=3*F~jn1o@cHr7gSVmK^ZeuprSX zoGGgjBWPFU`UxFIl2+jXMd6VyWPkk*V2EUVhKJWH-hK68pu>C}Gn+KM{tT{KOKZ&~`T<#vxzFj3ZTKjI~y}q7MYRyO5Y_^|ctJXShT0tuaX5v!+9P zuaiyx9})O7doF%eI`&*y76tmzT~u@W(5e6LnNq;VFsT>Rd`QO*((DP5+bwgVk1{{t zJ7{M#J)TvNa-$6BYY*sY5V8lNrxncAhlc8qb7V8e^w88C29soj%gkd@3P6nAj{1Wx<^MCh9^ z7B3t%V-yclHDkPXmSe>Mv z*cDaZho|N(nd3@rM64|UKBD6KBo|N-F&c{TMXfU}7%9#gMn6+MC5*I%WuXAS>%kuC zv*^Gc+{~wen1ROBPSHnN)USrehfS54{r~rLm372`KI~pdKS1kMG*tj`4lrc*Vls zei!5ryc*}z%rsZwQms=SNLF2HF=*sx)Zu2O)gCT4(`FBk;Wgm`6|OhHIh*3qFH_aJ z32VtuwjTi6_6}YAsMnNyxlc~cK|6Y zw`Fj#=>Rgp?4k@Gr4i#_QifBvr2ZJG3|Aeb$0XNtemoVA`=Qg`cPNxg+rIE}&5Wqy z_k~GB`GE2>W*q)X7#l`S=UkLMXz0Qrsb^#9b9yfecJw0>KiOt8R!z>tf)7CeF|J|f z{s}ScLBh`mUVvPUlIMy5%5J!}s>?T`C%_E>cBvZTJDIhpk0Ynt#=ss8;Kl(Oi1|I& zP$R>Yhxd!Fp;q&(*o8Q}vybni5%61eQ`G?TAdk-=?B zU(*0cZd{I`aTm8hKOBNef@_s2$(`SyaS-v%Svj^O7Se^WLp?rGuqV_vXL@G!qxSSU zBqBXgCGD({=DpbsFiMPl+RM+`)5wDUfK5FUC&;-&&oJxE8s6^&w9Jn<7fKGOrkW0# zaHg!8Y$keEAVy8Ka&M3Pg{8fHL)E2Zq@w_BZLFp6J#X+mgAvAjko6qheRZadq{U4b z<+QF_K-qwi*E1tan}_Q)CXzWfTvy-3GBJ9pb4^{kT>i%Jn*kh{0Cv!{Cry0cX|D!o zZDT+gE|Bkl5iymIsgj9q>Z!#T==R3^memI z_$8(Sb_N{r%F$>-W=ToI%nVqj*%+8cOGx2G0S)YNGnt>q{A9)O$DQj_TBPdFypeR>?BU0brMn9J!}L zq&}p_MCZeKqx5mtdDk#9vL8I(GYfT9Ft<-QMbmZ_ad zZy|VY+eBR=VeB5F1*YcmaQ#~*=Dt-P?|o>MI}Wk?(IMEop9WzB}F`%eJ#1h10nF#`~cExQO(u23s1HMBbP b{RH$M1E#zRD@MiW00000NkvXXu0mjf=hO%G 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..4dcc31b1e6 --- /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:2023-11-30 22:31:42Functions:31030.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 / 430.0 %3 / 10
<unnamed>100.0 %4 / 430.0 %3 / 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..a2677b61a7 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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 / 430.0 %3 / 10
<unnamed>100.0 %4 / 430.0 %3 / 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..09845818d9 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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 / 430.0 %3 / 10
<unnamed>100.0 %4 / 430.0 %3 / 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..8e4f562b34 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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 / 430.0 %3 / 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..8ce935e739 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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 / 430.0 %3 / 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..83439c7be6 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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 / 430.0 %3 / 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..d4eeafdf0a --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)1435
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)5868
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)7303
+
+
+ + + +
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..5002743bc6 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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*)7303
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)1435
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)5868
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)0
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)0
+
+
+ + + +
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..a51aecde42 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html @@ -0,0 +1,146 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions:31030.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        7303 :     PublisherVisitor(OutputPublisher* obj) : obj_(obj){};
+      48             : 
+      49             :     OutputPublisher* obj_;
+      50             : 
+      51             :     template <class T>
+      52        7303 :     void operator()(const T& msg) {
+      53        7303 :       obj_->publish(msg);
+      54        7303 :     }
+      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/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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)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&)0
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&)0
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&)0
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&)0
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&)0
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::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > 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&)521
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&)1435
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)2832
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&)5348
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)5866
+
+
+ + + +
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..96d36e1607 --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
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&)1435
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&)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&)0
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&)5348
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&)0
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&)0
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&)0
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&)0
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&)521
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&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)2832
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)5866
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)0
+
+
+ + + +
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..0f85b0a961 --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef CONTROL_MANAGER_COMMON_H
+       2             : #define CONTROL_MANAGER_COMMON_H
+       3             : 
+       4             : #include <ros/ros.h>
+       5             : 
+       6             : #include <vector>
+       7             : #include <string>
+       8             : #include <optional>
+       9             : #include <variant>
+      10             : 
+      11             : #include <mrs_lib/attitude_converter.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : 
+      14             : #include <mrs_uav_managers/control_manager/common_handlers.h>
+      15             : #include <mrs_uav_managers/controller.h>
+      16             : 
+      17             : #include <mrs_msgs/TrackerCommand.h>
+      18             : #include <mrs_msgs/UavState.h>
+      19             : #include <mrs_msgs/VelocityReference.h>
+      20             : 
+      21             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      22             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      23             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      24             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      25             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      26             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      27             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      28             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      29             : #include <mrs_msgs/HwApiPositionCmd.h>
+      30             : 
+      31             : #include <mrs_msgs/HwApiCapabilities.h>
+      32             : 
+      33             : #include <nav_msgs/Odometry.h>
+      34             : 
+      35             : namespace mrs_uav_managers
+      36             : {
+      37             : 
+      38             : namespace control_manager
+      39             : {
+      40             : 
+      41             : enum CONTROL_OUTPUT
+      42             : {
+      43             :   ACTUATORS_CMD,
+      44             :   CONTROL_GROUP,
+      45             :   ATTITUDE_RATE,
+      46             :   ATTITUDE,
+      47             :   ACCELERATION_HDG_RATE,
+      48             :   ACCELERATION_HDG,
+      49             :   VELOCITY_HDG_RATE,
+      50             :   VELOCITY_HDG,
+      51             :   POSITION
+      52             : };
+      53             : 
+      54             : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs);
+      55             : 
+      56             : CONTROL_OUTPUT getHighestOuput(const ControlOutputModalities_t& outputs);
+      57             : 
+      58             : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec);
+      59             : 
+      60             : // checks for invalid values in the result from trackers
+      61             : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name);
+      62             : 
+      63             : // checks for invalid messages in/out
+      64             : bool validateOdometry(const nav_msgs::Odometry& msg, const std::string& node_name, const std::string& var_name);
+      65             : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_nam);
+      66             : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name);
+      67             : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name);
+      68             : 
+      69             : std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name, const std::string& platform_config,
+      70             :                                                                 const std::string& custom_config);
+      71             : 
+      72             : // translates the channel values to desired range
+      73             : double RCChannelToRange(double rc_value, double range, double deadband);
+      74             : 
+      75             : /* throttle extraction //{ */
+      76             : 
+      77             : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output);
+      78             : 
+      79             : struct HwApiCmdExtractThrottleVisitor
+      80             : {
+      81           0 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      82             : 
+      83           0 :     if (msg.motors.size() == 0) {
+      84           0 :       return std::nullopt;
+      85             :     }
+      86             : 
+      87           0 :     double throttle = 0;
+      88             : 
+      89           0 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      90           0 :       throttle += msg.motors[i];
+      91             :     };
+      92             : 
+      93           0 :     throttle /= msg.motors.size();
+      94             : 
+      95           0 :     return throttle;
+      96             :   }
+      97           0 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      98           0 :     return msg.throttle;
+      99             :   }
+     100        2832 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+     101        2832 :     return msg.throttle;
+     102             :   }
+     103        5866 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+     104        5866 :     return msg.throttle;
+     105             :   }
+     106           0 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+     107           0 :     return std::nullopt;
+     108             :   }
+     109           0 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+     110           0 :     return std::nullopt;
+     111             :   }
+     112           0 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+     113           0 :     return std::nullopt;
+     114             :   }
+     115           0 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+     116           0 :     return std::nullopt;
+     117             :   }
+     118           0 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+     119           0 :     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           0 :   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           0 :     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           0 :     return validateHwApiActuatorCmd(msg, node_name, var_name);
+     152             :   }
+     153           0 :   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           0 :     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           0 :     return validateHwApiControlGroupCmd(msg, node_name, var_name);
+     162             :   }
+     163        1435 :   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        1435 :     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        1435 :     return validateHwApiAttitudeCmd(msg, node_name, var_name);
+     172             :   }
+     173        5348 :   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        5348 :     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        5348 :     return validateHwApiAttitudeRateCmd(msg, node_name, var_name);
+     182             :   }
+     183           0 :   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           0 :     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           0 :     return validateHwApiAccelerationHdgRateCmd(msg, node_name, var_name);
+     192             :   }
+     193           0 :   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           0 :     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           0 :     return validateHwApiAccelerationHdgCmd(msg, node_name, var_name);
+     202             :   }
+     203           0 :   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           0 :     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           0 :     return validateHwApiVelocityHdgRateCmd(msg, node_name, var_name);
+     212             :   }
+     213           0 :   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           0 :     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           0 :     return validateHwApiVelocityHdgCmd(msg, node_name, var_name);
+     222             :   }
+     223           0 :   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           0 :     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           0 :     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         521 :   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         521 :     initializeHwApiCmd(msg, min_throttle);
+     267         521 :   }
+     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..b78fb27ebf795998a5bca92308fbb9a5211b6233 GIT binary patch literal 979 zcmV;^11$WBP)@APkiaa0T7Kt{@xe{;OOoUx5S+AA0Te@~3K_N#J34pu;$has)P{j16Sl z%ZupbEA%M415c3-$WE3W+8doj3_NXAO=#Lb1?(M~2P|q#V-FD?*+3fF0xSqkHJt_$ zJ=6pwdz{@EwhwZ%kj2t{)fx@VnPm#y49LVyD5ac5RN9KxT4jSiCyzY_4i_nM0cN99TWnu65q-sMS zXQ@^TDy}@P3xBl|h?#MBL_mQ6)@4DfiB6-8;i!h|i0$Kmnubxcg}J9ZtP3rTH_@Y_ zQ$)8tvY3iQp6vA3-PvXj)DBJU+qSO5%CslYJJ@#hx;X`0eXzX8l#d9=HWzu@a*@kg zy&e9ZO;(`&yTTriUbVfQ^T-Qb)@mO)8M~;}hQTu+kHM8O9`%SEn1iPv^z?AH*n{~2 zZGJ4vG!4__|Cus2OiS)>2s{m~gI^Gu>Tr7oFI>>!|DnGp^aWzL`=aZZTSZS15Kq@w zTdx(CpG9w{Rqsb z>Pu!(X{MQpxVeQHnj0Q7>iw9ZxG9o_bF9tm*$iab;53y39{Yg7fAj+pnlnu`BW)@b^yXxkSC$X5xgDF1K~^L9zXyqX-vtW zfbV?Mz?y9wRP*eik=Llw#*cL2b4`cF!C}R9^+XYMm&}pcbZ*Ttni8HzRA}@67J01L zEFjjS_HYT7q;9!26ARN}nnE|>Vi^J}7H7zyo@t^mvA5Sc%p%40pJQc?VTM{3zE_Uq zsk4cu(qo@**ayzX*FSgKxfrQ&*UA6@002ovPDHLkV1nuG B(Hj5& 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..c5f70ccf6b --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 27
<unnamed>13.5 %13 / 9618.5 %5 / 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..b6f1cc6d44 --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 27
<unnamed>13.5 %13 / 9618.5 %5 / 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..02e8adba4a --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 27
<unnamed>13.5 %13 / 9618.5 %5 / 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..b4f35c7b3a --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 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..1879558dba --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 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..11d1904ae1 --- /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:139613.5 %
Date:2023-11-30 22:31:42Functions:52718.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
13.5%13.5%
+ 		13.5 %13 / 9618.5 %5 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + 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..63ce517394 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + 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:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
<unnamed>81.0 %85 / 10586.7 %13 / 15
+
+
+ + + + +
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..4b8c0ae47d --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + 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:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
<unnamed>81.0 %85 / 10586.7 %13 / 15
+
+
+ + + + +
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..c04e44807f --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + 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:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
<unnamed>81.0 %85 / 10586.7 %13 / 15
+
+
+ + + + +
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..b255506dc4 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + 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:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
+
+
+ + + + +
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..8bbd4f5051 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + 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:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
+
+
+ + + + +
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..f38638fddb --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + 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:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
+
+
+ + + + +
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..5690fc6078 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)0
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)28
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&)28
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)8866
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)8869
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)9757
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)10990
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)11087
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)22606
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)41709
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)41723
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)52704
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)83638
+
+
+ + + +
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..d2de31e2bd --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)10990
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)0
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)41709
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)52704
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)28
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)22606
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)8866
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)41723
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)11087
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)8869
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&)28
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)9757
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)83638
+
+
+ + + +
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..da17405a82 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html @@ -0,0 +1,405 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8510581.0 %
Date:2023-11-30 22:31:42Functions:131586.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..31acc42b914f3c4b0b1957dfbad8fa858a556f1a GIT binary patch literal 1253 zcmVV2m}&=1dIKgVcv9Xd%cj5pTK*JdrX!l0D0q8!rKtManR~C_iC*jqE*)WvMB!L)YER3m8-H zssX$AjJvM&eMi^I_05FtWUL>ACNHmnU-almjzzoLfcdzkpj>noC}-4`aQ1xKT{>%v@d~EKR50t$+KAAzDqiai;8y6iT14+6T z5L%#!aFS28S2(DeOd=@1aSC}^3{WP^bc54`xIBzQC*qPP*w;A~CK|ZbdD})nIyJF_ zFg@Uftn~=>-C#X8B&6;$16dfNVSGDKWC-|mi^JI;BJP0RtJpUy$qgy)rmPu6txS)TP9j6`4YK;$v}`-|MfCcp?t*gt%``9Ykxwmx08Gn!k4z@)RB zEnnz5mt3whS|S!G_CmS!{FFT|G`G9IF4bI^LX^fCAj&NI4ZwC`P=P{OuVe)d_(S%{ z=2u#K!0Zr1yEZd52d_-w`+&3pC&+&u&G_mpmK#ukpz>pAU;0mIM8I>4BXIkuMIc(w zY8)d!zQ=DYN+r+2eYSH+NXb7tH$OHbl7bI;phnB?Zv)C&j1=tN$4^9xxO7|PE*b?{ zPm$}d0cYSoGuACP0Mm3-EU#|?@&+m2IeR|$eKY^I zXT2GqK|Yl^W_&C=p5ZD`bZ=)xafF4|~ zr6LeB7P|&lSqhYIc^Y<_ssqoZA>7#>?0Mklc5=mM6@m}Cww^AG&sw+NA0iiY?iwi8 zs%Jk?ps>Ak0s4ruZri?=2GB0dp@vErB4f1D=g}8{R*=n$mBA)JRv1}J$LRWHfr-6v zjC+u!AWX{`S6=LgFnc`j%?folEG!JAhkdRDsUr$6l498PP>WaRh+8NxvI~{DXn9eX z$|#XAGO4kuPq*aqj9xP0T-u9slXJ>R?i9>vV{wY>I4JwvyjmEq>cAxbpOOD|$xavm z=#CE@8W`bFW~eOTd*#oB*kcCAx3(p{IBD#X2018BrF3V0B)du}$?@aPHTYvK$Hg%M zsUE+hV6Rr}Xsa?tif|SaY(BH)n;cDpQvmu)%0fo)SQf_29?OB_!xhr-248LXc`x3u zC;6JPt|&o&GGC99^*=z0Rw|?e6tB|tw+S_bK;m0>#@EMaNyvXM$ZFvaK;NVg$O{%g P00000NkvXXu0mjfEiFX= literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html new file mode 100644 index 0000000000..0a282d1fe8 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + 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:11100.0 %
Date:2023-11-30 22:31:42Functions:1250.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
+
+
+ + + + +
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..0835738542 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + 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:11100.0 %
Date:2023-11-30 22:31:42Functions:1250.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
+
+
+ + + + +
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..b8f8c5ea12 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + 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:11100.0 %
Date:2023-11-30 22:31:42Functions:1250.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
+
+
+ + + + +
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..ec8d2911a8 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + 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:11100.0 %
Date:2023-11-30 22:31:42Functions:1250.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
+
+
+ + + + +
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..a6f2b2ea04 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + 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:11100.0 %
Date:2023-11-30 22:31:42Functions:1250.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
+
+
+ + + + +
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..55fb103291 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index.html @@ -0,0 +1,102 @@ + + + + + + + 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:11100.0 %
Date:2023-11-30 22:31:42Functions:1250.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
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + 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..1170cb3c77 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2023-11-30 22:31:42Functions: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().27
+
+
+ + + +
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..86b5985489 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2023-11-30 22:31:42Functions: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().27
+
+
+ + + +
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..8b7b3eda44 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html @@ -0,0 +1,296 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2023-11-30 22:31:42Functions: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           7 :   virtual ~Tracker() = default;
+     208             : };
+     209             : 
+     210             : }  // namespace mrs_uav_managers
+     211             : 
+     212             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html new file mode 100644 index 0000000000..a0990b0d68 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html @@ -0,0 +1,73 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b561f831d1fbd36bed3927191837491b26db1cec GIT binary patch literal 751 zcmVp{wn9h{fhN)Q+1>Kh+UYP(hJc0`BSsn+CPNKWj|hnq z<0)*3@i`zVJfE>QPz&_v*)wW^qw+LBho)CktARt6Js;21ct&)08r8s&fFAJJZL1l( zz@E+#(66)4XwbM5IRv^zZW$-(*-xzo9?j(YX?%xTALAm%b9Ty_F;@HV40!s?Ay14a z66>?^B)TEnv{sBdR`sb^Mq*_s!eFBB1L(?LuJb11knP}L9V<}uxjuQD*H zkFhcm9{H*|y!052hEop2?ot8@vp;4hJT$n|Q~E&UPpRYy2(p^npi#)s8VZa|R_K6| zOOBv9W?X^^&{i2v8jtN2wZLGmOl35P-`Ou(Mz`sYPwNlwpGJGi?0L!Y&d5b8>50;~ z0BQNI*RV3e9$ch-5C(zUG*S`EyC>DcUP2B-y`gS_o<_P4IG0u{Fjz}iBF-(R^KZL7 zv{J@Ee*>WQ=Wf!T29R5^2x)-Ysmq!vOJfYQFkaJf>oDSPZtP`Q%W-rM)K2~8hS#QR z1%?9&&vyoUvz&%cgx}Zj;<#2|IC!R@$G--somv=&-P^%<!l7lf8xCYN#XB}3 z|3`-N7p=hHG`yS&Ys&?wo%+uhenrA#^%4mp_ZZjfhmYZXjdj9nuO;P~>SOqv(Ya|# zwp#-8DdmOTcQ+g_F`RAh0JT%ggLr!WpF9^@7;k + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18639547.1 %
Date:2023-11-30 22:31:42Functions:111957.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
<unnamed>78.5 %186 / 23778.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html b/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..1d51bc3dac --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18639547.1 %
Date:2023-11-30 22:31:42Functions:111957.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
<unnamed>78.5 %186 / 23778.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail.html b/mrs_uav_managers/include/transform_manager/index-detail.html new file mode 100644 index 0000000000..98c9fe09f9 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-detail.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18639547.1 %
Date:2023-11-30 22:31:42Functions:111957.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
<unnamed>78.5 %186 / 23778.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-f.html b/mrs_uav_managers/include/transform_manager/index-sort-f.html new file mode 100644 index 0000000000..71f8934c58 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18639547.1 %
Date:2023-11-30 22:31:42Functions:111957.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-l.html b/mrs_uav_managers/include/transform_manager/index-sort-l.html new file mode 100644 index 0000000000..9c4e50342b --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18639547.1 %
Date:2023-11-30 22:31:42Functions:111957.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index.html b/mrs_uav_managers/include/transform_manager/index.html new file mode 100644 index 0000000000..54ad2862ae --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18639547.1 %
Date:2023-11-30 22:31:42Functions:111957.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html new file mode 100644 index 0000000000..f02bd0424b --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2023-11-30 22:31:42Functions: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..ef748c527a --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2023-11-30 22:31:42Functions: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..29c8fd1064 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html @@ -0,0 +1,474 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2023-11-30 22:31:42Functions:050.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef TF_MAPPING_ORIGIN_H
+       3             : #define TF_MAPPING_ORIGIN_H
+       4             : 
+       5             : #include <ros/ros.h>
+       6             : 
+       7             : #include <mrs_lib/param_loader.h>
+       8             : #include <mrs_lib/subscribe_handler.h>
+       9             : #include <mrs_lib/transform_broadcaster.h>
+      10             : #include <mrs_lib/attitude_converter.h>
+      11             : #include <mrs_lib/publisher_handler.h>
+      12             : 
+      13             : #include <nav_msgs/Odometry.h>
+      14             : #include <geometry_msgs/QuaternionStamped.h>
+      15             : #include <geometry_msgs/TransformStamped.h>
+      16             : 
+      17             : #include <mrs_uav_managers/estimation_manager/support.h>
+      18             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      19             : 
+      20             : namespace mrs_uav_managers
+      21             : {
+      22             : 
+      23             : /*//{ class TfMappingOrigin */
+      24             : class TfMappingOrigin {
+      25             : 
+      26             :   using Support = estimation_manager::Support;
+      27             : 
+      28             : public:
+      29             :   /*//{ constructor */
+      30           0 :   TfMappingOrigin(ros::NodeHandle nh, std::shared_ptr<mrs_lib::ParamLoader> param_loader, const std::shared_ptr<mrs_lib::TransformBroadcaster>& broadcaster,
+      31             :                   const std::shared_ptr<estimation_manager::CommonHandlers_t> ch)
+      32           0 :       : nh_(nh), broadcaster_(broadcaster), ch_(ch) {
+      33             : 
+      34           0 :     ROS_INFO("[%s]: initializing", getPrintName().c_str());
+      35             : 
+      36           0 :     const std::string yaml_prefix = "mrs_uav_managers/transform_manager/mapping_origin_tf/";
+      37             : 
+      38             :     /*//{ load mapping origin parameters */
+      39           0 :     param_loader->loadParam(yaml_prefix + "debug_prints", debug_prints_);
+      40           0 :     param_loader->loadParam(yaml_prefix + "lateral_topic", lateral_topic_);
+      41           0 :     param_loader->loadParam(yaml_prefix + "altitude_topic", altitude_topic_);
+      42           0 :     param_loader->loadParam(yaml_prefix + "orientation_topic", orientation_topic_);
+      43           0 :     param_loader->loadParam(yaml_prefix + "inverted", tf_inverted_);
+      44           0 :     param_loader->loadParam(yaml_prefix + "custom_frame_id/enabled", custom_frame_id_enabled_);
+      45             : 
+      46           0 :     if (custom_frame_id_enabled_) {
+      47           0 :       param_loader->loadParam(yaml_prefix + "custom_frame_id/frame_id", custom_frame_id_);
+      48             :     }
+      49             : 
+      50           0 :     if (!param_loader->loadedSuccessfully()) {
+      51           0 :       ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      52           0 :       ros::shutdown();
+      53             :     }
+      54             : 
+      55             :     /*//}*/
+      56             : 
+      57             :     /*//{ initialize subscribers */
+      58           0 :     mrs_lib::SubscribeHandlerOptions shopts;
+      59           0 :     shopts.nh                 = nh_;
+      60           0 :     shopts.node_name          = getPrintName();
+      61           0 :     shopts.no_message_timeout = mrs_lib::no_timeout;
+      62           0 :     shopts.threadsafe         = true;
+      63           0 :     shopts.autostart          = true;
+      64           0 :     shopts.queue_size         = 10;
+      65           0 :     shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      66             : 
+      67             :     sh_mapping_odom_lat_ =
+      68           0 :         mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "/" + ch_->uav_name + "/" + lateral_topic_, &TfMappingOrigin::callbackMappingOdomLat, this);
+      69             : 
+      70           0 :     if (orientation_topic_ != lateral_topic_) {
+      71           0 :       sh_mapping_odom_rot_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orientation_topic_.c_str(),
+      72           0 :                                                                                          &TfMappingOrigin::callbackMappingOdomRot, this);
+      73             :     }
+      74             : 
+      75           0 :     if (altitude_topic_ != lateral_topic_) {
+      76           0 :       sh_mapping_odom_alt_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "/" + ch_->uav_name + "/" + altitude_topic_.c_str(),
+      77           0 :                                                                            &TfMappingOrigin::callbackMappingOdomAlt, this);
+      78             :     }
+      79             :     /*//}*/
+      80             : 
+      81           0 :     ph_map_delay_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "map_delay_out", 10);
+      82             : 
+      83           0 :     is_initialized_ = true;
+      84           0 :     ROS_INFO("[%s]: initialized", getPrintName().c_str());
+      85           0 :   }
+      86             :   /*//}*/
+      87             : 
+      88             :   /*//{ getName() */
+      89             :   std::string getName() {
+      90             :     return name_;
+      91             :   }
+      92             :   /*//}*/
+      93             : 
+      94             :   /*//{ getPrintName() */
+      95           0 :   std::string getPrintName() {
+      96           0 :     return ch_->nodelet_name + "/" + name_;
+      97             :   }
+      98             :   /*//}*/
+      99             : 
+     100             : private:
+     101             :   const std::string name_ = "TfMappingOrigin";
+     102             : 
+     103             :   ros::NodeHandle nh_;
+     104             : 
+     105             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     106             : 
+     107             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     108             : 
+     109             :   std::atomic_bool is_initialized_ = false;
+     110             : 
+     111             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_map_delay_;
+     112             : 
+     113             :   bool        debug_prints_;
+     114             :   bool        tf_inverted_;
+     115             :   bool        custom_frame_id_enabled_;
+     116             :   std::string custom_frame_id_;
+     117             :   double      cache_duration_;
+     118             : 
+     119             :   std::string                                   lateral_topic_;
+     120             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_mapping_odom_lat_;
+     121             :   std::vector<nav_msgs::Odometry>               vec_mapping_odom_lat_;
+     122             :   std::atomic_bool                              got_mapping_odom_lat_ = false;
+     123             :   std::atomic<double>                           timestamp_lat_;
+     124             : 
+     125             :   /*//{ callbackMappingOdomLat() */
+     126             : 
+     127           0 :   void callbackMappingOdomLat(const nav_msgs::Odometry::ConstPtr msg) {
+     128             : 
+     129           0 :     if (!is_initialized_) {
+     130           0 :       return;
+     131             :     }
+     132             : 
+     133           0 :     timestamp_lat_ = msg->header.stamp.toSec();
+     134             : 
+     135           0 :     if (!got_mapping_odom_lat_) {
+     136           0 :       got_mapping_odom_lat_ = true;
+     137             :     }
+     138             : 
+     139           0 :     if (!got_mapping_odom_rot_ && orientation_topic_ == lateral_topic_) {
+     140           0 :       got_mapping_odom_rot_ = true;
+     141             :     }
+     142             : 
+     143           0 :     if (!got_mapping_odom_alt_ && altitude_topic_ == lateral_topic_) {
+     144           0 :       got_mapping_odom_alt_ = true;
+     145             :     }
+     146             : 
+     147           0 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TfMappingOrigin::callbackMappingOdomLat", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     148             : 
+     149           0 :     const double hdg_mapping_old = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading();
+     150             : 
+     151             :     /* publish mapping origin tf //{ */
+     152             : 
+     153           0 :     bool clear_needed = false;
+     154             : 
+     155           0 :     if (got_mapping_odom_rot_ && got_mapping_odom_alt_) {
+     156           0 :       std::scoped_lock lock(mtx_mapping_odom_rot_);
+     157             : 
+     158             :       // Copy mapping odometry
+     159           0 :       nav_msgs::Odometry mapping_odom;
+     160           0 :       mapping_odom = *msg;
+     161             : 
+     162             :       // Find corresponding orientation
+     163           0 :       geometry_msgs::QuaternionStamped rot_tmp           = *sh_mapping_odom_rot_.getMsg();  // start with newest msg
+     164           0 :       ros::Time                        dbg_timestamp_rot = rot_tmp.header.stamp;
+     165           0 :       tf2::Quaternion                  tf2_rot;
+     166             : 
+     167           0 :       for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) {
+     168           0 :         if (mapping_odom.header.stamp < vec_mapping_odom_rot_.at(i).header.stamp) {
+     169             : 
+     170             :           // Choose an orientation with closest timestamp
+     171           0 :           double time_diff      = std::fabs(vec_mapping_odom_rot_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     172           0 :           double time_diff_prev = std::numeric_limits<double>::max();
+     173           0 :           if (i > 0) {
+     174           0 :             time_diff_prev = std::fabs(vec_mapping_odom_rot_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     175             :           }
+     176           0 :           if (time_diff_prev < time_diff && i > 0) {
+     177           0 :             i = i - 1;
+     178             :           }
+     179             : 
+     180             :           // Cache is too small if it is full and its oldest element is used
+     181           0 :           if (clear_needed && i == 0) {
+     182           0 :             ROS_WARN_THROTTLE(1.0, "[%s] Mapping orientation cache is too small.", getPrintName().c_str());
+     183             :           }
+     184           0 :           rot_tmp           = vec_mapping_odom_rot_.at(i);
+     185           0 :           dbg_timestamp_rot = vec_mapping_odom_rot_.at(i).header.stamp;
+     186           0 :           break;
+     187             :         }
+     188             :       }
+     189             : 
+     190           0 :       tf2_rot = mrs_lib::AttitudeConverter(rot_tmp.quaternion);
+     191             : 
+     192             :       // Obtain heading from orientation
+     193           0 :       double hdg = 0;
+     194             :       try {
+     195           0 :         hdg = mrs_lib::AttitudeConverter(rot_tmp.quaternion).getHeading();
+     196             :       }
+     197           0 :       catch (...) {
+     198           0 :         ROS_WARN("[%s]: failed to getHeading() from rot_tmp", getPrintName().c_str());
+     199             :       }
+     200             : 
+     201             :       // Build rotation matrix from difference between new heading and old heading
+     202           0 :       tf2::Matrix3x3 rot_mat = mrs_lib::AttitudeConverter(Eigen::AngleAxisd(hdg_mapping_old - hdg, Eigen::Vector3d::UnitZ()));
+     203             : 
+     204             :       // Transform the mavros orientation by the rotation matrix
+     205           0 :       geometry_msgs::Quaternion new_orientation = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_rot);
+     206             : 
+     207             :       // Set new orientation
+     208           0 :       mapping_odom.pose.pose.orientation = new_orientation;
+     209             : 
+     210             : 
+     211             :       // Find corresponding local odom
+     212           0 :       double    odom_alt          = msg->pose.pose.position.z;  // start with newest msg
+     213           0 :       ros::Time dbg_timestamp_alt = msg->header.stamp;
+     214           0 :       for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) {
+     215           0 :         if (mapping_odom.header.stamp < vec_mapping_odom_alt_.at(i).header.stamp) {
+     216             : 
+     217             :           // Choose orientation with closest timestamp
+     218           0 :           double time_diff      = std::fabs(vec_mapping_odom_alt_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     219           0 :           double time_diff_prev = std::numeric_limits<double>::max();
+     220           0 :           if (i > 0) {
+     221           0 :             time_diff_prev = std::fabs(vec_mapping_odom_alt_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     222             :           }
+     223           0 :           if (time_diff_prev < time_diff && i > 0) {
+     224           0 :             i = i - 1;
+     225             :           }
+     226             :           // Cache is too small if it is full and its oldest element is used
+     227           0 :           if (clear_needed && i == 0) {
+     228           0 :             ROS_WARN_THROTTLE(1.0, "[%s] mapping orientation cache (for mapping tf) is too small.", getPrintName().c_str());
+     229             :           }
+     230           0 :           odom_alt          = vec_mapping_odom_alt_.at(i).pose.pose.position.z;
+     231           0 :           dbg_timestamp_alt = vec_mapping_odom_alt_.at(i).header.stamp;
+     232           0 :           break;
+     233             :         }
+     234             :       }
+     235             : 
+     236             :       // Set altitude
+     237           0 :       mapping_odom.pose.pose.position.z = odom_alt;
+     238             : 
+     239             :       // Get inverse transform
+     240           0 :       tf2::Transform      tf_mapping_inv   = Support::tf2FromPose(mapping_odom.pose.pose).inverse();
+     241           0 :       geometry_msgs::Pose pose_mapping_inv = Support::poseFromTf2(tf_mapping_inv);
+     242             : 
+     243           0 :       geometry_msgs::TransformStamped tf_mapping;
+     244           0 :       tf_mapping.header.stamp    = mapping_odom.header.stamp;
+     245           0 :       tf_mapping.header.frame_id = ch_->frames.ns_fcu;
+     246           0 :       if (custom_frame_id_enabled_) {
+     247           0 :         tf_mapping.child_frame_id = ch_->uav_name + "/" + custom_frame_id_;
+     248             :       } else {
+     249           0 :         tf_mapping.child_frame_id = mapping_odom.header.frame_id;
+     250             :       }
+     251           0 :       tf_mapping.transform.translation = Support::pointToVector3(pose_mapping_inv.position);
+     252           0 :       tf_mapping.transform.rotation    = pose_mapping_inv.orientation;
+     253             : 
+     254           0 :       if (Support::noNans(tf_mapping)) {
+     255             :         try {
+     256           0 :           broadcaster_->sendTransform(tf_mapping);
+     257             :         }
+     258           0 :         catch (...) {
+     259           0 :           ROS_ERROR("[%s]: Exception caught during publishing TF: %s - %s.", getPrintName().c_str(), tf_mapping.child_frame_id.c_str(),
+     260             :                     tf_mapping.header.frame_id.c_str());
+     261             :         }
+     262             :       } else {
+     263           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_mapping.header.frame_id.c_str(),
+     264             :                           tf_mapping.child_frame_id.c_str());
+     265             :       }
+     266             : 
+     267             :       // debug timestamps: rot and alt timestamps should be as close as possible to lat_timestamp, the delay is the difference between current time and the time
+     268             :       // of acquisition of scan that was used to calculate the pose estimate
+     269           0 :       if (debug_prints_) {
+     270           0 :         ROS_INFO("[%s] lat timestamp: %.6f, rot stamp: %.6f (diff: %.6f),  alt stamp: %.6f (diff: %.6f), delay: %.6f", getPrintName().c_str(),
+     271             :                  mapping_odom.header.stamp.toSec(), dbg_timestamp_rot.toSec(), dbg_timestamp_rot.toSec() - mapping_odom.header.stamp.toSec(),
+     272             :                  dbg_timestamp_alt.toSec(), dbg_timestamp_alt.toSec() - mapping_odom.header.stamp.toSec(),
+     273             :                  ros::Time::now().toSec() - mapping_odom.header.stamp.toSec());
+     274             :       }
+     275             : 
+     276           0 :       mrs_msgs::Float64Stamped map_delay_msg;
+     277           0 :       map_delay_msg.header.stamp = ros::Time::now();
+     278           0 :       map_delay_msg.value        = ros::Time::now().toSec() - mapping_odom.header.stamp.toSec();
+     279           0 :       ph_map_delay_.publish(map_delay_msg);
+     280             :     }
+     281             : 
+     282             :     //}
+     283             :   }
+     284             :   /*//}*/
+     285             : 
+     286             :   std::string                                                 orientation_topic_;
+     287             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_mapping_odom_rot_;
+     288             :   std::vector<geometry_msgs::QuaternionStamped>               vec_mapping_odom_rot_;
+     289             :   std::mutex                                                  mtx_mapping_odom_rot_;
+     290             :   std::atomic_bool                                            got_mapping_odom_rot_ = false;
+     291             : 
+     292             :   /*//{ callbackMappingOdomRot() */
+     293           0 :   void callbackMappingOdomRot(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     294             : 
+     295           0 :     if (!is_initialized_) {
+     296           0 :       return;
+     297             :     }
+     298             : 
+     299           0 :     if (!got_mapping_odom_lat_) {
+     300           0 :       return;
+     301             :     }
+     302             : 
+     303           0 :     std::scoped_lock lock(mtx_mapping_odom_rot_);
+     304             : 
+     305             :     // Add new data
+     306           0 :     vec_mapping_odom_rot_.push_back(*msg);
+     307             : 
+     308             :     // Delete old data
+     309           0 :     size_t index_delete = 0;
+     310           0 :     bool   clear_needed = false;
+     311           0 :     for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) {
+     312           0 :       if (timestamp_lat_ - vec_mapping_odom_rot_.at(i).header.stamp.toSec() > cache_duration_) {
+     313           0 :         index_delete = i;
+     314           0 :         clear_needed = true;
+     315             :       } else {
+     316           0 :         break;
+     317             :       }
+     318             :     }
+     319           0 :     if (clear_needed) {
+     320           0 :       for (int i = (int)index_delete; i >= 0; i--) {
+     321           0 :         vec_mapping_odom_rot_.erase(vec_mapping_odom_rot_.begin() + i);
+     322             :       }
+     323           0 :       clear_needed = false;
+     324             :     }
+     325             : 
+     326           0 :     if (!got_mapping_odom_rot_) {
+     327           0 :       got_mapping_odom_rot_ = true;
+     328             :     }
+     329             : 
+     330           0 :     if (debug_prints_) {
+     331           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: mapping odom rot cache size: %lu", getPrintName().c_str(), vec_mapping_odom_rot_.size());
+     332             :     }
+     333             :   }
+     334             :   /*//}*/
+     335             : 
+     336             :   std::string                                   altitude_topic_;
+     337             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_mapping_odom_alt_;
+     338             :   std::vector<nav_msgs::Odometry>               vec_mapping_odom_alt_;
+     339             :   std::mutex                                    mtx_mapping_odom_alt_;
+     340             :   std::atomic_bool                              got_mapping_odom_alt_ = false;
+     341             : 
+     342             :   /*//{ callbackMappingOdomAlt() */
+     343           0 :   void callbackMappingOdomAlt(const nav_msgs::Odometry::ConstPtr msg) {
+     344             : 
+     345           0 :     if (!is_initialized_) {
+     346           0 :       return;
+     347             :     }
+     348             : 
+     349           0 :     if (!got_mapping_odom_lat_) {
+     350           0 :       return;
+     351             :     }
+     352             : 
+     353           0 :     std::scoped_lock lock(mtx_mapping_odom_alt_);
+     354             : 
+     355             :     // Add new data
+     356           0 :     vec_mapping_odom_alt_.push_back(*msg);
+     357             : 
+     358             :     // Delete old data
+     359           0 :     size_t index_delete = 0;
+     360           0 :     bool   clear_needed = false;
+     361           0 :     for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) {
+     362           0 :       if (timestamp_lat_ - vec_mapping_odom_alt_.at(i).header.stamp.toSec() > cache_duration_) {
+     363           0 :         index_delete = i;
+     364           0 :         clear_needed = true;
+     365             :       } else {
+     366           0 :         break;
+     367             :       }
+     368             :     }
+     369           0 :     if (clear_needed) {
+     370           0 :       for (int i = (int)index_delete; i >= 0; i--) {
+     371           0 :         vec_mapping_odom_alt_.erase(vec_mapping_odom_alt_.begin() + i);
+     372             :       }
+     373           0 :       clear_needed = false;
+     374             :     }
+     375             : 
+     376           0 :     if (!got_mapping_odom_alt_) {
+     377           0 :       got_mapping_odom_alt_ = true;
+     378             :     }
+     379             : 
+     380           0 :     if (debug_prints_) {
+     381           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: mapping odom alt cache size: %lu", getPrintName().c_str(), vec_mapping_odom_alt_.size());
+     382             :     }
+     383             :   }
+     384             :   /*//}*/
+     385             : };
+     386             : /*//}*/
+     387             : 
+     388             : }  // namespace mrs_uav_managers
+     389             : 
+     390             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html new file mode 100644 index 0000000000..8218f6ae36 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html @@ -0,0 +1,118 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e3d425e3e4319475911ce94a36b8a1ec26ea482f GIT binary patch literal 1536 zcmV+b2LJhqP)fFt&%}Y=;1N(Rt_>#|c)`|cl;5Az%*)9Km%cd5r zUFGeDo-=U20yZ*9J;hlZ%g6PN%&PDs(; zZQc_ql5}Li1&Wc7j2)Hi6v5(q4xMcKUGtTf7AM^CH`7`Hq6Oj%SYJ$l#Hs^eflJTm z6p`qXXZXg6UEq{=1lAoAtqYAy$_SbcWen$csNogxCj?gk&dhVSGAl{NP7wm29|q@Y zJs4C201!2gJu7X(j5MBlK)G+azz7s|2PmwkK%m&);SVZSo1!8DZ7cg4);@CJiEei| z@#X0H03yC>iXcB^isGE>I+Nt-zrYjN%B+1Rv4N zlHIWnIcxoG_QBZN1S=8k+u|La^#LJA-*Fo7%06O(&vKt-d2wQvr|baD(zTYWcZ@0& zP1W!{os`xbscoT+wK$x073aO#paFUR0W!q)WsE;mM%nOwSVkT#oz`@HFoZgGdiJF= z7Yv2+U}%3iS9nYnN!^h$cCCq?k^K=lkPbA+iVa1xc~1MdQV2#`S7!t7uGZCeW388E z#CoN>(6^^%oM_OURDjfKwBv1n6dPJ|uld@;H1_FdxxGX6cb)-=S)A0a$Y{1>js{mI zL)Gz4*s-1hyf*rvtm}qQ1dPNy&mvYQQ@^#7Xl5^5uId|p8ohg&dsTWvo$Go-Dzv0|7P7xJ!4NNnyJ3y7(SM_ z^0KpDSt+z2)$L?BzT?m5Z<6d&q;$uH%f1FiWfHNYEW+1yTb19^SF5b|`O^iyEt*yL zOSgNAo~ebsJDl*4Ct!cd^y71Oi?y($AuEu}=zl~RK57!!TQ?>6<6DMD6wPJ$KUdeh zxz?+zw!Nv{|5kO~AW2PUK)y{&|FoPEg2)(oe_G9(Z$1*qEu(GOW{kc?k!N763n#bLRnqN0) zx1>EhgkziLaw*D{3X&4MJaWlONLPEx0`s)C&V3E00k?sLLUCoJ=4s+YJYR$wu_qSU zaNhCJ(%yW;mV1N2g@;R@q?(7Thw2Z2Cmoh1B0tYGn@hh=eaE9kMmv7={(3|ccg5&V zlkK;X z!aARa46O0;V?gl)dc!TrW;}=UPO3YbiFpQqHxMJQGvMf)wA{LyfYvFOG2d3}jJvop z`5?wl2i-=$rVm$ajVH~;KD_ft5dnTUrBoF^bLBhSYoe!s9y~%vhu=$2EB?F=pslL2 z(tq&*mX1U%u*DJ{PNg)``koHp&pwy(?EoyS$ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18623778.5 %
Date:2023-11-30 22:31:42Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::getIsUtmBased()0
mrs_uav_managers::TfSource::getIsUtmSource()0
mrs_uav_managers::TfSource::setIsUtmSource(bool)0
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)7
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)7
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)7
mrs_uav_managers::TfSource::getName[abi:cxx11]()53
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> > >&)4119
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)5505
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)5505
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)9614
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)9614
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()34145
+
+
+ + + +
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..412f1d9e05 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.func.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18623778.5 %
Date:2023-11-30 22:31:42Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()34145
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)7
mrs_uav_managers::TfSource::getIsUtmBased()0
mrs_uav_managers::TfSource::getIsUtmSource()0
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
mrs_uav_managers::TfSource::setIsUtmSource(bool)0
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)7
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)5505
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> > >&)4119
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)9614
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)5505
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)9614
mrs_uav_managers::TfSource::getName[abi:cxx11]()53
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)7
+
+
+ + + +
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..36959e28c9 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html @@ -0,0 +1,701 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18623778.5 %
Date:2023-11-30 22:31:42Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..fffaa91ed328119a62fbcaa8a472c597b693e295 GIT binary patch literal 2207 zcmV;Q2w?Y#P);hgBY8c=wazLAiaeYF^1WlAS{M;cfg(+1JJEGR@nc<|aydlt3@Jph$(9<} z-8I+Lz%eyALufvJowT{@CGt^f$JSU!d+$_FHQe$9Y?)b~D0KoBB0g60k@7Z`1 zaK>BDn3;Me*dANAjlG?;H6NPmdh{e%AZP5;wkF2b@R()=3}lV{Zd+eVZ)0N*^^anT zd)R-EpX299@5AfyQvj<|oO&+oQqOA!SK6&eBaCx!#vRjKm^{cZAP7uOot8)yAogt$ zXaYsnk-4s$z(7%0AeB63VF23+sA>hpUD`0*Mi)rz@)l532LN4zJPK>fxAWMsalU#G zK4p%(Mrdjpq_%P>)b&_E1rM5(YJ~ejX|xbv@gL!MQue&L{j~<$q45y(ntMku2iN>I zNWHiD)Jy%sP~TiN7lxD9Qc3Cn^eY`kzz>nSZti=W=%4>LCib)@_^HkClWzXED}Yx@ zT{kw6`lt&7q^>P|zD;Vos7uzwtXrwIPC+sx4^B}Ln0h({Z9<9>$hsWv8fIif;1MXY zw&J>O0s}?f_=v|W3}Bm5q@dg;(uCqsd%<3KYE)yw%BaYh)%_-i3nbqLwsW(0A ziGXAboB8m6+FX*WMzLJFR)E^Y$NQHGP18Wj)J${MGSoC#;SJM}{G){Y3d`uvf$~94 z;}a0*AN1_VOs?8ufF6pt&E%YjK)f(BdVjv_6Y_C#LipPbCnA$tj&ERdti zQsjY!&6Wy9F9JSE)&ftsZ|)?~T-foGdz;Vp2`QojUzRQpJlA!60>gw88%uprq|gK& zEOn*IrsPzzU=NRk;z?mTx-QfmrJy4R1LIpI@C?OnVGz|ZVuT{cY<~~z=aayIA`gss z4X{d)XKV?N3JK;b%TWOFrOIV&8_K<*q~;!%b0f>bMl#iJrP>=6{Z>ldz&cZ`r?BhK zfx47TRRRv^v2|YPKyGc_$X+Y9Jw40o5rr$$vq_Kgk%o@P!XHVZd51uHQpslv874X8 z3-oAGLgpfz5U@1#ti{JUn5Ya$eh2X5A+nzg_zo^R-&~yH)$@+5c>;Lz9qm^Mp2dk zJA1hLoLe#bu}MOJxAj}GbkJ=@?k$Do^xGX z%e8ft?I;E;uK9>dalVAr)b~6|kA4(hZ?kLm-lbx}-mNEp#8yf7_EqA(h-z*37| z(IxepSCF%xldd2~?P6Fz>nr-wv|=b!V-GT+x1kG>2l$l_q^iYhpNDJph?YqFWQc)z zCP%FgIHYjNl4!WnrJjMnvDlO9G|IBvS6c?HDP+sAx)y*20}a%p+A^WjTdNYVYx>|< zS_s%>Lb8e(9$Y|b%IVPMG~3QhNJRA*=y){Y5#K}low*tmmylnoOH>qy*uGcG~LRP=1y7QkT7y606-y?34pK2 zy4Dlo0#HbZm%vIw{I6z)Z)EXNGW1oM;d0KtIy21N1ppNBJoA;AVfKDeR|*+lco%_R zkZb-QXVNV{jc= z06PLTa3G5N{woQm?OGX{i=IQ*?kRkUf_<kImcL}_lshmvB=6fu-q>-RQY!~ z`h7wgrxlywMURVOGotq!nk(J|0%`ul_?LYuKPML%G#o6#BZD%<8kMc4m<22 zBnz4+zD1fl2-F5Pt|6$-nn{M;+=2`G{FOU&_iqBQy(yhuaM4CT3YFvfx^E!p?OETJ h6s*MwtFvi|`~fwtzAyX38ma&Q002ovPDHLkV1kIQEMouw literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..0b2917f033 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:14822765.2 %
Date:2023-11-30 22:31:42Functions: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()7
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)7
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()7
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)114
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&)189
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)1182
+
+
+ + + +
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..bf8e495c54 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:14822765.2 %
Date:2023-11-30 22:31:42Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)7
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&)189
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)114
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&)1182
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()7
+
+
+ + + +
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..99e49808c1 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.html @@ -0,0 +1,706 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:14822765.2 %
Date:2023-11-30 22:31:42Functions: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           7 : void ConstraintManager::onInit() {
+     119             : 
+     120           7 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     121             : 
+     122           7 :   ros::Time::waitForValid();
+     123             : 
+     124           7 :   ROS_INFO("[ConstraintManager]: initializing");
+     125             : 
+     126             :   // | ------------------------- params ------------------------- |
+     127             : 
+     128          14 :   mrs_lib::ParamLoader param_loader(nh_, "ConstraintManager");
+     129             : 
+     130          14 :   std::string custom_config_path;
+     131          14 :   std::string platform_config_path;
+     132             : 
+     133           7 :   param_loader.loadParam("custom_config", custom_config_path);
+     134           7 :   param_loader.loadParam("platform_config", platform_config_path);
+     135             : 
+     136           7 :   if (custom_config_path != "") {
+     137           7 :     param_loader.addYamlFile(custom_config_path);
+     138             :   }
+     139             : 
+     140           7 :   if (platform_config_path != "") {
+     141           7 :     param_loader.addYamlFile(platform_config_path);
+     142             :   }
+     143             : 
+     144           7 :   param_loader.addYamlFileFromParam("private_config");
+     145           7 :   param_loader.addYamlFileFromParam("public_config");
+     146           7 :   param_loader.addYamlFileFromParam("public_constraints");
+     147             : 
+     148          14 :   const std::string yaml_prefix = "mrs_uav_managers/constraint_manager/";
+     149             : 
+     150             :   // params passed from the launch file are not prefixed
+     151           7 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     152             : 
+     153           7 :   param_loader.loadParam(yaml_prefix + "constraints", _constraint_names_);
+     154             : 
+     155           7 :   param_loader.loadParam(yaml_prefix + "estimator_types", _estimator_type_names_);
+     156             : 
+     157           7 :   param_loader.loadParam(yaml_prefix + "rate", _constraint_management_rate_);
+     158           7 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     159             : 
+     160           7 :   std::vector<std::string>::iterator it;
+     161             : 
+     162             :   // loading constraint names
+     163          28 :   for (it = _constraint_names_.begin(); it != _constraint_names_.end(); ++it) {
+     164             : 
+     165          21 :     ROS_INFO_STREAM("[ConstraintManager]: loading constraints '" << *it << "'");
+     166             : 
+     167          21 :     mrs_msgs::DynamicsConstraintsSrvRequest new_constraints;
+     168             : 
+     169          21 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/speed", new_constraints.constraints.horizontal_speed);
+     170          21 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/acceleration", new_constraints.constraints.horizontal_acceleration);
+     171          21 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/jerk", new_constraints.constraints.horizontal_jerk);
+     172          21 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/snap", new_constraints.constraints.horizontal_snap);
+     173             : 
+     174          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/speed", new_constraints.constraints.vertical_ascending_speed);
+     175          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/acceleration", new_constraints.constraints.vertical_ascending_acceleration);
+     176          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/jerk", new_constraints.constraints.vertical_ascending_jerk);
+     177          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/snap", new_constraints.constraints.vertical_ascending_snap);
+     178             : 
+     179          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/speed", new_constraints.constraints.vertical_descending_speed);
+     180          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/acceleration", new_constraints.constraints.vertical_descending_acceleration);
+     181          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/jerk", new_constraints.constraints.vertical_descending_jerk);
+     182          21 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/snap", new_constraints.constraints.vertical_descending_snap);
+     183             : 
+     184          21 :     param_loader.loadParam(yaml_prefix + *it + "/heading/speed", new_constraints.constraints.heading_speed);
+     185          21 :     param_loader.loadParam(yaml_prefix + *it + "/heading/acceleration", new_constraints.constraints.heading_acceleration);
+     186          21 :     param_loader.loadParam(yaml_prefix + *it + "/heading/jerk", new_constraints.constraints.heading_jerk);
+     187          21 :     param_loader.loadParam(yaml_prefix + *it + "/heading/snap", new_constraints.constraints.heading_snap);
+     188             : 
+     189          21 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/roll", new_constraints.constraints.roll_rate);
+     190          21 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/pitch", new_constraints.constraints.pitch_rate);
+     191          21 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/yaw", new_constraints.constraints.yaw_rate);
+     192             : 
+     193             :     double tilt_deg;
+     194             : 
+     195          21 :     param_loader.loadParam(yaml_prefix + *it + "/tilt", tilt_deg);
+     196             : 
+     197          21 :     new_constraints.constraints.tilt = M_PI * (tilt_deg / 180.0);
+     198             : 
+     199          21 :     _constraints_.insert(std::pair<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>(*it, new_constraints));
+     200             :   }
+     201             : 
+     202             :   // loading the allowed constraints lists
+     203          56 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     204             : 
+     205          49 :     std::vector<std::string> temp_vector;
+     206          49 :     param_loader.loadParam(yaml_prefix + "allowed_constraints/" + *it, temp_vector);
+     207             : 
+     208          49 :     std::vector<std::string>::iterator it2;
+     209         182 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     210         133 :       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          49 :     _map_type_allowed_constraints_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     217             :   }
+     218             : 
+     219             :   // loading the default constraints
+     220          56 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     221             : 
+     222          49 :     std::string temp_str;
+     223          49 :     param_loader.loadParam(yaml_prefix + "default_constraints/" + *it, temp_str);
+     224             : 
+     225          49 :     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          49 :     _map_type_default_constraints_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     231             :   }
+     232             : 
+     233           7 :   ROS_INFO("[ConstraintManager]: done loading dynamical params");
+     234             : 
+     235           7 :   current_constraints_ = "";
+     236           7 :   last_estimator_name_ = "";
+     237             : 
+     238             :   // | ------------------------ services ------------------------ |
+     239             : 
+     240           7 :   service_server_set_constraints_ = nh_.advertiseService("set_constraints_in", &ConstraintManager::callbackSetConstraints, this);
+     241             : 
+     242           7 :   service_server_constraints_override_ = nh_.advertiseService("constraints_override_in", &ConstraintManager::callbackConstraintsOverride, this);
+     243             : 
+     244           7 :   sc_set_constraints_ = mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>(nh_, "set_constraints_out");
+     245             : 
+     246             :   // | ----------------------- subscribers ---------------------- |
+     247             : 
+     248          14 :   mrs_lib::SubscribeHandlerOptions shopts;
+     249           7 :   shopts.nh                 = nh_;
+     250           7 :   shopts.node_name          = "ConstraintManager";
+     251           7 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     252           7 :   shopts.threadsafe         = true;
+     253           7 :   shopts.autostart          = true;
+     254           7 :   shopts.queue_size         = 10;
+     255           7 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     256             : 
+     257           7 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     258             : 
+     259             :   // | ----------------------- publishers ----------------------- |
+     260             : 
+     261           7 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     262             : 
+     263             :   // | ------------------------- timers ------------------------- |
+     264             : 
+     265           7 :   timer_constraint_management_ = nh_.createTimer(ros::Rate(_constraint_management_rate_), &ConstraintManager::timerConstraintManagement, this);
+     266           7 :   timer_diagnostics_           = nh_.createTimer(ros::Rate(_diagnostics_rate_), &ConstraintManager::timerDiagnostics, this);
+     267             : 
+     268             :   // | ------------------------ profiler ------------------------ |
+     269             : 
+     270           7 :   profiler_ = mrs_lib::Profiler(nh_, "ConstraintManager", _profiler_enabled_);
+     271             : 
+     272             :   // | ------------------- scope timer logger ------------------- |
+     273             : 
+     274           7 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     275          21 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     276           7 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     277             : 
+     278             :   // | ----------------------- finish init ---------------------- |
+     279             : 
+     280           7 :   if (!param_loader.loadedSuccessfully()) {
+     281           0 :     ROS_ERROR("[ConstraintManager]: Could not load all parameters!");
+     282           0 :     ros::shutdown();
+     283             :   }
+     284             : 
+     285           7 :   is_initialized_ = true;
+     286             : 
+     287           7 :   ROS_INFO("[ConstraintManager]: initialized");
+     288             : 
+     289           7 :   ROS_DEBUG("[ConstraintManager]: debug output is enabled");
+     290           7 : }
+     291             : 
+     292             : //}
+     293             : 
+     294             : // --------------------------------------------------------------
+     295             : // |                           methods                          |
+     296             : // --------------------------------------------------------------
+     297             : 
+     298             : /* setConstraints() //{ */
+     299             : 
+     300           7 : bool ConstraintManager::setConstraints(std::string constraints_name) {
+     301             : 
+     302           7 :   std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     303           7 :   it = _constraints_.find(constraints_name);
+     304             : 
+     305           7 :   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          14 :   mrs_msgs::DynamicsConstraintsSrv srv_call;
+     311             : 
+     312           7 :   srv_call.request = it->second;
+     313             : 
+     314           7 :   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           7 :   bool res = sc_set_constraints_.call(srv_call);
+     336             : 
+     337           7 :   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           7 :     if (srv_call.response.success) {
+     345             : 
+     346           7 :       mrs_lib::set_mutexed(mutex_current_constraints_, constraints_name, current_constraints_);
+     347           7 :       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        1182 : void ConstraintManager::timerConstraintManagement(const ros::TimerEvent& event) {
+     471             : 
+     472        1182 :   if (!is_initialized_) {
+     473         297 :     return;
+     474             :   }
+     475             : 
+     476        2364 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerConstraintManagement", _constraint_management_rate_, 0.01, event);
+     477        2364 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerConstraintManagement", scope_timer_logger_, scope_timer_enabled_);
+     478             : 
+     479        1182 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     480        1182 :   auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_);
+     481             : 
+     482        1182 :   if (!sh_estimation_diag_.hasMsg()) {
+     483         297 :     return;
+     484             :   }
+     485             : 
+     486         885 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     487             : 
+     488             :   // | --- automatically set constraints when the state estimator changes -- |
+     489         885 :   if (estimation_diagnostics.current_state_estimator != last_estimator_name_) {
+     490             : 
+     491           7 :     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           7 :     std::map<std::string, std::string>::iterator it;
+     495           7 :     it = _map_type_default_constraints_.find(estimation_diagnostics.current_state_estimator);
+     496             : 
+     497           7 :     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           7 :       if (stringInVector(current_constraints, _map_type_allowed_constraints_.at(estimation_diagnostics.current_state_estimator))) {
+     506             : 
+     507           0 :         last_estimator_name = estimation_diagnostics.current_state_estimator;
+     508             : 
+     509             :         // else, try to set the initial constraints
+     510             :       } else {
+     511             : 
+     512           7 :         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           7 :         if (setConstraints(it->second)) {
+     516             : 
+     517           7 :           last_estimator_name = estimation_diagnostics.current_state_estimator;
+     518             : 
+     519           7 :           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         885 :   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         885 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     544             : }
+     545             : 
+     546             : //}
+     547             : 
+     548             : /* timerDiagnostics() //{ */
+     549             : 
+     550         114 : void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) {
+     551             : 
+     552         114 :   if (!is_initialized_) {
+     553          29 :     return;
+     554             :   }
+     555             : 
+     556         228 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     557         228 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     558             : 
+     559         114 :   if (!sh_estimation_diag_.hasMsg()) {
+     560          29 :     ROS_WARN_THROTTLE(10.0, "[ConstraintManager]: can not do constraint management, missing estimation diagnostics!");
+     561          29 :     return;
+     562             :   }
+     563             : 
+     564         170 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     565             : 
+     566         170 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     567             : 
+     568         170 :   mrs_msgs::ConstraintManagerDiagnostics diagnostics;
+     569             : 
+     570          85 :   diagnostics.stamp        = ros::Time::now();
+     571          85 :   diagnostics.current_name = current_constraints;
+     572          85 :   diagnostics.loaded       = _constraint_names_;
+     573             : 
+     574             :   // get the available constraints
+     575             :   {
+     576          85 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     577          85 :     it = _map_type_allowed_constraints_.find(estimation_diagnostics.current_state_estimator);
+     578             : 
+     579          85 :     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          85 :       diagnostics.available = it->second;
+     584             :     }
+     585             :   }
+     586             : 
+     587             :   // get the current constraint values
+     588             :   {
+     589          85 :     std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     590          85 :     it = _constraints_.find(current_constraints);
+     591             : 
+     592          85 :     diagnostics.current_values = it->second.constraints;
+     593             :   }
+     594             : 
+     595          85 :   ph_diagnostics_.publish(diagnostics);
+     596             : }
+     597             : 
+     598             : //}
+     599             : 
+     600             : // --------------------------------------------------------------
+     601             : // |                          routines                          |
+     602             : // --------------------------------------------------------------
+     603             : 
+     604             : /* stringInVector() //{ */
+     605             : 
+     606         189 : bool ConstraintManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     607             : 
+     608         189 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     609           7 :     return false;
+     610             :   } else {
+     611         182 :     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           7 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::constraint_manager::ConstraintManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..9a046a2692 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html @@ -0,0 +1,176 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.gcov.png b/mrs_uav_managers/src/constraint_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..9ae1d964f15f811ca5d79039c0f6b44997d706ce GIT binary patch literal 2171 zcmV->2!!{EP)!T1s;KO`*+mrIUdn^6rG`EmP#e05rgF98fy{m zr;)5{;LpV{WybbtU595vkTB%~d`dMOj~QYskKa>I99&a;jqf!KF2Z{fLifPBnO3#A zPDYGlgq`jPFWw;`>H`x7fSm7;egnBXKDDe?#r)iM%@ zX!*eI-KNLsj>UX|_I_2{pfIEVH?;{vya^)!FCytOqHINX)f@4rvkDu3s=Rl+1~St!^ZhlR_a3;XdmYz1hqOMon#q(a1IGArMa+0y8@F zdqQh00pTJ=jD)>}aRSK*MLaq_HH1BdIUzd>BUaDaQyBjs>3WqAhC!hr>}Hu&EX*P# zT$DUsX z)>A0?7!$@W*XWN`UAz8?z764udUI=EG|=J+QQQo)g0tA+eYOv7$N6PmKl>5iM1=1` zGlWNpAQ1LH{$x`CWYj<8xEzm9Ij$Y?0z#ybX2(U#8M(W$ST~b+*#{Dqy+`O}?;qhk z`KC*VaFJQ7Qhe4-iJ2hheliWN~*vL&oOBlR- zWp@@Mq9;T@lj=z{#1gI!k=^e}yS^oKh&OwC9yGUN%P4%(DcXudULp<~c(NG3nkGQ%d2BpG=P#AG3x_;yY zve;tr1FmsO5B~>1C`8or*sUl(+tE(E1E!>Er{I^QWxI~Ah`w8#!b|`Qn=AuDb^?IO z%{d)d!rjjYUzq@~McUNkkrqbPdPKC1S0I!k8inuCEXRjwgG}9a4&P~vecum6n7M@~ zL~Ib*Fam{yW=#?C2oaCuaSu-qq|K~)uyzXS!P@z#2uZro-iMIz2!!1f9ub8K@4g_t z%#bIh$j0HymI>4j*Jxk(xP#YAk00SgDr~31tE-JUg{J`|GofiV*w{@ebn4>>mr@Oo zvg`i(c?*s02=9&kz~Fk|M$TUoXl5bfqk$$aEyUHhRmF-U&b!;g*AKU@0I<5VuYyi~)dV#!XBJBXNg+ zI6@ubZ!sZM2hV~cYBuX|sPimD2C4UO3u2}B#POc35C7K*K+0qP#HT#2kD!U4Hm>gi zq~7ED2WB4~Hj*WAxM{yD4i_6c4uwoj^^p`DO@&qA(FMu;B{#^1bo}##!Zxs*4`JfE zUe$Q@T??k3_aL;O!i0a%hDLTf^iQ0e**3O+vG_QCaDHR4e}`M+Ux>`Px|l$ zY*V;lY&Ql+y70oO30}3Vt>#8I02cT25;o3#V_2}#EA+@5V5ilU*Kq+m1%Rfk$4yI# zt?E)jA>zBZXTRI0BZ}Idess9E=&H6y>!5R5K2I1^YZ;^#_Eh@N zWO0LClt4PprsSO!X(U394Ibki$0bou$-{MBCs(MLTN*;kVBXj&NY&g^(2vv7s`8%x zHcZDU4v76;oEz7Y81iCK*!kBE;}pxV&@w2MLi$Y9W41_&X>QvVNs`bwUamcvhDsME zdCu!8m~E`udWzf|{XXB9>nZ4#)bGlx6~*nzoU2zUAd}C)E24{TaQ(|?%EG9U3gn{x zG29;n*MWaa_VGj(mpAI7Xa%AHh&uR=n9LpK-Yi$-e_G+I%fT zDlAC5f5=XoOY8d}4&h!%KPzcxCyZ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/common - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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::RCChannelToRange(double, double, double)0
mrs_uav_managers::control_manager::validateOdometry(nav_msgs::Odometry_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::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::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&)0
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&)0
mrs_uav_managers::control_manager::validateVelocityReference(mrs_msgs::VelocityReference_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::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&)0
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&)0
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&)0
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&)0
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&)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&)7
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&)124
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&)190
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)521
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&)521
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)556
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&)5348
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&)5386
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&)6783
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&)8057
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&)8217
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)9601
+
+
+ + + +
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..745b8aa71d --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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&)124
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)556
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)9601
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)0
mrs_uav_managers::control_manager::validateOdometry(nav_msgs::Odometry_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::validateUavState(mrs_msgs::UavState_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8057
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&)190
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&)521
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&)6783
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&)5386
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&)521
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&)0
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&)8217
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&)0
mrs_uav_managers::control_manager::validateVelocityReference(mrs_msgs::VelocityReference_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::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&)7
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&)0
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&)5348
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&)0
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&)0
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&)0
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&)0
+
+
+ + + +
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..dff4c6077e --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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         124 : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec) {
+      12             : 
+      13         354 :   for (unsigned int i = 0; i < vec.size(); i++) {
+      14         354 :     if (str == vec[i]) {
+      15         124 :       return {i};
+      16             :     }
+      17             :   }
+      18             : 
+      19           0 :   return {};
+      20             : }
+      21             : 
+      22             : //}
+      23             : 
+      24             : /* validateTrackerCommand() //{ */
+      25             : 
+      26        5386 : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name) {
+      27             : 
+      28        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   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        5386 :   return true;
+     165             : }
+     166             : 
+     167             : //}
+     168             : 
+     169             : /* validateOdometry() //{ */
+     170             : 
+     171           0 : bool validateOdometry(const nav_msgs::Odometry& msg, const std::string& node_name, const std::string& var_name) {
+     172             : 
+     173             :   // check position
+     174             : 
+     175           0 :   if (!std::isfinite(msg.pose.pose.position.x)) {
+     176           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.position.x'!!!", node_name.c_str(), var_name.c_str());
+     177           0 :     return false;
+     178             :   }
+     179             : 
+     180           0 :   if (!std::isfinite(msg.pose.pose.position.y)) {
+     181           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.position.y'!!!", node_name.c_str(), var_name.c_str());
+     182           0 :     return false;
+     183             :   }
+     184             : 
+     185           0 :   if (!std::isfinite(msg.pose.pose.position.z)) {
+     186           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.position.z'!!!", node_name.c_str(), var_name.c_str());
+     187           0 :     return false;
+     188             :   }
+     189             : 
+     190             :   // check orientation
+     191             : 
+     192           0 :   if (!std::isfinite(msg.pose.pose.orientation.x)) {
+     193           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.x'!!!", node_name.c_str(), var_name.c_str());
+     194           0 :     return false;
+     195             :   }
+     196             : 
+     197           0 :   if (!std::isfinite(msg.pose.pose.orientation.y)) {
+     198           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.y'!!!", node_name.c_str(), var_name.c_str());
+     199           0 :     return false;
+     200             :   }
+     201             : 
+     202           0 :   if (!std::isfinite(msg.pose.pose.orientation.z)) {
+     203           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.z'!!!", node_name.c_str(), var_name.c_str());
+     204           0 :     return false;
+     205             :   }
+     206             : 
+     207           0 :   if (!std::isfinite(msg.pose.pose.orientation.w)) {
+     208           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.w'!!!", node_name.c_str(), var_name.c_str());
+     209           0 :     return false;
+     210             :   }
+     211             : 
+     212             :   // check velocity
+     213             : 
+     214           0 :   if (!std::isfinite(msg.twist.twist.linear.x)) {
+     215           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.twist.twist.linear.x'!!!", node_name.c_str(), var_name.c_str());
+     216           0 :     return false;
+     217             :   }
+     218             : 
+     219           0 :   if (!std::isfinite(msg.twist.twist.linear.y)) {
+     220           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.twist.twist.linear.y'!!!", node_name.c_str(), var_name.c_str());
+     221           0 :     return false;
+     222             :   }
+     223             : 
+     224           0 :   if (!std::isfinite(msg.twist.twist.linear.z)) {
+     225           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.twist.twist.linear.z'!!!", node_name.c_str(), var_name.c_str());
+     226           0 :     return false;
+     227             :   }
+     228             : 
+     229           0 :   return true;
+     230             : }
+     231             : 
+     232             : //}
+     233             : 
+     234             : /* validateVelocityReference() //{ */
+     235             : 
+     236           0 : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name) {
+     237             : 
+     238             :   // check velocity
+     239             : 
+     240           0 :   if (!std::isfinite(msg.velocity.x)) {
+     241           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     242           0 :     return false;
+     243             :   }
+     244             : 
+     245           0 :   if (!std::isfinite(msg.velocity.y)) {
+     246           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     247           0 :     return false;
+     248             :   }
+     249             : 
+     250           0 :   if (!std::isfinite(msg.velocity.z)) {
+     251           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     252           0 :     return false;
+     253             :   }
+     254             : 
+     255           0 :   if (!std::isfinite(msg.altitude)) {
+     256           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.altitude'!!!", node_name.c_str(), var_name.c_str());
+     257           0 :     return false;
+     258             :   }
+     259             : 
+     260           0 :   if (!std::isfinite(msg.heading)) {
+     261           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     262           0 :     return false;
+     263             :   }
+     264             : 
+     265           0 :   if (!std::isfinite(msg.heading_rate)) {
+     266           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     267           0 :     return false;
+     268             :   }
+     269             : 
+     270           0 :   return true;
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* validateReference() //{ */
+     276             : 
+     277         190 : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name) {
+     278             : 
+     279             :   // check position
+     280             : 
+     281         190 :   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         190 :   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         190 :   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         190 :   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         190 :   return true;
+     304             : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* validateUavState() //{ */
+     309             : 
+     310        8057 : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_name) {
+     311             : 
+     312             :   // check position
+     313             : 
+     314        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   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        8057 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* RCChannelToRange() //{ */
+     459             : 
+     460           0 : double RCChannelToRange(double rc_value, double range, double deadband) {
+     461             : 
+     462           0 :   double tmp_neg1_to_1 = (rc_value - 0.5) * 2.0;
+     463             : 
+     464           0 :   if (tmp_neg1_to_1 > 1.0) {
+     465           0 :     tmp_neg1_to_1 = 1.0;
+     466           0 :   } else if (tmp_neg1_to_1 < -1.0) {
+     467           0 :     tmp_neg1_to_1 = -1.0;
+     468             :   }
+     469             : 
+     470             :   // check the deadband
+     471           0 :   if (tmp_neg1_to_1 < deadband && tmp_neg1_to_1 > -deadband) {
+     472           0 :     return 0.0;
+     473             :   }
+     474             : 
+     475           0 :   if (tmp_neg1_to_1 > 0) {
+     476             : 
+     477           0 :     double tmp = (tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     478             : 
+     479           0 :     return range * tmp;
+     480             : 
+     481             :   } else {
+     482             : 
+     483           0 :     double tmp = (-tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     484             : 
+     485           0 :     return -range * tmp;
+     486             :   }
+     487             : 
+     488             :   return 0.0;
+     489             : }
+     490             : 
+     491             : //}
+     492             : 
+     493             : /* loadDetailedUavModelParams() //{ */
+     494             : 
+     495           7 : 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          14 :   mrs_lib::ParamLoader param_loader(nh, node_name);
+     499             : 
+     500           7 :   if (custom_config != "") {
+     501           7 :     param_loader.addYamlFile(custom_config);
+     502             :   }
+     503             : 
+     504           7 :   if (platform_config != "") {
+     505           7 :     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           7 :   param_loader.loadParam("uav_mass", mass);
+     518             : 
+     519           7 :   param_loader.loadParam("model_params/arm_length", arm_length);
+     520           7 :   param_loader.loadParam("model_params/body_height", body_height);
+     521             : 
+     522           7 :   param_loader.loadParam("model_params/propulsion/force_constant", force_constant);
+     523           7 :   param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant);
+     524           7 :   param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius);
+     525           7 :   param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min);
+     526           7 :   param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max);
+     527             : 
+     528          21 :   Eigen::MatrixXd allocation_matrix = param_loader.loadMatrixDynamic2("model_params/propulsion/allocation_matrix", 4, -1);
+     529             : 
+     530           7 :   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           7 :   int n_motors = allocation_matrix.cols();
+     536             : 
+     537          14 :   DetailedModelParams_t model_params;
+     538             : 
+     539           7 :   model_params.arm_length  = arm_length;
+     540           7 :   model_params.body_height = body_height;
+     541           7 :   model_params.prop_radius = prop_radius;
+     542             : 
+     543          21 :   Eigen::MatrixXd inertia_matrix = param_loader.loadMatrixDynamic2("model_params/inertia_matrix", 3, 3);
+     544             : 
+     545           7 :   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           7 :     model_params.inertia       = Eigen::Matrix3d::Zero();
+     555           7 :     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     556           7 :     model_params.inertia(1, 1) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     557           7 :     model_params.inertia(2, 2) = (mass * arm_length * arm_length) / 2.0;
+     558             : 
+     559           7 :     ROS_INFO("[%s]: inertia computed form parameters:", node_name.c_str());
+     560           7 :     ROS_INFO_STREAM(model_params.inertia);
+     561             :   }
+     562             : 
+     563             :   // create the force-torque allocation matrix
+     564           7 :   model_params.force_torque_mixer = allocation_matrix;
+     565           7 :   model_params.force_torque_mixer.row(0) *= arm_length * force_constant;
+     566           7 :   model_params.force_torque_mixer.row(1) *= arm_length * force_constant;
+     567           7 :   model_params.force_torque_mixer.row(2) *= torque_constant * (3.0 * prop_radius) * force_constant;
+     568           7 :   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          14 :       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          35 :   for (int i = 0; i < n_motors; i++) {
+     581          28 :     alloc_tmp.block(i, 0, 1, 2).normalize();
+     582             :   }
+     583             : 
+     584             :   // the 3rd column (yaw)
+     585          35 :   for (int i = 0; i < n_motors; i++) {
+     586          28 :     if (alloc_tmp(i, 2) > 1e-2) {
+     587          14 :       alloc_tmp(i, 2) = 1.0;
+     588          14 :     } else if (alloc_tmp(i, 2) < -1e-2) {
+     589          14 :       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          35 :   for (int i = 0; i < n_motors; i++) {
+     597          28 :     alloc_tmp(i, 3) = 1.0;
+     598             :   }
+     599             : 
+     600           7 :   std::cout << "Control group mixer: " << std::endl << alloc_tmp << std::endl;
+     601             : 
+     602           7 :   model_params.control_group_mixer = alloc_tmp;
+     603             : 
+     604           7 :   return model_params;
+     605             : }
+     606             : 
+     607             : //}
+     608             : 
+     609             : /* getLowestOutput() //{ */
+     610             : 
+     611         556 : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs) {
+     612             : 
+     613         556 :   if (outputs.actuators) {
+     614           0 :     return ACTUATORS_CMD;
+     615             :   }
+     616             : 
+     617         556 :   if (outputs.control_group) {
+     618           0 :     return CONTROL_GROUP;
+     619             :   }
+     620             : 
+     621         556 :   if (outputs.attitude_rate) {
+     622         556 :     return ATTITUDE_RATE;
+     623             :   }
+     624             : 
+     625           0 :   if (outputs.attitude) {
+     626           0 :     return ATTITUDE;
+     627             :   }
+     628             : 
+     629           0 :   if (outputs.acceleration_hdg_rate) {
+     630           0 :     return ACCELERATION_HDG_RATE;
+     631             :   }
+     632             : 
+     633           0 :   if (outputs.acceleration_hdg) {
+     634           0 :     return ACCELERATION_HDG;
+     635             :   }
+     636             : 
+     637           0 :   if (outputs.velocity_hdg_rate) {
+     638           0 :     return VELOCITY_HDG_RATE;
+     639             :   }
+     640             : 
+     641           0 :   if (outputs.velocity_hdg) {
+     642           0 :     return VELOCITY_HDG;
+     643             :   }
+     644             : 
+     645           0 :   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        9601 : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output) {
+     696             : 
+     697        9601 :   if (!control_output.control_output) {
+     698         903 :     return {};
+     699             :   }
+     700             : 
+     701       17396 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     702             : }
+     703             : 
+     704             : //}
+     705             : 
+     706             : // | -------------- validation of HW api commands ------------- |
+     707             : 
+     708             : /* validateControlOutput() //{ */
+     709             : 
+     710        6783 : 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        6783 :   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        6783 :   std::variant<ControlOutputModalities_t> output_modalities_var{output_modalities};
+     719       13566 :   std::variant<std::string>               node_name_var{node_name};
+     720        6783 :   std::variant<std::string>               var_name_var{var_name};
+     721             : 
+     722        6783 :   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           0 : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name) {
+     730             : 
+     731           0 :   for (size_t i = 0; i < msg.motors.size(); i++) {
+     732           0 :     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           0 :   return true;
+     739             : }
+     740             : 
+     741             : //}
+     742             : 
+     743             : /* validateHwApiControlGroupCmd() //{ */
+     744             : 
+     745           0 : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name) {
+     746             : 
+     747           0 :   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           0 :   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           0 :   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           0 :   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           0 :   return true;
+     768             : }
+     769             : 
+     770             : //}
+     771             : 
+     772             : /* validateHwApiAttitudeCmd() //{ */
+     773             : 
+     774        8217 : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name) {
+     775             : 
+     776             :   // check the orientation
+     777             : 
+     778        8217 :   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        8217 :   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        8217 :   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        8217 :   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        8217 :   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        8217 :   return true;
+     806             : }
+     807             : 
+     808             : //}
+     809             : 
+     810             : /* validateHwApiAttitudeRateCmd() //{ */
+     811             : 
+     812        5348 : 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        5348 :   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        5348 :   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        5348 :   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        5348 :   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        5348 :   return true;
+     839             : }
+     840             : 
+     841             : //}
+     842             : 
+     843             : /* validateHwApiAccelerationHdgRateCmd() //{ */
+     844             : 
+     845           0 : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     846             : 
+     847             :   // | ----------------- check the acceleration ----------------- |
+     848             : 
+     849           0 :   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           0 :   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           0 :   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           0 :   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           0 :   return true;
+     872             : }
+     873             : 
+     874             : //}
+     875             : 
+     876             : /* validateHwApiAccelerationHdgCmd() //{ */
+     877             : 
+     878           0 : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     879             : 
+     880             :   // | ----------------- check the acceleration ----------------- |
+     881             : 
+     882           0 :   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           0 :   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           0 :   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           0 :   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           0 :   return true;
+     905             : }
+     906             : 
+     907             : //}
+     908             : 
+     909             : /* validateHwApiVelocityHdgRateCmd() //{ */
+     910             : 
+     911           0 : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     912             : 
+     913             :   // | ----------------- check the velocity ----------------- |
+     914             : 
+     915           0 :   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           0 :   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           0 :   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           0 :   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           0 :   return true;
+     938             : }
+     939             : 
+     940             : //}
+     941             : 
+     942             : /* validateHwApiVelocityHdgCmd() //{ */
+     943             : 
+     944           0 : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     945             : 
+     946             :   // | ----------------- check the velocity ----------------- |
+     947             : 
+     948           0 :   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           0 :   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           0 :   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           0 :   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           0 :   return true;
+     971             : }
+     972             : 
+     973             : //}
+     974             : 
+     975             : /* validateHwApiPositionHdgCmd() //{ */
+     976             : 
+     977           0 : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name) {
+     978             : 
+     979             :   // | ----------------- check the position ----------------- |
+     980             : 
+     981           0 :   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           0 :   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           0 :   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           0 :   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           0 :   return true;
+    1004             : }
+    1005             : 
+    1006             : //}
+    1007             : 
+    1008             : // | ------------ initialization of HW api commands ----------- |
+    1009             : 
+    1010             : /* initializeDefaultOutput() //{ */
+    1011             : 
+    1012         521 : 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         521 :   CONTROL_OUTPUT lowest_output = getLowestOuput(possible_outputs);
+    1016             : 
+    1017         521 :   Controller::HwApiOutputVariant output;
+    1018             : 
+    1019         521 :   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         521 :     case ATTITUDE_RATE: {
+    1029         521 :       output = mrs_msgs::HwApiAttitudeRateCmd();
+    1030         521 :       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        1042 :   std::variant<mrs_msgs::UavState> uav_state_var{uav_state};
+    1059         521 :   std::variant<double>             min_throttle_var{min_throttle};
+    1060         521 :   std::variant<double>             n_motors_var{n_motors};
+    1061             : 
+    1062         521 :   std::visit(HwApiInitializeVisitor(), output, uav_state_var, min_throttle_var, n_motors_var);
+    1063             : 
+    1064        1042 :   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         521 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle) {
+    1099             : 
+    1100         521 :   msg.stamp = ros::Time::now();
+    1101             : 
+    1102         521 :   msg.body_rate.x = 0;
+    1103         521 :   msg.body_rate.y = 0;
+    1104         521 :   msg.body_rate.z = 0;
+    1105         521 :   msg.throttle    = min_throttle;
+    1106         521 : }
+    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..58b74d00a9deb1cd9f5cd110532759eb06ca8cdb GIT binary patch literal 2419 zcmV-(35@oMP)!9`mS#u)}e?unai%RoC#7Cli!#;l|Wcp0(7i1xXEb}}IO~etLXa<+S z9M*VBQWepxr1H%P>K73X||oGJJJB|2)>XEc2o#vHlo(`r~Fr2s<`+SahJiT$Gd4AiI>R@UG(}~-` zGzms_6~>}n1z;@OM1$19)CxutH{mCV=J8CxV9m4DR@YFRYND<|;sSu01=DQJsOBT# zH;Lwfn^&xP_TSbuof`%L7+`Qy``6??HkF(>&oXO%CR#0e70OSW_^+Ka99uKl`MBz3 z)Jkh}r3rWKU}^=UP1z;<=!SVtfDM|bgXv(t0H!w|9ZaKOXwGqs&F#?0vLEPh>e_r3 zJ!$Z2_jwof4klMzUX{>g}=l?76mlTEm52U9DU{8PyA zx~tpPG+~|&ri1wc7~zFuHa~>T<3_0_%+tZt3Pv^`+4?@O^6L(!Rxn1y%@T88zu&Bb zsR4`;akIo+pLuN0Zo)hrOs!zFk>)7xi+Ds|pLuK}HDR6(rdBW(aWhPogv~Rz<<}id zyYgBS!jh7-p(5sZO z-4B<>5H-t1-1wY=VIGBxJ!!~<7e-53a)-uQsz42U^x6h>Itr8@NY_L;ME3lzLddlb)vOpO>=*~BKc0>xsbZUZO zovB&JoiR}`Duz#PRnLwaa8e-a8YYi)u_Z1jc$zP3OL1I~-~_oCiDb>LhD4On5KKj_ z?y&^#8BDs&(hbGXMlv-*+Ih#CcB*&&#V$M9q8UcC*9AsM)Eu(`2}GT0aGe0K;0=un z;U$d<3P@1q34*C*B&O2LGy@O3qc!RycG-ypX{HmGjRyIcP#Z5Af_j2AVu_o**UWln zqnvW}yMo4w(_BUk>f|tc0M~uCG^Q|l9$o0e6G2nz`X{ZfC!LVY#icvysNn7lsIfqI zMplgIG{6;P7f}o0Rv7^-vu0@Vi!6eRR-15Js059g!P@a};HzAcyrZxbeNG?7$h)F- ztB-$fzex^cBKb_7-mu-sCZiUF7!)sfuTqxg8a~o7q?jw=)*5avsxTc=kvCe&#@)iDe5Gajt=*2$~}W%q*~wz&*=~6tjq1UO}RqgoQm&tEJIzE3VO> zr4(F6)3t@?K2AYgpR3T-?t}Euz_=!vkCBQgGrneZ%*sb-pe5=%L4zmb8Qx?9s1r1r zqbrYKTgwHx-V7kjtfIVW{)M~my*_TpYvn|xF}n-8YvBd4z{F4sWt=pMhe%u?0BGkl zJ!fZyd88d9=K021Qh2?>sOwODmoKB&mR1=9S(mq?Z`H>tqrV)b%NSs!QO01_#f}3< z4+J-M9600p^!{c6$8FQ~ID*LTo|?UOe8Jp&%?0DW@XKlLx88Y`iIEN; zZANlV-Wuf6Uxx=iF_vw=zA-Ijq!|OVR|Up920($)w-!|Z#!IwDu1tJwkjl|$Iz0+v zKe4`I@_kYnFXnmG8$>#o4(1DB(C3iuo2Tv9MZNNTVXk%oG0vnd!;My<ia zQ!AJrNF!ig97vzf(gJe;#{qnqu+Lu3O8@B{?NJ8*Ir2h!)8uYjoO?mtP|Zqfs32h${&-Xo2G>47wm^6%U0fwY5Z5=;-I5iqX` zq~?U9b-du|>4GfB3+z=~-(zSW06%NMZ`(nq$;`yPmP@P`BG(n&YbWN#cNwj@}>su#E6k(V5G59iR}GAcSv0U}vCwq_G7tb&WhMrR*5G zG1+fNuV79)YIPl~Z*N^6R{Sqr#_;{&o-*#BV6ifWPjuH%#vmhoWpuJBS;mTeCja>v z_Z}oTROy238#}I{1g)@#i)O&NIMteKceAE9no&U_u9@=0ZYhXP-`O$5J9RhvU!EO9 z+BldAq?{U}!O82-y3b%W(i{7?o!{T@^ILOJ5y&1tRzUack#g%7NpYVRUQ$@J_$?`D zz&%-k${H!Sqcy5#!7uoKvd4EMf6?C?pLE8D-3h3ozh5v(BUAb#L3#sa2n~J@cZ2l+x97v#@p#k=)^F6(;p}O%3%DRDT=T6q)7Nn$L zr15H=W_L&yp_wFEr&&}GTgDG|I+=IaEhD-K-(g>k#g)xFO`>HT)A)ak6}oe28e@Zi zf`SnzY@KHJNEW1-MzY3e78`1o5X6?T$MCR5tl)n<=K8gB8A!e&h%IAxuCPX|V1DNs l*#)0smqZ5rmPT*e_YWY%j5aNxn0EjG002ovPDHLkV1j!PhWY>i 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..1acf0c90da --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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 +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 31
<unnamed>27.9 %157 / 56338.7 %12 / 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..30b4a5808d --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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 +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 31
<unnamed>27.9 %157 / 56338.7 %12 / 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..c3159370ff --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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 +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 31
<unnamed>27.9 %157 / 56338.7 %12 / 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..25b7c3f9c3 --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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 +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 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..df007aa67e --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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 +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 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..61684389f2 --- /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:15756327.9 %
Date:2023-11-30 22:31:42Functions:123138.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 +
27.9%27.9%
+ 		27.9 %157 / 56338.7 %12 / 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..93140b9d1a --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html @@ -0,0 +1,524 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1679366945.8 %
Date:2023-11-30 22:31:42Functions:5811152.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)0
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()0
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)1
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::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)3
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)3
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()3
mrs_uav_managers::control_manager::ControlManager::elandSrv()3
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)4
mrs_uav_managers::control_manager::ControlManager::isOffboard()5
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)5
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)5
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)6
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::control_manager::ControlManager::initialize()7
mrs_uav_managers::control_manager::ControlManager::preinitialize()7
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)7
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::onInit()7
mrs_uav_managers::control_manager::ControlManager::shutdown()8
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)10
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)15
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)15
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)16
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()19
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)23
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)24
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)24
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)32
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)35
mrs_uav_managers::control_manager::ControlManager::getMass()39
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)42
mrs_uav_managers::control_manager::ControlManager::ungripSrv()103
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)187
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)194
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)235
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)478
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)1064
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)1071
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()1078
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()1078
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)1397
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)3218
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3371
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)5273
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)6762
mrs_uav_managers::control_manager::ControlManager::updateTrackers()6807
mrs_uav_managers::control_manager::ControlManager::asyncControl()8056
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)8204
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)8204
mrs_uav_managers::control_manager::ControlManager::publish()8204
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)8866
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)17542
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)37558
+
+
+ + + +
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..c1d3b441b3 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func.html @@ -0,0 +1,524 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1679366945.8 %
Date:2023-11-30 22:31:42Functions:5811152.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()7
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)42
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)1064
mrs_uav_managers::control_manager::ControlManager::initialize()7
mrs_uav_managers::control_manager::ControlManager::isOffboard()5
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)235
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)17542
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)1071
mrs_uav_managers::control_manager::ControlManager::asyncControl()8056
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)5273
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)1
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)3
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)15
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()7
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)24
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)1397
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)3218
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&)7
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::updateTrackers()6807
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)8866
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()1078
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)23
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&)8204
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)6
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()1078
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)37558
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> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)4
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)16
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&)194
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)5
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> >&)7
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)0
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)10
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()0
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()19
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> >&)15
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&)24
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)187
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)3
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)8204
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)32
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)6762
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()3
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)5
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::onInit()7
mrs_uav_managers::control_manager::ControlManager::getMass()39
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)478
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3371
mrs_uav_managers::control_manager::ControlManager::publish()8204
mrs_uav_managers::control_manager::ControlManager::elandSrv()3
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::shutdown()8
mrs_uav_managers::control_manager::ControlManager::ungripSrv()103
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)35
+
+
+ + + +
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..3701e0ca9c --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html @@ -0,0 +1,8819 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1679366945.8 %
Date:2023-11-30 22:31:42Functions:5811152.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_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          35 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+     207          35 :                                    double odometry_innovation_threshold, bool human_switchable) {
+     208             : 
+     209          35 :   this->eland_threshold               = eland_threshold;
+     210          35 :   this->odometry_innovation_threshold = odometry_innovation_threshold;
+     211          35 :   this->failsafe_threshold            = failsafe_threshold;
+     212          35 :   this->address                       = address;
+     213          35 :   this->name_space                    = name_space;
+     214          35 :   this->human_switchable              = human_switchable;
+     215          35 : }
+     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          42 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+     233             : 
+     234          42 :   this->address          = address;
+     235          42 :   this->name_space       = name_space;
+     236          42 :   this->human_switchable = human_switchable;
+     237          42 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : class ControlManager : public nodelet::Nodelet {
+     242             : 
+     243             : public:
+     244             :   virtual void onInit();
+     245             : 
+     246             : private:
+     247             :   ros::NodeHandle   nh_;
+     248             :   std::atomic<bool> is_initialized_ = false;
+     249             :   std::string       _uav_name_;
+     250             :   std::string       _body_frame_;
+     251             : 
+     252             :   std::string _custom_config_;
+     253             :   std::string _platform_config_;
+     254             :   std::string _world_config_;
+     255             :   std::string _network_config_;
+     256             : 
+     257             :   // | --------------- dynamic loading of trackers -------------- |
+     258             : 
+     259             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_;  // pluginlib loader of dynamically loaded trackers
+     260             :   std::vector<std::string>                                           _tracker_names_;  // list of tracker names
+     261             :   std::map<std::string, TrackerParams>                               trackers_;        // map between tracker names and tracker param
+     262             :   std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>>          tracker_list_;    // list of trackers, routines are callable from this
+     263             :   std::mutex                                                         mutex_tracker_list_;
+     264             : 
+     265             :   // | ------------- dynamic loading of controllers ------------- |
+     266             : 
+     267             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_;  // pluginlib loader of dynamically loaded controllers
+     268             :   std::vector<std::string>                                              _controller_names_;  // list of controller names
+     269             :   std::map<std::string, ControllerParams>                               controllers_;        // map between controller names and controller params
+     270             :   std::vector<boost::shared_ptr<mrs_uav_managers::Controller>>          controller_list_;    // list of controllers, routines are callable from this
+     271             :   std::mutex                                                            mutex_controller_list_;
+     272             : 
+     273             :   // | ------------------------- HW API ------------------------- |
+     274             : 
+     275             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     276             : 
+     277             :   OutputPublisher control_output_publisher_;
+     278             : 
+     279             :   ControlOutputModalities_t _hw_api_inputs_;
+     280             : 
+     281             :   double desired_uav_state_rate_ = 100.0;
+     282             : 
+     283             :   // this timer will check till we already got the hardware api diagnostics
+     284             :   // then it will trigger the initialization of the controllers and finish
+     285             :   // the initialization of the ControlManager
+     286             :   ros::Timer timer_hw_api_capabilities_;
+     287             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     288             : 
+     289             :   void preinitialize(void);
+     290             :   void initialize(void);
+     291             : 
+     292             :   // | ------------ tracker and controller switching ------------ |
+     293             : 
+     294             :   std::tuple<bool, std::string> switchController(const std::string& controller_name);
+     295             :   std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+     296             : 
+     297             :   // the time of last switching of a tracker or a controller
+     298             :   ros::Time  controller_tracker_switch_time_;
+     299             :   std::mutex mutex_controller_tracker_switch_time_;
+     300             : 
+     301             :   // | -------------------- the transformer  -------------------- |
+     302             : 
+     303             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     304             : 
+     305             :   // | ------------------- scope timer logger ------------------- |
+     306             : 
+     307             :   bool                                       scope_timer_enabled_ = false;
+     308             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     309             : 
+     310             :   // | --------------------- general params --------------------- |
+     311             : 
+     312             :   // defines the type of state input: odometry or uav_state mesasge types
+     313             :   int _state_input_;
+     314             : 
+     315             :   // names of important trackers
+     316             :   std::string _null_tracker_name_;     // null tracker is active when UAV is not in the air
+     317             :   std::string _ehover_tracker_name_;   // ehover tracker is used for emergency hovering
+     318             :   std::string _landoff_tracker_name_;  // landoff is used for landing and takeoff
+     319             : 
+     320             :   // names of important controllers
+     321             :   std::string _failsafe_controller_name_;  // controller used for feed-forward failsafe
+     322             :   std::string _eland_controller_name_;     // controller used for emergancy landing
+     323             : 
+     324             :   // joystick control
+     325             :   bool        _joystick_enabled_ = false;
+     326             :   int         _joystick_mode_;
+     327             :   std::string _joystick_tracker_name_;
+     328             :   std::string _joystick_controller_name_;
+     329             :   std::string _joystick_fallback_tracker_name_;
+     330             :   std::string _joystick_fallback_controller_name_;
+     331             : 
+     332             :   // should disarm after emergancy landing?
+     333             :   bool _eland_disarm_enabled_ = false;
+     334             : 
+     335             :   // enabling the emergency handoff -> will disable eland and failsafe
+     336             :   bool _rc_emergency_handoff_ = false;
+     337             : 
+     338             :   // what throttle should be output when null tracker is active?
+     339             :   double _min_throttle_null_tracker_ = 0.0;
+     340             : 
+     341             :   // rates of all the timers
+     342             :   double _status_timer_rate_   = 0;
+     343             :   double _safety_timer_rate_   = 0;
+     344             :   double _elanding_timer_rate_ = 0;
+     345             :   double _failsafe_timer_rate_ = 0;
+     346             :   double _bumper_timer_rate_   = 0;
+     347             : 
+     348             :   bool _snap_trajectory_to_safety_area_ = false;
+     349             : 
+     350             :   // | -------------- uav_state/odometry subscriber ------------- |
+     351             : 
+     352             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+     353             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     354             : 
+     355             :   mrs_msgs::UavState uav_state_;
+     356             :   mrs_msgs::UavState previous_uav_state_;
+     357             :   bool               got_uav_state_               = false;
+     358             :   double             _uav_state_max_missing_time_ = 0;  // how long should we tolerate missing state estimate?
+     359             :   double             uav_roll_                    = 0;
+     360             :   double             uav_pitch_                   = 0;
+     361             :   double             uav_yaw_                     = 0;
+     362             :   double             uav_heading_                 = 0;
+     363             :   std::mutex         mutex_uav_state_;
+     364             : 
+     365             :   // odometry hiccup detection
+     366             :   double uav_state_avg_dt_        = 1;
+     367             :   double uav_state_hiccup_factor_ = 1;
+     368             :   int    uav_state_count_         = 0;
+     369             : 
+     370             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     371             : 
+     372             :   // | -------------- safety area max z subscriber -------------- |
+     373             : 
+     374             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+     375             : 
+     376             :   // | ------------- odometry innovation subscriber ------------- |
+     377             : 
+     378             :   // odometry innovation is published by the odometry node
+     379             :   // it is used to issue eland if the estimator's input is too wonky
+     380             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+     381             : 
+     382             :   // | --------------------- common handlers -------------------- |
+     383             : 
+     384             :   // contains handlers that are shared with trackers and controllers
+     385             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+     386             : 
+     387             :   // | --------------- tracker and controller IDs --------------- |
+     388             : 
+     389             :   // keeping track of currently active controllers and trackers
+     390             :   unsigned int active_tracker_idx_    = 0;
+     391             :   unsigned int active_controller_idx_ = 0;
+     392             : 
+     393             :   // indeces of some notable trackers
+     394             :   unsigned int _ehover_tracker_idx_               = 0;
+     395             :   unsigned int _landoff_tracker_idx_              = 0;
+     396             :   unsigned int _joystick_tracker_idx_             = 0;
+     397             :   unsigned int _joystick_controller_idx_          = 0;
+     398             :   unsigned int _failsafe_controller_idx_          = 0;
+     399             :   unsigned int _joystick_fallback_controller_idx_ = 0;
+     400             :   unsigned int _joystick_fallback_tracker_idx_    = 0;
+     401             :   unsigned int _null_tracker_idx_                 = 0;
+     402             :   unsigned int _eland_controller_idx_             = 0;
+     403             : 
+     404             :   // | -------------- enabling the output publisher ------------- |
+     405             : 
+     406             :   void              toggleOutput(const bool& input);
+     407             :   std::atomic<bool> output_enabled_ = false;
+     408             : 
+     409             :   // | ----------------------- publishers ----------------------- |
+     410             : 
+     411             :   mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>     ph_controller_diagnostics_;
+     412             :   mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>            ph_tracker_cmd_;
+     413             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>            ph_mrs_odom_input_;
+     414             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>                  ph_control_reference_odom_;
+     415             :   mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+     416             :   mrs_lib::PublisherHandler<std_msgs::Empty>                     ph_offboard_on_;
+     417             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_tilt_error_;
+     418             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_estimate_;
+     419             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_throttle_;
+     420             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_thrust_;
+     421             :   mrs_lib::PublisherHandler<mrs_msgs::ControlError>              ph_control_error_;
+     422             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_markers_;
+     423             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_coordinates_markers_;
+     424             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_disturbances_markers_;
+     425             :   mrs_lib::PublisherHandler<mrs_msgs::BumperStatus>              ph_bumper_status_;
+     426             :   mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>       ph_current_constraints_;
+     427             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_heading_;
+     428             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_speed_;
+     429             : 
+     430             :   // | --------------------- service servers -------------------- |
+     431             : 
+     432             :   ros::ServiceServer service_server_switch_tracker_;
+     433             :   ros::ServiceServer service_server_switch_controller_;
+     434             :   ros::ServiceServer service_server_reset_tracker_;
+     435             :   ros::ServiceServer service_server_hover_;
+     436             :   ros::ServiceServer service_server_ehover_;
+     437             :   ros::ServiceServer service_server_failsafe_;
+     438             :   ros::ServiceServer service_server_failsafe_escalating_;
+     439             :   ros::ServiceServer service_server_toggle_output_;
+     440             :   ros::ServiceServer service_server_arm_;
+     441             :   ros::ServiceServer service_server_enable_callbacks_;
+     442             :   ros::ServiceServer service_server_set_constraints_;
+     443             :   ros::ServiceServer service_server_use_joystick_;
+     444             :   ros::ServiceServer service_server_use_safety_area_;
+     445             :   ros::ServiceServer service_server_emergency_reference_;
+     446             :   ros::ServiceServer service_server_pirouette_;
+     447             :   ros::ServiceServer service_server_eland_;
+     448             :   ros::ServiceServer service_server_parachute_;
+     449             : 
+     450             :   // human callbable services for references
+     451             :   ros::ServiceServer service_server_goto_;
+     452             :   ros::ServiceServer service_server_goto_fcu_;
+     453             :   ros::ServiceServer service_server_goto_relative_;
+     454             :   ros::ServiceServer service_server_goto_altitude_;
+     455             :   ros::ServiceServer service_server_set_heading_;
+     456             :   ros::ServiceServer service_server_set_heading_relative_;
+     457             : 
+     458             :   // the reference service and subscriber
+     459             :   ros::ServiceServer                                    service_server_reference_;
+     460             :   mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+     461             : 
+     462             :   // the velocity reference service and subscriber
+     463             :   ros::ServiceServer                                            service_server_velocity_reference_;
+     464             :   mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+     465             : 
+     466             :   // trajectory tracking
+     467             :   ros::ServiceServer                                       service_server_trajectory_reference_;
+     468             :   mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+     469             :   ros::ServiceServer                                       service_server_start_trajectory_tracking_;
+     470             :   ros::ServiceServer                                       service_server_stop_trajectory_tracking_;
+     471             :   ros::ServiceServer                                       service_server_resume_trajectory_tracking_;
+     472             :   ros::ServiceServer                                       service_server_goto_trajectory_start_;
+     473             : 
+     474             :   // transform service servers
+     475             :   ros::ServiceServer service_server_transform_reference_;
+     476             :   ros::ServiceServer service_server_transform_pose_;
+     477             :   ros::ServiceServer service_server_transform_vector3_;
+     478             : 
+     479             :   // safety area services
+     480             :   ros::ServiceServer service_server_validate_reference_;
+     481             :   ros::ServiceServer service_server_validate_reference_2d_;
+     482             :   ros::ServiceServer service_server_validate_reference_list_;
+     483             : 
+     484             :   // bumper service servers
+     485             :   ros::ServiceServer service_server_bumper_enabler_;
+     486             :   ros::ServiceServer service_server_bumper_repulsion_enabler_;
+     487             : 
+     488             :   // service clients
+     489             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+     490             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+     491             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+     492             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+     493             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+     494             : 
+     495             :   // safety area min z servers
+     496             :   ros::ServiceServer service_server_set_min_z_;
+     497             :   ros::ServiceServer service_server_get_min_z_;
+     498             : 
+     499             :   // | --------- trackers' and controllers' last results -------- |
+     500             : 
+     501             :   // the last result of an active tracker
+     502             :   std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+     503             :   std::mutex                              mutex_last_tracker_cmd_;
+     504             : 
+     505             :   // the last result of an active controller
+     506             :   Controller::ControlOutput last_control_output_;
+     507             :   std::mutex                mutex_last_control_output_;
+     508             : 
+     509             :   // | -------------- HW API diagnostics subscriber ------------- |
+     510             : 
+     511             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+     512             : 
+     513             :   bool offboard_mode_          = false;
+     514             :   bool offboard_mode_was_true_ = false;  // if it was ever true
+     515             :   bool armed_                  = false;
+     516             : 
+     517             :   // | -------------------- throttle and mass ------------------- |
+     518             : 
+     519             :   // throttle mass estimation during eland
+     520             :   double    throttle_mass_estimate_   = 0;
+     521             :   bool      throttle_under_threshold_ = false;
+     522             :   ros::Time throttle_mass_estimate_first_time_;
+     523             : 
+     524             :   // | ---------------------- safety params --------------------- |
+     525             : 
+     526             :   // failsafe when tilt error is too large
+     527             :   bool   _tilt_error_disarm_enabled_;
+     528             :   double _tilt_error_disarm_timeout_;
+     529             :   double _tilt_error_disarm_threshold_;
+     530             : 
+     531             :   ros::Time tilt_error_disarm_time_;
+     532             :   bool      tilt_error_disarm_over_thr_ = false;
+     533             : 
+     534             :   // elanding when tilt error is too large
+     535             :   bool   _tilt_limit_eland_enabled_;
+     536             :   double _tilt_limit_eland_ = 0;  // [rad]
+     537             : 
+     538             :   // disarming when tilt error is too large
+     539             :   bool   _tilt_limit_disarm_enabled_;
+     540             :   double _tilt_limit_disarm_ = 0;  // [rad]
+     541             : 
+     542             :   // elanding when yaw error is too large
+     543             :   bool   _yaw_error_eland_enabled_;
+     544             :   double _yaw_error_eland_ = 0;  // [rad]
+     545             : 
+     546             :   // keeping track of control errors
+     547             :   std::optional<double> tilt_error_ = 0;
+     548             :   std::optional<double> yaw_error_  = 0;
+     549             :   std::mutex            mutex_attitude_error_;
+     550             : 
+     551             :   std::optional<Eigen::Vector3d> position_error_;
+     552             :   std::mutex                     mutex_position_error_;
+     553             : 
+     554             :   // control error for triggering failsafe, eland, etc.
+     555             :   // this filled with the current controllers failsafe threshold
+     556             :   double _failsafe_threshold_                = 0;  // control error for triggering failsafe
+     557             :   double _eland_threshold_                   = 0;  // control error for triggering eland
+     558             :   bool   _odometry_innovation_check_enabled_ = false;
+     559             :   double _odometry_innovation_threshold_     = 0;  // innovation size for triggering eland
+     560             : 
+     561             :   bool callbacks_enabled_ = true;
+     562             : 
+     563             :   // | ------------------------ parachute ----------------------- |
+     564             : 
+     565             :   bool _parachute_enabled_ = false;
+     566             : 
+     567             :   std::tuple<bool, std::string> deployParachute(void);
+     568             :   bool                          parachuteSrv(void);
+     569             : 
+     570             :   // | ----------------------- safety area ---------------------- |
+     571             : 
+     572             :   // safety area
+     573             :   std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+     574             : 
+     575             :   std::atomic<bool> use_safety_area_ = false;
+     576             : 
+     577             :   std::string _safety_area_horizontal_frame_;
+     578             :   std::string _safety_area_vertical_frame_;
+     579             : 
+     580             :   double _safety_area_min_z_ = 0;
+     581             :   double _safety_area_max_z_ = 0;
+     582             : 
+     583             :   // safety area routines
+     584             :   // those are passed to trackers using the common_handlers object
+     585             :   bool   isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+     586             :   bool   isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+     587             :   bool   isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     588             :   bool   isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     589             :   double getMinZ(const std::string& frame_id);
+     590             :   double getMaxZ(const std::string& frame_id);
+     591             : 
+     592             :   // | ------------------------ callbacks ----------------------- |
+     593             : 
+     594             :   // topic callbacks
+     595             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     596             :   void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     597             :   void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     598             :   void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     599             :   void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+     600             : 
+     601             :   // topic timeouts
+     602             :   void timeoutUavState(const double& missing_for);
+     603             : 
+     604             :   // switching controller and tracker services
+     605             :   bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     606             :   bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     607             :   bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     608             : 
+     609             :   // reference callbacks
+     610             :   void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+     611             :   void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+     612             :   void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+     613             :   bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     614             :   bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     615             :   bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     616             :   bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     617             :   bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     618             :   bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     619             :   bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     620             :   bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+     621             :   bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+     622             :   bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     623             : 
+     624             :   // safety callbacks
+     625             :   bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     626             :   bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     627             :   bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     628             :   bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     629             :   bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     630             :   bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     631             :   bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     632             :   bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     633             :   bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     634             :   bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     635             :   bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     636             :   bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     637             :   bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     638             :   bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     639             :   bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     640             :   bool callbackBumperSetParams(mrs_msgs::BumperParamsSrv::Request& req, mrs_msgs::BumperParamsSrv::Response& res);
+     641             : 
+     642             :   bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+     643             : 
+     644             :   bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     645             :   bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     646             :   bool callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res);
+     647             : 
+     648             :   // transformation callbacks
+     649             :   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+     650             :   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+     651             :   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+     652             : 
+     653             :   // | ----------------------- constraints ---------------------- |
+     654             : 
+     655             :   // sets constraints to all trackers
+     656             :   bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+     657             : 
+     658             :   // constraints management
+     659             :   bool              got_constraints_ = false;
+     660             :   std::mutex        mutex_constraints_;
+     661             :   void              setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     662             :   void              setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     663             :   void              setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     664             :   std::atomic<bool> constraints_being_enforced_ = false;
+     665             : 
+     666             :   std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     667             : 
+     668             :   mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+     669             :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+     670             : 
+     671             :   // | ------------------ emergency triggered? ------------------ |
+     672             : 
+     673             :   bool failsafe_triggered_ = false;
+     674             :   bool eland_triggered_    = false;
+     675             : 
+     676             :   // | ------------------------- timers ------------------------- |
+     677             : 
+     678             :   // timer for regular status publishing
+     679             :   ros::Timer timer_status_;
+     680             :   void       timerStatus(const ros::TimerEvent& event);
+     681             : 
+     682             :   // timer for issuing the failsafe landing
+     683             :   ros::Timer timer_failsafe_;
+     684             :   void       timerFailsafe(const ros::TimerEvent& event);
+     685             : 
+     686             :   // oneshot timer for running controllers and trackers
+     687             :   void              asyncControl(void);
+     688             :   std::atomic<bool> running_async_control_ = false;
+     689             :   std::future<void> async_control_result_;
+     690             : 
+     691             :   // timer for issuing emergancy landing
+     692             :   ros::Timer timer_eland_;
+     693             :   void       timerEland(const ros::TimerEvent& event);
+     694             : 
+     695             :   // timer for regular checking of controller errors
+     696             :   ros::Timer        timer_safety_;
+     697             :   void              timerSafety(const ros::TimerEvent& event);
+     698             :   std::atomic<bool> running_safety_timer_        = false;
+     699             :   std::atomic<bool> odometry_switch_in_progress_ = false;
+     700             : 
+     701             :   // timer for issuing the pirouette
+     702             :   ros::Timer timer_pirouette_;
+     703             :   void       timerPirouette(const ros::TimerEvent& event);
+     704             : 
+     705             :   // | --------------------- obstacle bumper -------------------- |
+     706             : 
+     707             :   // bumper timer
+     708             :   ros::Timer timer_bumper_;
+     709             :   void       timerBumper(const ros::TimerEvent& event);
+     710             : 
+     711             :   // bumper subscriber
+     712             :   mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+     713             : 
+     714             :   bool        _bumper_switch_tracker_    = false;
+     715             :   bool        _bumper_switch_controller_ = false;
+     716             :   std::string _bumper_tracker_name_;
+     717             :   std::string _bumper_controller_name_;
+     718             :   std::string bumper_previous_tracker_;
+     719             :   std::string bumper_previous_controller_;
+     720             : 
+     721             :   std::atomic<bool> bumper_enabled_ = false;
+     722             :   std::atomic<bool> repulsing_      = false;
+     723             : 
+     724             :   double _bumper_horizontal_distance_ = 0;
+     725             :   double _bumper_vertical_distance_   = 0;
+     726             : 
+     727             :   double _bumper_horizontal_overshoot_ = 0;
+     728             :   double _bumper_vertical_overshoot_   = 0;
+     729             : 
+     730             :   int  bumperGetSectorId(const double& x, const double& y, const double& z);
+     731             :   bool bumperPushFromObstacle(void);
+     732             : 
+     733             :   // | --------------- safety checks and failsafes -------------- |
+     734             : 
+     735             :   // escalating failsafe (eland -> failsafe -> disarm)
+     736             :   bool                       _service_escalating_failsafe_enabled_ = false;
+     737             :   bool                       _rc_escalating_failsafe_enabled_      = false;
+     738             :   double                     _escalating_failsafe_timeout_         = 0;
+     739             :   ros::Time                  escalating_failsafe_time_;
+     740             :   bool                       _escalating_failsafe_ehover_   = false;
+     741             :   bool                       _escalating_failsafe_eland_    = false;
+     742             :   bool                       _escalating_failsafe_failsafe_ = false;
+     743             :   double                     _rc_escalating_failsafe_threshold_;
+     744             :   int                        _rc_escalating_failsafe_channel_  = 0;
+     745             :   bool                       rc_escalating_failsafe_triggered_ = false;
+     746             :   EscalatingFailsafeStates_t state_escalating_failsafe_        = ESC_NONE_STATE;
+     747             : 
+     748             :   std::string _tracker_error_action_;
+     749             : 
+     750             :   // emergancy landing state machine
+     751             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     752             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     753             :   std::mutex      mutex_landing_state_machine_;
+     754             :   void            changeLandingState(LandingStates_t new_state);
+     755             :   double          _uav_mass_ = 0;
+     756             :   double          _elanding_cutoff_mass_factor_;
+     757             :   double          _elanding_cutoff_timeout_;
+     758             :   double          landing_uav_mass_ = 0;
+     759             : 
+     760             :   // initial body disturbance loaded from params
+     761             :   double _initial_body_disturbance_x_ = 0;
+     762             :   double _initial_body_disturbance_y_ = 0;
+     763             : 
+     764             :   // profiling
+     765             :   mrs_lib::Profiler profiler_;
+     766             :   bool              _profiler_enabled_ = false;
+     767             : 
+     768             :   // automatic pc shutdown (DARPA specific)
+     769             :   bool _automatic_pc_shutdown_enabled_ = false;
+     770             : 
+     771             :   // diagnostics publishing
+     772             :   void       publishDiagnostics(void);
+     773             :   std::mutex mutex_diagnostics_;
+     774             : 
+     775             :   void                                             ungripSrv(void);
+     776             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+     777             : 
+     778             :   bool isFlyingNormally(void);
+     779             : 
+     780             :   // | ------------------------ pirouette ----------------------- |
+     781             : 
+     782             :   bool       _pirouette_enabled_ = false;
+     783             :   double     _pirouette_speed_;
+     784             :   double     _pirouette_timer_rate_;
+     785             :   std::mutex mutex_pirouette_;
+     786             :   double     pirouette_initial_heading_;
+     787             :   double     pirouette_iterator_;
+     788             :   bool       callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     789             : 
+     790             :   // | -------------------- joystick control -------------------- |
+     791             : 
+     792             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     793             : 
+     794             :   void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+     795             :   bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     796             : 
+     797             :   // joystick buttons mappings
+     798             :   int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+     799             : 
+     800             :   // channel numbers and channel multipliers
+     801             :   int    _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+     802             :   double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+     803             : 
+     804             :   ros::Timer timer_joystick_;
+     805             :   void       timerJoystick(const ros::TimerEvent& event);
+     806             :   double     _joystick_timer_rate_ = 0;
+     807             : 
+     808             :   double _joystick_carrot_distance_ = 0;
+     809             : 
+     810             :   ros::Time joystick_start_press_time_;
+     811             :   bool      joystick_start_pressed_ = false;
+     812             : 
+     813             :   ros::Time joystick_back_press_time_;
+     814             :   bool      joystick_back_pressed_ = false;
+     815             :   bool      joystick_goto_enabled_ = false;
+     816             : 
+     817             :   bool      joystick_failsafe_pressed_ = false;
+     818             :   ros::Time joystick_failsafe_press_time_;
+     819             : 
+     820             :   bool      joystick_eland_pressed_ = false;
+     821             :   ros::Time joystick_eland_press_time_;
+     822             : 
+     823             :   // | ------------------- RC joystick control ------------------ |
+     824             : 
+     825             :   // listening to the RC channels as told by pixhawk
+     826             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+     827             : 
+     828             :   // the RC channel mapping of the main 4 control signals
+     829             :   double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+     830             : 
+     831             :   bool              _rc_goto_enabled_               = false;
+     832             :   std::atomic<bool> rc_goto_active_                 = false;
+     833             :   double            rc_joystick_channel_last_value_ = 0.5;
+     834             :   bool              rc_joystick_channel_was_low_    = false;
+     835             :   int               _rc_joystick_channel_           = 0;
+     836             : 
+     837             :   double _rc_horizontal_speed_ = 0;
+     838             :   double _rc_vertical_speed_   = 0;
+     839             :   double _rc_heading_rate_     = 0;
+     840             : 
+     841             :   // | ------------------- trajectory loading ------------------- |
+     842             : 
+     843             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_original_trajectory_poses_;
+     844             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+     845             : 
+     846             :   // | --------------------- other routines --------------------- |
+     847             : 
+     848             :   // this is called to update the trackers and to receive position control command from the active one
+     849             :   void updateTrackers(void);
+     850             : 
+     851             :   // this is called to update the controllers and to receive attitude control command from the active one
+     852             :   void updateControllers(const mrs_msgs::UavState& uav_state);
+     853             : 
+     854             :   // sets the reference to the active tracker
+     855             :   std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+     856             : 
+     857             :   // sets the velocity reference to the active tracker
+     858             :   std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+     859             : 
+     860             :   // sets the reference trajectory to the active tracker
+     861             :   std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+     862             :       const mrs_msgs::TrajectoryReference trajectory_in);
+     863             : 
+     864             :   // publishes
+     865             :   void publish(void);
+     866             : 
+     867             :   bool loadConfigFile(const std::string& file_path, const std::string ns);
+     868             : 
+     869             :   double getMass(void);
+     870             : 
+     871             :   // publishes rviz-visualizable control reference
+     872             :   void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+     873             : 
+     874             :   void initializeControlOutput(void);
+     875             : 
+     876             :   // tell the mrs_odometry to disable its callbacks
+     877             :   void odometryCallbacksSrv(const bool input);
+     878             : 
+     879             :   mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+     880             : 
+     881             :   void                          shutdown();
+     882             :   void                          setCallbacks(bool in);
+     883             :   bool                          isOffboard(void);
+     884             :   bool                          elandSrv(void);
+     885             :   std::tuple<bool, std::string> arming(const bool input);
+     886             : 
+     887             :   // safety functions impl
+     888             :   std::tuple<bool, std::string> ehover(void);
+     889             :   std::tuple<bool, std::string> hover(void);
+     890             :   std::tuple<bool, std::string> startTrajectoryTracking(void);
+     891             :   std::tuple<bool, std::string> stopTrajectoryTracking(void);
+     892             :   std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+     893             :   std::tuple<bool, std::string> gotoTrajectoryStart(void);
+     894             :   std::tuple<bool, std::string> eland(void);
+     895             :   std::tuple<bool, std::string> failsafe(void);
+     896             :   std::tuple<bool, std::string> escalatingFailsafe(void);
+     897             : 
+     898             :   EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+     899             : };
+     900             : 
+     901             : //}
+     902             : 
+     903             : /* //{ onInit() */
+     904             : 
+     905           7 : void ControlManager::onInit() {
+     906           7 :   preinitialize();
+     907           7 : }
+     908             : 
+     909             : //}
+     910             : 
+     911             : /* preinitialize() //{ */
+     912             : 
+     913           7 : void ControlManager::preinitialize(void) {
+     914             : 
+     915           7 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     916             : 
+     917           7 :   ros::Time::waitForValid();
+     918             : 
+     919           7 :   mrs_lib::SubscribeHandlerOptions shopts;
+     920           7 :   shopts.nh                 = nh_;
+     921           7 :   shopts.node_name          = "ControlManager";
+     922           7 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     923           7 :   shopts.threadsafe         = true;
+     924           7 :   shopts.autostart          = true;
+     925           7 :   shopts.queue_size         = 10;
+     926           7 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     927             : 
+     928           7 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     929             : 
+     930           7 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+     931           7 : }
+     932             : 
+     933             : //}
+     934             : 
+     935             : /* initialize() //{ */
+     936             : 
+     937           7 : void ControlManager::initialize(void) {
+     938             : 
+     939           7 :   joystick_start_press_time_      = ros::Time(0);
+     940           7 :   joystick_failsafe_press_time_   = ros::Time(0);
+     941           7 :   joystick_eland_press_time_      = ros::Time(0);
+     942           7 :   escalating_failsafe_time_       = ros::Time(0);
+     943           7 :   controller_tracker_switch_time_ = ros::Time(0);
+     944             : 
+     945           7 :   ROS_INFO("[ControlManager]: initializing");
+     946             : 
+     947             :   // --------------------------------------------------------------
+     948             :   // |         common handler for trackers and controllers        |
+     949             :   // --------------------------------------------------------------
+     950             : 
+     951           7 :   common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+     952             : 
+     953             :   // --------------------------------------------------------------
+     954             :   // |                           params                           |
+     955             :   // --------------------------------------------------------------
+     956             : 
+     957          14 :   mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+     958             : 
+     959           7 :   param_loader.loadParam("custom_config", _custom_config_);
+     960           7 :   param_loader.loadParam("platform_config", _platform_config_);
+     961           7 :   param_loader.loadParam("world_config", _world_config_);
+     962           7 :   param_loader.loadParam("network_config", _network_config_);
+     963             : 
+     964           7 :   if (_custom_config_ != "") {
+     965           7 :     param_loader.addYamlFile(_custom_config_);
+     966             :   }
+     967             : 
+     968           7 :   if (_platform_config_ != "") {
+     969           7 :     param_loader.addYamlFile(_platform_config_);
+     970             :   }
+     971             : 
+     972           7 :   if (_world_config_ != "") {
+     973           7 :     param_loader.addYamlFile(_world_config_);
+     974             :   }
+     975             : 
+     976           7 :   if (_network_config_ != "") {
+     977           7 :     param_loader.addYamlFile(_network_config_);
+     978             :   }
+     979             : 
+     980           7 :   param_loader.addYamlFileFromParam("private_config");
+     981           7 :   param_loader.addYamlFileFromParam("public_config");
+     982           7 :   param_loader.addYamlFileFromParam("private_trackers");
+     983           7 :   param_loader.addYamlFileFromParam("private_controllers");
+     984           7 :   param_loader.addYamlFileFromParam("public_controllers");
+     985             : 
+     986             :   // params passed from the launch file are not prefixed
+     987           7 :   param_loader.loadParam("uav_name", _uav_name_);
+     988           7 :   param_loader.loadParam("body_frame", _body_frame_);
+     989           7 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     990           7 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     991           7 :   param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+     992           7 :   param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+     993           7 :   param_loader.loadParam("g", common_handlers_->g);
+     994             : 
+     995             :   // motor params are also not prefixed, since they are common to more nodes
+     996           7 :   param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+     997           7 :   param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+     998           7 :   param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+     999             : 
+    1000             :   // | ----------------------- safety area ---------------------- |
+    1001             : 
+    1002             :   bool use_safety_area;
+    1003           7 :   param_loader.loadParam("safety_area/enabled", use_safety_area);
+    1004           7 :   use_safety_area_ = use_safety_area;
+    1005             : 
+    1006           7 :   param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+    1007             : 
+    1008           7 :   param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+    1009           7 :   param_loader.loadParam("safety_area/vertical/min_z", _safety_area_min_z_);
+    1010           7 :   param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+    1011             : 
+    1012           7 :   if (use_safety_area_) {
+    1013             : 
+    1014          12 :     Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+    1015             : 
+    1016             :     try {
+    1017             : 
+    1018           8 :       std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+    1019           4 :       std::vector<Eigen::MatrixXd> point_obstacle_points;
+    1020             : 
+    1021           4 :       safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+    1022             :     }
+    1023             : 
+    1024           0 :     catch (mrs_lib::SafetyZone::BorderError& e) {
+    1025           0 :       ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+    1026           0 :       ros::shutdown();
+    1027             :     }
+    1028           0 :     catch (...) {
+    1029           0 :       ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+    1030           0 :       ros::shutdown();
+    1031             :     }
+    1032             : 
+    1033           4 :     ROS_INFO("[ControlManager]: safety area initialized");
+    1034             :   }
+    1035             : 
+    1036           7 :   param_loader.setPrefix("mrs_uav_managers/control_manager/");
+    1037             : 
+    1038           7 :   param_loader.loadParam("state_input", _state_input_);
+    1039             : 
+    1040           7 :   if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+    1041           0 :     ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+    1042           0 :     ros::shutdown();
+    1043             :   }
+    1044             : 
+    1045           7 :   param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+    1046           7 :   param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+    1047           7 :   param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+    1048             : 
+    1049           7 :   param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+    1050           7 :   param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+    1051           7 :   param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+    1052           7 :   param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+    1053           7 :   param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+    1054             : 
+    1055           7 :   param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+    1056           7 :   param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+    1057           7 :   param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+    1058           7 :   param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+    1059           7 :   param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+    1060           7 :   param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+    1061           7 :   param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+    1062           7 :   param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+    1063             : 
+    1064           7 :   param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+    1065           7 :   param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+    1066             : 
+    1067           7 :   _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+    1068             : 
+    1069           7 :   if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+    1070           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+    1071           0 :     ros::shutdown();
+    1072             :   }
+    1073             : 
+    1074           7 :   param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+    1075           7 :   param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+    1076             : 
+    1077           7 :   _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+    1078             : 
+    1079           7 :   if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+    1080           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+    1081           0 :     ros::shutdown();
+    1082             :   }
+    1083             : 
+    1084           7 :   param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+    1085           7 :   param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+    1086             : 
+    1087           7 :   _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+    1088             : 
+    1089           7 :   if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+    1090           0 :     ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+    1091           0 :     ros::shutdown();
+    1092             :   }
+    1093             : 
+    1094           7 :   param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+    1095           7 :   param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+    1096           7 :   param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+    1097           7 :   param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+    1098             : 
+    1099           7 :   param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+    1100           7 :   param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+    1101             : 
+    1102           7 :   param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+    1103           7 :   param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+    1104           7 :   param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+    1105             : 
+    1106           7 :   _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+    1107             : 
+    1108           7 :   if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+    1109           0 :     ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+    1110           0 :     ros::shutdown();
+    1111             :   }
+    1112             : 
+    1113             :   // default constraints
+    1114             : 
+    1115           7 :   param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+    1116           7 :   param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+    1117           7 :   param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+    1118           7 :   param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+    1119             : 
+    1120           7 :   param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+    1121           7 :   param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+    1122           7 :   param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+    1123           7 :   param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+    1124             : 
+    1125           7 :   param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+    1126           7 :   param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+    1127           7 :   param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+    1128           7 :   param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+    1129             : 
+    1130           7 :   param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+    1131           7 :   param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+    1132           7 :   param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+    1133           7 :   param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+    1134             : 
+    1135           7 :   param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+    1136           7 :   param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+    1137           7 :   param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+    1138             : 
+    1139           7 :   param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+    1140             : 
+    1141           7 :   current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+    1142             : 
+    1143             :   // joystick
+    1144             : 
+    1145           7 :   param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+    1146           7 :   param_loader.loadParam("joystick/mode", _joystick_mode_);
+    1147           7 :   param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+    1148           7 :   param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+    1149           7 :   param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+    1150           7 :   param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+    1151           7 :   param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+    1152           7 :   param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+    1153             : 
+    1154           7 :   param_loader.loadParam("joystick/channels/A", _channel_A_);
+    1155           7 :   param_loader.loadParam("joystick/channels/B", _channel_B_);
+    1156           7 :   param_loader.loadParam("joystick/channels/X", _channel_X_);
+    1157           7 :   param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+    1158           7 :   param_loader.loadParam("joystick/channels/start", _channel_start_);
+    1159           7 :   param_loader.loadParam("joystick/channels/back", _channel_back_);
+    1160           7 :   param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+    1161           7 :   param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+    1162           7 :   param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+    1163           7 :   param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+    1164             : 
+    1165             :   // load channels
+    1166           7 :   param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+    1167           7 :   param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+    1168           7 :   param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+    1169           7 :   param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+    1170             : 
+    1171             :   // load channel multipliers
+    1172           7 :   param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+    1173           7 :   param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+    1174           7 :   param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+    1175           7 :   param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+    1176             : 
+    1177             :   bool bumper_enabled;
+    1178           7 :   param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+    1179           7 :   bumper_enabled_ = bumper_enabled;
+    1180             : 
+    1181           7 :   param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+    1182           7 :   param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+    1183           7 :   param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+    1184           7 :   param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+    1185           7 :   param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+    1186             : 
+    1187           7 :   param_loader.loadParam("obstacle_bumper/horizontal/threshold_distance", _bumper_horizontal_distance_);
+    1188           7 :   param_loader.loadParam("obstacle_bumper/vertical/threshold_distance", _bumper_vertical_distance_);
+    1189             : 
+    1190           7 :   param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+    1191           7 :   param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+    1192             : 
+    1193           7 :   param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+    1194             : 
+    1195           7 :   param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+    1196             : 
+    1197             :   // check the values of tracker error action
+    1198           7 :   if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+    1199           0 :     ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+    1200             :               EHOVER_STR);
+    1201           0 :     ros::shutdown();
+    1202             :   }
+    1203             : 
+    1204           7 :   param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+    1205           7 :   param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+    1206           7 :   param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+    1207           7 :   param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+    1208           7 :   param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+    1209             : 
+    1210           7 :   param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+    1211           7 :   param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+    1212           7 :   param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+    1213           7 :   param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+    1214             : 
+    1215           7 :   param_loader.loadParam("automatic_pc_shutdown/enabled", _automatic_pc_shutdown_enabled_);
+    1216             : 
+    1217           7 :   param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+    1218           7 :   param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+    1219             : 
+    1220           7 :   param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+    1221             : 
+    1222             :   // --------------------------------------------------------------
+    1223             :   // |             initialize the last control output             |
+    1224             :   // --------------------------------------------------------------
+    1225             : 
+    1226           7 :   initializeControlOutput();
+    1227             : 
+    1228             :   // | --------------------- tf transformer --------------------- |
+    1229             : 
+    1230           7 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+    1231           7 :   transformer_->setDefaultPrefix(_uav_name_);
+    1232           7 :   transformer_->retryLookupNewest(true);
+    1233             : 
+    1234             :   // | ------------------- scope timer logger ------------------- |
+    1235             : 
+    1236           7 :   param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+    1237          21 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+    1238           7 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+    1239             : 
+    1240             :   // bind transformer to trackers and controllers for use
+    1241           7 :   common_handlers_->transformer = transformer_;
+    1242             : 
+    1243             :   // bind scope timer to trackers and controllers for use
+    1244           7 :   common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+    1245           7 :   common_handlers_->scope_timer.logger  = scope_timer_logger_;
+    1246             : 
+    1247           7 :   common_handlers_->safety_area.use_safety_area       = use_safety_area_;
+    1248           7 :   common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+    1249           7 :   common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+    1250           7 :   common_handlers_->safety_area.getMinZ               = boost::bind(&ControlManager::getMinZ, this, _1);
+    1251           7 :   common_handlers_->safety_area.getMaxZ               = boost::bind(&ControlManager::getMaxZ, this, _1);
+    1252             : 
+    1253           7 :   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+    1254             : 
+    1255           7 :   common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+    1256             : 
+    1257           7 :   common_handlers_->control_output_modalities = _hw_api_inputs_;
+    1258             : 
+    1259           7 :   common_handlers_->uav_name = _uav_name_;
+    1260             : 
+    1261           7 :   common_handlers_->parent_nh = nh_;
+    1262             : 
+    1263             :   // --------------------------------------------------------------
+    1264             :   // |                        load trackers                       |
+    1265             :   // --------------------------------------------------------------
+    1266             : 
+    1267          14 :   std::vector<std::string> custom_trackers;
+    1268             : 
+    1269           7 :   param_loader.loadParam("mrs_trackers", _tracker_names_);
+    1270           7 :   param_loader.loadParam("trackers", custom_trackers);
+    1271             : 
+    1272           7 :   if (!custom_trackers.empty()) {
+    1273           0 :     _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+    1274             :   }
+    1275             : 
+    1276           7 :   param_loader.loadParam("null_tracker", _null_tracker_name_);
+    1277           7 :   param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+    1278             : 
+    1279           7 :   tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+    1280             : 
+    1281          49 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    1282             : 
+    1283          84 :     std::string tracker_name = _tracker_names_[i];
+    1284             : 
+    1285             :     // load the controller parameters
+    1286          84 :     std::string address;
+    1287          84 :     std::string name_space;
+    1288             :     bool        human_switchable;
+    1289             : 
+    1290          42 :     param_loader.loadParam(tracker_name + "/address", address);
+    1291          42 :     param_loader.loadParam(tracker_name + "/namespace", name_space);
+    1292          42 :     param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+    1293             : 
+    1294         126 :     TrackerParams new_tracker(address, name_space, human_switchable);
+    1295          42 :     trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+    1296             : 
+    1297             :     try {
+    1298          42 :       ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+    1299          42 :       tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+    1300             :     }
+    1301           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1302           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+    1303           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1304           0 :       ros::shutdown();
+    1305             :     }
+    1306           0 :     catch (pluginlib::PluginlibException& ex) {
+    1307           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+    1308           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1309           0 :       ros::shutdown();
+    1310             :     }
+    1311             :   }
+    1312             : 
+    1313           7 :   ROS_INFO("[ControlManager]: trackers were loaded");
+    1314             : 
+    1315          49 :   for (int i = 0; i < int(tracker_list_.size()); i++) {
+    1316             : 
+    1317          42 :     std::map<std::string, TrackerParams>::iterator it;
+    1318          42 :     it = trackers_.find(_tracker_names_[i]);
+    1319             : 
+    1320             :     // create private handlers
+    1321             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1322          84 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1323             : 
+    1324          42 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1325          42 :     private_handlers->name_space     = it->second.name_space;
+    1326          42 :     private_handlers->runtime_name   = _tracker_names_[i];
+    1327          42 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_[i]);
+    1328             : 
+    1329          42 :     if (_custom_config_ != "") {
+    1330          42 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1331             :     }
+    1332             : 
+    1333          42 :     if (_platform_config_ != "") {
+    1334          42 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1335             :     }
+    1336             : 
+    1337          42 :     if (_world_config_ != "") {
+    1338          42 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1339             :     }
+    1340             : 
+    1341          42 :     if (_network_config_ != "") {
+    1342          42 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1343             :     }
+    1344             : 
+    1345          42 :     bool success = false;
+    1346             : 
+    1347             :     try {
+    1348          42 :       ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+    1349          42 :       success = tracker_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1350             :     }
+    1351           0 :     catch (std::runtime_error& ex) {
+    1352           0 :       ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+    1353             :     }
+    1354             : 
+    1355          42 :     if (!success) {
+    1356           0 :       ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+    1357           0 :       ros::shutdown();
+    1358             :     }
+    1359             :   }
+    1360             : 
+    1361           7 :   ROS_INFO("[ControlManager]: trackers were initialized");
+    1362             : 
+    1363             :   // --------------------------------------------------------------
+    1364             :   // |           check the existance of selected trackers         |
+    1365             :   // --------------------------------------------------------------
+    1366             : 
+    1367             :   // | ------ check for the existance of the hover tracker ------ |
+    1368             : 
+    1369             :   // check if the hover_tracker is within the loaded trackers
+    1370             :   {
+    1371           7 :     auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+    1372             : 
+    1373           7 :     if (idx) {
+    1374           7 :       _ehover_tracker_idx_ = idx.value();
+    1375             :     } else {
+    1376           0 :       ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+    1377           0 :       ros::shutdown();
+    1378             :     }
+    1379             :   }
+    1380             : 
+    1381             :   // | ----- check for the existence of the landoff tracker ----- |
+    1382             : 
+    1383             :   {
+    1384           7 :     auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+    1385             : 
+    1386           7 :     if (idx) {
+    1387           7 :       _landoff_tracker_idx_ = idx.value();
+    1388             :     } else {
+    1389           0 :       ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+    1390           0 :       ros::shutdown();
+    1391             :     }
+    1392             :   }
+    1393             : 
+    1394             :   // | ------- check for the existence of the null tracker ------ |
+    1395             : 
+    1396             :   {
+    1397           7 :     auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+    1398             : 
+    1399           7 :     if (idx) {
+    1400           7 :       _null_tracker_idx_ = idx.value();
+    1401             :     } else {
+    1402           0 :       ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+    1403           0 :       ros::shutdown();
+    1404             :     }
+    1405             :   }
+    1406             : 
+    1407             :   // --------------------------------------------------------------
+    1408             :   // |         check existance of trackers for joystick           |
+    1409             :   // --------------------------------------------------------------
+    1410             : 
+    1411           7 :   if (_joystick_enabled_) {
+    1412             : 
+    1413           7 :     auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+    1414             : 
+    1415           7 :     if (idx) {
+    1416           7 :       _joystick_tracker_idx_ = idx.value();
+    1417             :     } else {
+    1418           0 :       ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+    1419           0 :       ros::shutdown();
+    1420             :     }
+    1421             :   }
+    1422             : 
+    1423           7 :   if (_bumper_switch_tracker_) {
+    1424             : 
+    1425           7 :     auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+    1426             : 
+    1427           7 :     if (!idx) {
+    1428           0 :       ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+    1429           0 :       ros::shutdown();
+    1430             :     }
+    1431             :   }
+    1432             : 
+    1433             :   {
+    1434           7 :     auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+    1435             : 
+    1436           7 :     if (idx) {
+    1437           7 :       _joystick_fallback_tracker_idx_ = idx.value();
+    1438             :     } else {
+    1439           0 :       ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+    1440           0 :       ros::shutdown();
+    1441             :     }
+    1442             :   }
+    1443             : 
+    1444             :   // --------------------------------------------------------------
+    1445             :   // |                    load the controllers                    |
+    1446             :   // --------------------------------------------------------------
+    1447             : 
+    1448          14 :   std::vector<std::string> custom_controllers;
+    1449             : 
+    1450           7 :   param_loader.loadParam("mrs_controllers", _controller_names_);
+    1451           7 :   param_loader.loadParam("controllers", custom_controllers);
+    1452             : 
+    1453           7 :   if (!custom_controllers.empty()) {
+    1454           0 :     _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+    1455             :   }
+    1456             : 
+    1457           7 :   controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+    1458             : 
+    1459             :   // for each controller in the list
+    1460          42 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    1461             : 
+    1462          70 :     std::string controller_name = _controller_names_[i];
+    1463             : 
+    1464             :     // load the controller parameters
+    1465          70 :     std::string address;
+    1466          70 :     std::string name_space;
+    1467             :     double      eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+    1468             :     bool        human_switchable;
+    1469          35 :     param_loader.loadParam(controller_name + "/address", address);
+    1470          35 :     param_loader.loadParam(controller_name + "/namespace", name_space);
+    1471          35 :     param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+    1472          35 :     param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+    1473          35 :     param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+    1474          35 :     param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+    1475             : 
+    1476             :     // check if the controller can output some of the required outputs
+    1477             :     {
+    1478             : 
+    1479          35 :       ControlOutputModalities_t outputs;
+    1480          35 :       param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+    1481          35 :       param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+    1482          35 :       param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+    1483          35 :       param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+    1484          35 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+    1485          35 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+    1486          35 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+    1487          35 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+    1488          35 :       param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+    1489             : 
+    1490          35 :       bool meets_actuators             = (_hw_api_inputs_.actuators && outputs.actuators);
+    1491          35 :       bool meets_control_group         = (_hw_api_inputs_.control_group && outputs.control_group);
+    1492          35 :       bool meets_attitude_rate         = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+    1493          35 :       bool meets_attitude              = (_hw_api_inputs_.attitude && outputs.attitude);
+    1494          35 :       bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+    1495          35 :       bool meets_acceleration_hdg      = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+    1496          35 :       bool meets_velocity_hdg_rate     = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+    1497          35 :       bool meets_velocity_hdg          = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+    1498          35 :       bool meets_position              = (_hw_api_inputs_.position && outputs.position);
+    1499             : 
+    1500          35 :       bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+    1501          70 :                                 meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+    1502             : 
+    1503          35 :       if (!meets_requirements) {
+    1504             : 
+    1505           0 :         ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+    1506             :                   controller_name.c_str());
+    1507             : 
+    1508           0 :         if (_hw_api_inputs_.actuators) {
+    1509           0 :           ROS_ERROR("[ControlManager]: - actuators");
+    1510             :         }
+    1511             : 
+    1512           0 :         if (_hw_api_inputs_.control_group) {
+    1513           0 :           ROS_ERROR("[ControlManager]: - control group");
+    1514             :         }
+    1515             : 
+    1516           0 :         if (_hw_api_inputs_.attitude_rate) {
+    1517           0 :           ROS_ERROR("[ControlManager]: - attitude rate");
+    1518             :         }
+    1519             : 
+    1520           0 :         if (_hw_api_inputs_.attitude) {
+    1521           0 :           ROS_ERROR("[ControlManager]: - attitude");
+    1522             :         }
+    1523             : 
+    1524           0 :         if (_hw_api_inputs_.acceleration_hdg_rate) {
+    1525           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+    1526             :         }
+    1527             : 
+    1528           0 :         if (_hw_api_inputs_.acceleration_hdg) {
+    1529           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg");
+    1530             :         }
+    1531             : 
+    1532           0 :         if (_hw_api_inputs_.velocity_hdg_rate) {
+    1533           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+    1534             :         }
+    1535             : 
+    1536           0 :         if (_hw_api_inputs_.velocity_hdg) {
+    1537           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg");
+    1538             :         }
+    1539             : 
+    1540           0 :         if (_hw_api_inputs_.position) {
+    1541           0 :           ROS_ERROR("[ControlManager]: - position");
+    1542             :         }
+    1543             : 
+    1544           0 :         ros::shutdown();
+    1545             :       }
+    1546             : 
+    1547          35 :       if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+    1548           0 :         ROS_ERROR(
+    1549             :             "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+    1550           0 :         ros::shutdown();
+    1551             :       }
+    1552             :     }
+    1553             : 
+    1554             :     // | --- alter the timer rates based on the hw capabilities --- |
+    1555             : 
+    1556          35 :     CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+    1557             : 
+    1558          35 :     if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+    1559           0 :       _safety_timer_rate_     = 200.0;
+    1560           0 :       desired_uav_state_rate_ = 250.0;
+    1561          35 :     } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+    1562          35 :       _safety_timer_rate_     = 100.0;
+    1563          35 :       desired_uav_state_rate_ = 100.0;
+    1564           0 :     } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+    1565           0 :       _safety_timer_rate_     = 30.0;
+    1566           0 :       _status_timer_rate_     = 1.0;
+    1567           0 :       desired_uav_state_rate_ = 40.0;
+    1568             : 
+    1569           0 :       if (_uav_state_max_missing_time_ < 0.2) {
+    1570           0 :         _uav_state_max_missing_time_ = 0.2;
+    1571             :       }
+    1572           0 :     } else if (lowest_output >= VELOCITY_HDG_RATE) {
+    1573           0 :       _safety_timer_rate_     = 20.0;
+    1574           0 :       _status_timer_rate_     = 1.0;
+    1575           0 :       desired_uav_state_rate_ = 20.0;
+    1576             : 
+    1577           0 :       if (_uav_state_max_missing_time_ < 1.0) {
+    1578           0 :         _uav_state_max_missing_time_ = 1.0;
+    1579             :       }
+    1580             :     }
+    1581             : 
+    1582          35 :     if (eland_threshold == 0) {
+    1583           8 :       eland_threshold = 1e6;
+    1584             :     }
+    1585             : 
+    1586          35 :     if (failsafe_threshold == 0) {
+    1587           8 :       failsafe_threshold = 1e6;
+    1588             :     }
+    1589             : 
+    1590          35 :     if (odometry_innovation_threshold == 0) {
+    1591           9 :       odometry_innovation_threshold = 1e6;
+    1592             :     }
+    1593             : 
+    1594         105 :     ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+    1595          35 :     controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+    1596             : 
+    1597             :     try {
+    1598          35 :       ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+    1599          35 :       controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+    1600             :     }
+    1601           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1602           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+    1603           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1604           0 :       ros::shutdown();
+    1605             :     }
+    1606           0 :     catch (pluginlib::PluginlibException& ex) {
+    1607           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+    1608           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1609           0 :       ros::shutdown();
+    1610             :     }
+    1611             :   }
+    1612             : 
+    1613           7 :   ROS_INFO("[ControlManager]: controllers were loaded");
+    1614             : 
+    1615          42 :   for (int i = 0; i < int(controller_list_.size()); i++) {
+    1616             : 
+    1617          35 :     std::map<std::string, ControllerParams>::iterator it;
+    1618          35 :     it = controllers_.find(_controller_names_[i]);
+    1619             : 
+    1620             :     // create private handlers
+    1621             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1622          70 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1623             : 
+    1624          35 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1625          35 :     private_handlers->name_space     = it->second.name_space;
+    1626          35 :     private_handlers->runtime_name   = _controller_names_[i];
+    1627          35 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_[i]);
+    1628             : 
+    1629          35 :     if (_custom_config_ != "") {
+    1630          35 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1631             :     }
+    1632             : 
+    1633          35 :     if (_platform_config_ != "") {
+    1634          35 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1635             :     }
+    1636             : 
+    1637          35 :     if (_world_config_ != "") {
+    1638          35 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1639             :     }
+    1640             : 
+    1641          35 :     if (_network_config_ != "") {
+    1642          35 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1643             :     }
+    1644             : 
+    1645          35 :     bool success = false;
+    1646             : 
+    1647             :     try {
+    1648             : 
+    1649          35 :       ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+    1650          35 :       success = controller_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1651             :     }
+    1652           0 :     catch (std::runtime_error& ex) {
+    1653           0 :       ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+    1654             :     }
+    1655             : 
+    1656          35 :     if (!success) {
+    1657           0 :       ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+    1658           0 :       ros::shutdown();
+    1659             :     }
+    1660             :   }
+    1661             : 
+    1662           7 :   ROS_INFO("[ControlManager]: controllers were initialized");
+    1663             : 
+    1664             :   {
+    1665           7 :     auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+    1666             : 
+    1667           7 :     if (idx) {
+    1668           7 :       _failsafe_controller_idx_ = idx.value();
+    1669             :     } else {
+    1670           0 :       ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+    1671           0 :       ros::shutdown();
+    1672             :     }
+    1673             :   }
+    1674             : 
+    1675             :   {
+    1676           7 :     auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+    1677             : 
+    1678           7 :     if (idx) {
+    1679           7 :       _eland_controller_idx_ = idx.value();
+    1680             :     } else {
+    1681           0 :       ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+    1682           0 :       ros::shutdown();
+    1683             :     }
+    1684             :   }
+    1685             : 
+    1686             :   {
+    1687           7 :     auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+    1688             : 
+    1689           7 :     if (idx) {
+    1690           7 :       _joystick_controller_idx_ = idx.value();
+    1691             :     } else {
+    1692           0 :       ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+    1693           0 :       ros::shutdown();
+    1694             :     }
+    1695             :   }
+    1696             : 
+    1697           7 :   if (_bumper_switch_controller_) {
+    1698             : 
+    1699           7 :     auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+    1700             : 
+    1701           7 :     if (!idx) {
+    1702           0 :       ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+    1703           0 :       ros::shutdown();
+    1704             :     }
+    1705             :   }
+    1706             : 
+    1707             :   {
+    1708           7 :     auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+    1709             : 
+    1710           7 :     if (idx) {
+    1711           7 :       _joystick_fallback_controller_idx_ = idx.value();
+    1712             :     } else {
+    1713           0 :       ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+    1714           0 :       ros::shutdown();
+    1715             :     }
+    1716             :   }
+    1717             : 
+    1718             :   // --------------------------------------------------------------
+    1719             :   // |                  activate the NullTracker                  |
+    1720             :   // --------------------------------------------------------------
+    1721             : 
+    1722           7 :   ROS_INFO("[ControlManager]: activating the null tracker");
+    1723             : 
+    1724           7 :   tracker_list_[_null_tracker_idx_]->activate(last_tracker_cmd_);
+    1725           7 :   active_tracker_idx_ = _null_tracker_idx_;
+    1726             : 
+    1727             :   // --------------------------------------------------------------
+    1728             :   // |    activate the eland controller as the first controller   |
+    1729             :   // --------------------------------------------------------------
+    1730             : 
+    1731           7 :   ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_[_eland_controller_idx_].c_str());
+    1732             : 
+    1733           7 :   controller_list_[_eland_controller_idx_]->activate(last_control_output_);
+    1734           7 :   active_controller_idx_ = _eland_controller_idx_;
+    1735             : 
+    1736             :   // update the time
+    1737             :   {
+    1738          14 :     std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    1739             : 
+    1740           7 :     controller_tracker_switch_time_ = ros::Time::now();
+    1741             :   }
+    1742             : 
+    1743           7 :   output_enabled_ = false;
+    1744             : 
+    1745             :   // | --------------- set the default constraints -------------- |
+    1746             : 
+    1747           7 :   sanitized_constraints_ = current_constraints_;
+    1748           7 :   setConstraints(current_constraints_);
+    1749             : 
+    1750             :   // | ------------------------ profiler ------------------------ |
+    1751             : 
+    1752           7 :   profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+    1753             : 
+    1754             :   // | ----------------------- publishers ----------------------- |
+    1755             : 
+    1756           7 :   control_output_publisher_ = OutputPublisher(nh_);
+    1757             : 
+    1758           7 :   ph_controller_diagnostics_             = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+    1759           7 :   ph_tracker_cmd_                        = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+    1760           7 :   ph_mrs_odom_input_                     = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+    1761           7 :   ph_control_reference_odom_             = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+    1762           7 :   ph_diagnostics_                        = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+    1763           7 :   ph_offboard_on_                        = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+    1764           7 :   ph_tilt_error_                         = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+    1765           7 :   ph_mass_estimate_                      = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+    1766           7 :   ph_throttle_                           = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+    1767           7 :   ph_thrust_                             = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+    1768           7 :   ph_control_error_                      = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+    1769           7 :   ph_safety_area_markers_                = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+    1770           7 :   ph_safety_area_coordinates_markers_    = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+    1771           7 :   ph_disturbances_markers_               = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+    1772           7 :   ph_bumper_status_                      = mrs_lib::PublisherHandler<mrs_msgs::BumperStatus>(nh_, "bumper_status_out", 1);
+    1773           7 :   ph_current_constraints_                = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+    1774           7 :   ph_heading_                            = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+    1775           7 :   ph_speed_                              = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+    1776           7 :   pub_debug_original_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+    1777           7 :   pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+    1778             : 
+    1779             :   // | ----------------------- subscribers ---------------------- |
+    1780             : 
+    1781          14 :   mrs_lib::SubscribeHandlerOptions shopts;
+    1782           7 :   shopts.nh                 = nh_;
+    1783           7 :   shopts.node_name          = "ControlManager";
+    1784           7 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+    1785           7 :   shopts.threadsafe         = true;
+    1786           7 :   shopts.autostart          = true;
+    1787           7 :   shopts.queue_size         = 10;
+    1788           7 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+    1789             : 
+    1790           7 :   if (_state_input_ == INPUT_UAV_STATE) {
+    1791           7 :     sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+    1792           0 :   } else if (_state_input_ == INPUT_ODOMETRY) {
+    1793           0 :     sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+    1794             :   }
+    1795             : 
+    1796           7 :   if (_odometry_innovation_check_enabled_) {
+    1797           7 :     sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+    1798             :   }
+    1799             : 
+    1800           7 :   sh_bumper_    = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+    1801           7 :   sh_max_z_     = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+    1802           7 :   sh_joystick_  = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+    1803           7 :   sh_gnss_      = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+    1804           7 :   sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+    1805             : 
+    1806           7 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+    1807             : 
+    1808             :   // | -------------------- general services -------------------- |
+    1809             : 
+    1810           7 :   service_server_switch_tracker_             = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+    1811           7 :   service_server_switch_controller_          = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+    1812           7 :   service_server_reset_tracker_              = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+    1813           7 :   service_server_hover_                      = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+    1814           7 :   service_server_ehover_                     = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+    1815           7 :   service_server_failsafe_                   = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+    1816           7 :   service_server_failsafe_escalating_        = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+    1817           7 :   service_server_toggle_output_              = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+    1818           7 :   service_server_arm_                        = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+    1819           7 :   service_server_enable_callbacks_           = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+    1820           7 :   service_server_set_constraints_            = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+    1821           7 :   service_server_use_joystick_               = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+    1822           7 :   service_server_use_safety_area_            = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+    1823           7 :   service_server_eland_                      = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+    1824           7 :   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+    1825           7 :   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+    1826           7 :   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+    1827           7 :   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+    1828           7 :   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+    1829           7 :   service_server_get_min_z_                  = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+    1830           7 :   service_server_validate_reference_         = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+    1831           7 :   service_server_validate_reference_2d_      = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+    1832           7 :   service_server_validate_reference_list_    = nh_.advertiseService("validate_reference_list_in", &ControlManager::callbackValidateReferenceList, this);
+    1833           7 :   service_server_start_trajectory_tracking_  = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+    1834           7 :   service_server_stop_trajectory_tracking_   = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+    1835           7 :   service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+    1836           7 :   service_server_goto_trajectory_start_      = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+    1837             : 
+    1838           7 :   sch_arming_                 = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+    1839           7 :   sch_eland_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+    1840           7 :   sch_shutdown_               = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+    1841           7 :   sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+    1842           7 :   sch_ungrip_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+    1843           7 :   sch_parachute_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+    1844             : 
+    1845             :   // | ---------------- setpoint command services --------------- |
+    1846             : 
+    1847             :   // human callable
+    1848           7 :   service_server_goto_                 = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+    1849           7 :   service_server_goto_fcu_             = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+    1850           7 :   service_server_goto_relative_        = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+    1851           7 :   service_server_goto_altitude_        = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+    1852           7 :   service_server_set_heading_          = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+    1853           7 :   service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+    1854             : 
+    1855           7 :   service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+    1856           7 :   sh_reference_             = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+    1857             : 
+    1858           7 :   service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+    1859             :   sh_velocity_reference_ =
+    1860           7 :       mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+    1861             : 
+    1862           7 :   service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+    1863             :   sh_trajectory_reference_ =
+    1864           7 :       mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+    1865             : 
+    1866             :   // | --------------------- other services --------------------- |
+    1867             : 
+    1868           7 :   service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+    1869           7 :   service_server_pirouette_           = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+    1870             : 
+    1871             :   // | ------------------------- timers ------------------------- |
+    1872             : 
+    1873           7 :   timer_status_    = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+    1874           7 :   timer_safety_    = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+    1875           7 :   timer_bumper_    = nh_.createTimer(ros::Rate(_bumper_timer_rate_), &ControlManager::timerBumper, this, false, bumper_enabled_);
+    1876           7 :   timer_eland_     = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+    1877           7 :   timer_failsafe_  = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+    1878           7 :   timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+    1879           7 :   timer_joystick_  = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+    1880             : 
+    1881             :   // | ----------------------- finish init ---------------------- |
+    1882             : 
+    1883           7 :   if (!param_loader.loadedSuccessfully()) {
+    1884           0 :     ROS_ERROR("[ControlManager]: could not load all parameters!");
+    1885           0 :     ros::shutdown();
+    1886             :   }
+    1887             : 
+    1888           7 :   is_initialized_ = true;
+    1889             : 
+    1890           7 :   ROS_INFO("[ControlManager]: initialized");
+    1891           7 : }
+    1892             : 
+    1893             : //}
+    1894             : 
+    1895             : // --------------------------------------------------------------
+    1896             : // |                           timers                           |
+    1897             : // --------------------------------------------------------------
+    1898             : 
+    1899             : /* timerHwApiCapabilities() //{ */
+    1900             : 
+    1901          10 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+    1902             : 
+    1903          20 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+    1904          20 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+    1905             : 
+    1906          10 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+    1907           3 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+    1908           3 :     return;
+    1909             :   }
+    1910             : 
+    1911          14 :   auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+    1912             : 
+    1913           7 :   ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+    1914             : 
+    1915           7 :   if (hw_ap_capabilities->accepts_actuator_cmd) {
+    1916           0 :     ROS_INFO("[ControlManager]: - actuator command");
+    1917           0 :     _hw_api_inputs_.actuators = true;
+    1918             :   }
+    1919             : 
+    1920           7 :   if (hw_ap_capabilities->accepts_control_group_cmd) {
+    1921           0 :     ROS_INFO("[ControlManager]: - control group command");
+    1922           0 :     _hw_api_inputs_.control_group = true;
+    1923             :   }
+    1924             : 
+    1925           7 :   if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+    1926           7 :     ROS_INFO("[ControlManager]: - attitude rate command");
+    1927           7 :     _hw_api_inputs_.attitude_rate = true;
+    1928             :   }
+    1929             : 
+    1930           7 :   if (hw_ap_capabilities->accepts_attitude_cmd) {
+    1931           7 :     ROS_INFO("[ControlManager]: - attitude command");
+    1932           7 :     _hw_api_inputs_.attitude = true;
+    1933             :   }
+    1934             : 
+    1935           7 :   if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+    1936           0 :     ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+    1937           0 :     _hw_api_inputs_.acceleration_hdg_rate = true;
+    1938             :   }
+    1939             : 
+    1940           7 :   if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+    1941           0 :     ROS_INFO("[ControlManager]: - acceleration+hdg command");
+    1942           0 :     _hw_api_inputs_.acceleration_hdg = true;
+    1943             :   }
+    1944             : 
+    1945           7 :   if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+    1946           0 :     ROS_INFO("[ControlManager]: - velocityhdg rate command");
+    1947           0 :     _hw_api_inputs_.velocity_hdg_rate = true;
+    1948             :   }
+    1949             : 
+    1950           7 :   if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+    1951           0 :     ROS_INFO("[ControlManager]: - velocityhdg command");
+    1952           0 :     _hw_api_inputs_.velocity_hdg = true;
+    1953             :   }
+    1954             : 
+    1955           7 :   if (hw_ap_capabilities->accepts_position_cmd) {
+    1956           0 :     ROS_INFO("[ControlManager]: - position command");
+    1957           0 :     _hw_api_inputs_.position = true;
+    1958             :   }
+    1959             : 
+    1960           7 :   initialize();
+    1961             : 
+    1962           7 :   timer_hw_api_capabilities_.stop();
+    1963             : }
+    1964             : 
+    1965             : //}
+    1966             : 
+    1967             : /* //{ timerStatus() */
+    1968             : 
+    1969        1071 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+    1970             : 
+    1971        1071 :   if (!is_initialized_)
+    1972           0 :     return;
+    1973             : 
+    1974        3213 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+    1975        3213 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+    1976             : 
+    1977             :   // copy member variables
+    1978        2142 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1979        2142 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1980        2142 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    1981        1071 :   auto yaw_error             = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+    1982        1071 :   auto position_error        = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    1983        1071 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    1984        1071 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    1985             : 
+    1986             :   double uav_x, uav_y, uav_z;
+    1987        1071 :   uav_x = uav_state.pose.position.x;
+    1988        1071 :   uav_y = uav_state.pose.position.y;
+    1989        1071 :   uav_z = uav_state.pose.position.z;
+    1990             : 
+    1991             :   // --------------------------------------------------------------
+    1992             :   // |                      print the status                      |
+    1993             :   // --------------------------------------------------------------
+    1994             : 
+    1995             :   {
+    1996        2142 :     std::string controller = _controller_names_[active_controller_idx];
+    1997        2142 :     std::string tracker    = _tracker_names_[active_tracker_idx];
+    1998        1071 :     double      mass       = last_control_output.diagnostics.total_mass;
+    1999        1071 :     double      bx_b       = last_control_output.diagnostics.disturbance_bx_b;
+    2000        1071 :     double      by_b       = last_control_output.diagnostics.disturbance_by_b;
+    2001        1071 :     double      wx_w       = last_control_output.diagnostics.disturbance_wx_w;
+    2002        1071 :     double      wy_w       = last_control_output.diagnostics.disturbance_wy_w;
+    2003             : 
+    2004        1071 :     ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+    2005             :                       tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+    2006             :   }
+    2007             : 
+    2008             :   // --------------------------------------------------------------
+    2009             :   // |                   publish the diagnostics                  |
+    2010             :   // --------------------------------------------------------------
+    2011             : 
+    2012        1071 :   publishDiagnostics();
+    2013             : 
+    2014             :   // --------------------------------------------------------------
+    2015             :   // |                publish if the offboard is on               |
+    2016             :   // --------------------------------------------------------------
+    2017             : 
+    2018        1071 :   if (offboard_mode_) {
+    2019             : 
+    2020         799 :     std_msgs::Empty offboard_on_out;
+    2021             : 
+    2022         799 :     ph_offboard_on_.publish(offboard_on_out);
+    2023             :   }
+    2024             : 
+    2025             :   // --------------------------------------------------------------
+    2026             :   // |                   publish the tilt error                   |
+    2027             :   // --------------------------------------------------------------
+    2028             :   {
+    2029        2142 :     std::scoped_lock lock(mutex_attitude_error_);
+    2030             : 
+    2031        1071 :     if (tilt_error_) {
+    2032             : 
+    2033        2142 :       mrs_msgs::Float64Stamped tilt_error_out;
+    2034        1071 :       tilt_error_out.header.stamp    = ros::Time::now();
+    2035        1071 :       tilt_error_out.header.frame_id = uav_state.header.frame_id;
+    2036        1071 :       tilt_error_out.value           = (180.0 / M_PI) * tilt_error_.value();
+    2037             : 
+    2038        1071 :       ph_tilt_error_.publish(tilt_error_out);
+    2039             :     }
+    2040             :   }
+    2041             : 
+    2042             :   // --------------------------------------------------------------
+    2043             :   // |                  publish the control error                 |
+    2044             :   // --------------------------------------------------------------
+    2045             : 
+    2046        1071 :   if (position_error) {
+    2047             : 
+    2048         750 :     Eigen::Vector3d pos_error_value = position_error.value();
+    2049             : 
+    2050        1500 :     mrs_msgs::ControlError msg_out;
+    2051             : 
+    2052         750 :     msg_out.header.stamp    = ros::Time::now();
+    2053         750 :     msg_out.header.frame_id = uav_state.header.frame_id;
+    2054             : 
+    2055         750 :     msg_out.position_errors.x    = pos_error_value[0];
+    2056         750 :     msg_out.position_errors.y    = pos_error_value[1];
+    2057         750 :     msg_out.position_errors.z    = pos_error_value[2];
+    2058         750 :     msg_out.total_position_error = pos_error_value.norm();
+    2059             : 
+    2060         750 :     if (yaw_error_) {
+    2061         750 :       msg_out.yaw_error = yaw_error.value();
+    2062             :     }
+    2063             : 
+    2064         750 :     std::map<std::string, ControllerParams>::iterator it;
+    2065             : 
+    2066         750 :     it = controllers_.find(_controller_names_[active_controller_idx]);
+    2067             : 
+    2068         750 :     msg_out.position_eland_threshold    = it->second.eland_threshold;
+    2069         750 :     msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+    2070             : 
+    2071         750 :     ph_control_error_.publish(msg_out);
+    2072             :   }
+    2073             : 
+    2074             :   // --------------------------------------------------------------
+    2075             :   // |                  publish the mass estimate                 |
+    2076             :   // --------------------------------------------------------------
+    2077             : 
+    2078        1071 :   if (last_control_output.diagnostics.mass_estimator) {
+    2079             : 
+    2080         588 :     std_msgs::Float64 mass_estimate_out;
+    2081         588 :     mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    2082             : 
+    2083         588 :     ph_mass_estimate_.publish(mass_estimate_out);
+    2084             :   }
+    2085             : 
+    2086             :   // --------------------------------------------------------------
+    2087             :   // |                 publish the current heading                |
+    2088             :   // --------------------------------------------------------------
+    2089             : 
+    2090        1071 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2091             : 
+    2092             :     try {
+    2093             : 
+    2094             :       double heading;
+    2095             : 
+    2096         887 :       heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2097             : 
+    2098        1774 :       mrs_msgs::Float64Stamped heading_out;
+    2099         887 :       heading_out.header = uav_state.header;
+    2100         887 :       heading_out.value  = heading;
+    2101             : 
+    2102         887 :       ph_heading_.publish(heading_out);
+    2103             :     }
+    2104           0 :     catch (...) {
+    2105           0 :       ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+    2106             :     }
+    2107             :   }
+    2108             : 
+    2109             :   // --------------------------------------------------------------
+    2110             :   // |                  publish the current speed                 |
+    2111             :   // --------------------------------------------------------------
+    2112             : 
+    2113        1071 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2114             : 
+    2115         887 :     double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+    2116             : 
+    2117        1774 :     mrs_msgs::Float64Stamped speed_out;
+    2118         887 :     speed_out.header = uav_state.header;
+    2119         887 :     speed_out.value  = speed;
+    2120             : 
+    2121         887 :     ph_speed_.publish(speed_out);
+    2122             :   }
+    2123             : 
+    2124             :   // --------------------------------------------------------------
+    2125             :   // |               publish the safety area markers              |
+    2126             :   // --------------------------------------------------------------
+    2127             : 
+    2128        1071 :   if (use_safety_area_) {
+    2129             : 
+    2130        1200 :     mrs_msgs::ReferenceStamped temp_ref;
+    2131         600 :     temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+    2132             : 
+    2133        1200 :     geometry_msgs::TransformStamped tf;
+    2134             : 
+    2135        1800 :     auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+    2136             : 
+    2137         600 :     if (ret) {
+    2138             : 
+    2139         475 :       ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+    2140             : 
+    2141         950 :       visualization_msgs::MarkerArray safety_area_marker_array;
+    2142         950 :       visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+    2143             : 
+    2144         950 :       mrs_lib::Polygon border = safety_zone_->getBorder();
+    2145             : 
+    2146         950 :       std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+    2147         950 :       std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+    2148             : 
+    2149         950 :       std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+    2150         950 :       std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+    2151             : 
+    2152             :       // if we fail in transforming the area at some point
+    2153             :       // do not publish it at all
+    2154         475 :       bool tf_success = true;
+    2155             : 
+    2156         950 :       geometry_msgs::TransformStamped tf = ret.value();
+    2157             : 
+    2158             :       /* transform area points to local origin //{ */
+    2159             : 
+    2160             :       // transform border bottom points to local origin
+    2161        2375 :       for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+    2162             : 
+    2163        1900 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2164        1900 :         temp_ref.header.stamp         = ros::Time(0);
+    2165        1900 :         temp_ref.reference.position.x = border_points_bot_original[i].x;
+    2166        1900 :         temp_ref.reference.position.y = border_points_bot_original[i].y;
+    2167        1900 :         temp_ref.reference.position.z = border_points_bot_original[i].z;
+    2168             : 
+    2169        3800 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2170             : 
+    2171        1900 :           temp_ref = ret.value();
+    2172             : 
+    2173        1900 :           border_points_bot_transformed[i].x = temp_ref.reference.position.x;
+    2174        1900 :           border_points_bot_transformed[i].y = temp_ref.reference.position.y;
+    2175        1900 :           border_points_bot_transformed[i].z = temp_ref.reference.position.z;
+    2176             : 
+    2177             :         } else {
+    2178           0 :           tf_success = false;
+    2179             :         }
+    2180             :       }
+    2181             : 
+    2182             :       // transform border top points to local origin
+    2183        2375 :       for (size_t i = 0; i < border_points_top_original.size(); i++) {
+    2184             : 
+    2185        1900 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2186        1900 :         temp_ref.header.stamp         = ros::Time(0);
+    2187        1900 :         temp_ref.reference.position.x = border_points_top_original[i].x;
+    2188        1900 :         temp_ref.reference.position.y = border_points_top_original[i].y;
+    2189        1900 :         temp_ref.reference.position.z = border_points_top_original[i].z;
+    2190             : 
+    2191        3800 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2192             : 
+    2193        1900 :           temp_ref = ret.value();
+    2194             : 
+    2195        1900 :           border_points_top_transformed[i].x = temp_ref.reference.position.x;
+    2196        1900 :           border_points_top_transformed[i].y = temp_ref.reference.position.y;
+    2197        1900 :           border_points_top_transformed[i].z = temp_ref.reference.position.z;
+    2198             : 
+    2199             :         } else {
+    2200           0 :           tf_success = false;
+    2201             :         }
+    2202             :       }
+    2203             : 
+    2204             :       //}
+    2205             : 
+    2206         950 :       visualization_msgs::Marker safety_area_marker;
+    2207             : 
+    2208         475 :       safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2209         475 :       safety_area_marker.type            = visualization_msgs::Marker::LINE_LIST;
+    2210         475 :       safety_area_marker.color.a         = 0.15;
+    2211         475 :       safety_area_marker.scale.x         = 0.2;
+    2212         475 :       safety_area_marker.color.r         = 1;
+    2213         475 :       safety_area_marker.color.g         = 0;
+    2214         475 :       safety_area_marker.color.b         = 0;
+    2215             : 
+    2216         475 :       safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2217             : 
+    2218         950 :       visualization_msgs::Marker safety_area_coordinates_marker;
+    2219             : 
+    2220         475 :       safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2221         475 :       safety_area_coordinates_marker.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    2222         475 :       safety_area_coordinates_marker.color.a         = 1;
+    2223         475 :       safety_area_coordinates_marker.scale.z         = 1.0;
+    2224         475 :       safety_area_coordinates_marker.color.r         = 0;
+    2225         475 :       safety_area_coordinates_marker.color.g         = 0;
+    2226         475 :       safety_area_coordinates_marker.color.b         = 0;
+    2227             : 
+    2228         475 :       safety_area_coordinates_marker.id = 0;
+    2229             : 
+    2230         475 :       safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2231             : 
+    2232             :       /* adding safety area points //{ */
+    2233             : 
+    2234             :       // bottom border
+    2235        2375 :       for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+    2236             : 
+    2237        1900 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2238        1900 :         safety_area_marker.points.push_back(border_points_bot_transformed[(i + 1) % border_points_bot_transformed.size()]);
+    2239             : 
+    2240        3800 :         std::stringstream ss;
+    2241             : 
+    2242        1900 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2243           0 :           ss << "idx: " << i << std::endl
+    2244           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2245           0 :              << "lon: " << border_points_bot_original[i].y;
+    2246             :         } else {
+    2247        1900 :           ss << "idx: " << i << std::endl
+    2248        1900 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2249        1900 :              << "y: " << border_points_bot_original[i].y;
+    2250             :         }
+    2251             : 
+    2252        1900 :         safety_area_coordinates_marker.color.r = 0;
+    2253        1900 :         safety_area_coordinates_marker.color.g = 0;
+    2254        1900 :         safety_area_coordinates_marker.color.b = 0;
+    2255             : 
+    2256        1900 :         safety_area_coordinates_marker.pose.position = border_points_bot_transformed[i];
+    2257        1900 :         safety_area_coordinates_marker.text          = ss.str();
+    2258        1900 :         safety_area_coordinates_marker.id++;
+    2259             : 
+    2260        1900 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2261             :       }
+    2262             : 
+    2263             :       // top border + top/bot edges
+    2264        2375 :       for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+    2265             : 
+    2266        1900 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2267        1900 :         safety_area_marker.points.push_back(border_points_top_transformed[(i + 1) % border_points_top_transformed.size()]);
+    2268             : 
+    2269        1900 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2270        1900 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2271             : 
+    2272        3800 :         std::stringstream ss;
+    2273             : 
+    2274        1900 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2275           0 :           ss << "idx: " << i << std::endl
+    2276           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2277           0 :              << "lon: " << border_points_bot_original[i].y;
+    2278             :         } else {
+    2279        1900 :           ss << "idx: " << i << std::endl
+    2280        1900 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2281        1900 :              << "y: " << border_points_bot_original[i].y;
+    2282             :         }
+    2283             : 
+    2284        1900 :         safety_area_coordinates_marker.color.r = 1;
+    2285        1900 :         safety_area_coordinates_marker.color.g = 1;
+    2286        1900 :         safety_area_coordinates_marker.color.b = 1;
+    2287             : 
+    2288        1900 :         safety_area_coordinates_marker.pose.position = border_points_top_transformed[i];
+    2289        1900 :         safety_area_coordinates_marker.text          = ss.str();
+    2290        1900 :         safety_area_coordinates_marker.id++;
+    2291             : 
+    2292        1900 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2293             :       }
+    2294             : 
+    2295             :       //}
+    2296             : 
+    2297         475 :       if (tf_success) {
+    2298             : 
+    2299         475 :         safety_area_marker_array.markers.push_back(safety_area_marker);
+    2300             : 
+    2301         475 :         ph_safety_area_markers_.publish(safety_area_marker_array);
+    2302             : 
+    2303         475 :         ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+    2304             :       }
+    2305             : 
+    2306             :     } else {
+    2307         125 :       ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+    2308             :     }
+    2309             :   }
+    2310             : 
+    2311             :   // --------------------------------------------------------------
+    2312             :   // |              publish the disturbances markers              |
+    2313             :   // --------------------------------------------------------------
+    2314             : 
+    2315        1071 :   if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+    2316             : 
+    2317        1176 :     visualization_msgs::MarkerArray msg_out;
+    2318             : 
+    2319         588 :     double id = 0;
+    2320             : 
+    2321         588 :     double multiplier = 1.0;
+    2322             : 
+    2323         588 :     Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2324             : 
+    2325         588 :     Eigen::Vector3d      vec3d;
+    2326         588 :     geometry_msgs::Point point;
+    2327             : 
+    2328             :     /* world disturbance //{ */
+    2329             :     {
+    2330             : 
+    2331        1176 :       visualization_msgs::Marker marker;
+    2332             : 
+    2333         588 :       marker.header.frame_id = uav_state.header.frame_id;
+    2334         588 :       marker.header.stamp    = ros::Time::now();
+    2335         588 :       marker.ns              = "control_manager";
+    2336         588 :       marker.id              = id++;
+    2337         588 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2338         588 :       marker.action          = visualization_msgs::Marker::ADD;
+    2339             : 
+    2340             :       /* position //{ */
+    2341             : 
+    2342         588 :       marker.pose.position.x = 0.0;
+    2343         588 :       marker.pose.position.y = 0.0;
+    2344         588 :       marker.pose.position.z = 0.0;
+    2345             : 
+    2346             :       //}
+    2347             : 
+    2348             :       /* orientation //{ */
+    2349             : 
+    2350         588 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2351             : 
+    2352             :       //}
+    2353             : 
+    2354             :       /* origin //{ */
+    2355         588 :       point.x = uav_x;
+    2356         588 :       point.y = uav_y;
+    2357         588 :       point.z = uav_z;
+    2358             : 
+    2359         588 :       marker.points.push_back(point);
+    2360             : 
+    2361             :       //}
+    2362             : 
+    2363             :       /* tip //{ */
+    2364             : 
+    2365         588 :       point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+    2366         588 :       point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+    2367         588 :       point.z = uav_z;
+    2368             : 
+    2369         588 :       marker.points.push_back(point);
+    2370             : 
+    2371             :       //}
+    2372             : 
+    2373         588 :       marker.scale.x = 0.05;
+    2374         588 :       marker.scale.y = 0.05;
+    2375         588 :       marker.scale.z = 0.05;
+    2376             : 
+    2377         588 :       marker.color.a = 0.5;
+    2378         588 :       marker.color.r = 1.0;
+    2379         588 :       marker.color.g = 0.0;
+    2380         588 :       marker.color.b = 0.0;
+    2381             : 
+    2382         588 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2383             : 
+    2384         588 :       msg_out.markers.push_back(marker);
+    2385             :     }
+    2386             : 
+    2387             :     //}
+    2388             : 
+    2389             :     /* body disturbance //{ */
+    2390             :     {
+    2391             : 
+    2392        1176 :       visualization_msgs::Marker marker;
+    2393             : 
+    2394         588 :       marker.header.frame_id = uav_state.header.frame_id;
+    2395         588 :       marker.header.stamp    = ros::Time::now();
+    2396         588 :       marker.ns              = "control_manager";
+    2397         588 :       marker.id              = id++;
+    2398         588 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2399         588 :       marker.action          = visualization_msgs::Marker::ADD;
+    2400             : 
+    2401             :       /* position //{ */
+    2402             : 
+    2403         588 :       marker.pose.position.x = 0.0;
+    2404         588 :       marker.pose.position.y = 0.0;
+    2405         588 :       marker.pose.position.z = 0.0;
+    2406             : 
+    2407             :       //}
+    2408             : 
+    2409             :       /* orientation //{ */
+    2410             : 
+    2411         588 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2412             : 
+    2413             :       //}
+    2414             : 
+    2415             :       /* origin //{ */
+    2416             : 
+    2417         588 :       point.x = uav_x;
+    2418         588 :       point.y = uav_y;
+    2419         588 :       point.z = uav_z;
+    2420             : 
+    2421         588 :       marker.points.push_back(point);
+    2422             : 
+    2423             :       //}
+    2424             : 
+    2425             :       /* tip //{ */
+    2426             : 
+    2427         588 :       vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+    2428         588 :       vec3d = quat_eigen * vec3d;
+    2429             : 
+    2430         588 :       point.x = uav_x + vec3d[0];
+    2431         588 :       point.y = uav_y + vec3d[1];
+    2432         588 :       point.z = uav_z + vec3d[2];
+    2433             : 
+    2434         588 :       marker.points.push_back(point);
+    2435             : 
+    2436             :       //}
+    2437             : 
+    2438         588 :       marker.scale.x = 0.05;
+    2439         588 :       marker.scale.y = 0.05;
+    2440         588 :       marker.scale.z = 0.05;
+    2441             : 
+    2442         588 :       marker.color.a = 0.5;
+    2443         588 :       marker.color.r = 0.0;
+    2444         588 :       marker.color.g = 1.0;
+    2445         588 :       marker.color.b = 0.0;
+    2446             : 
+    2447         588 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2448             : 
+    2449         588 :       msg_out.markers.push_back(marker);
+    2450             :     }
+    2451             : 
+    2452             :     //}
+    2453             : 
+    2454         588 :     ph_disturbances_markers_.publish(msg_out);
+    2455             :   }
+    2456             : 
+    2457             :   // --------------------------------------------------------------
+    2458             :   // |               publish the current constraints              |
+    2459             :   // --------------------------------------------------------------
+    2460             : 
+    2461        1071 :   if (got_constraints_) {
+    2462             : 
+    2463         883 :     auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+    2464             : 
+    2465         883 :     mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+    2466             : 
+    2467         883 :     ph_current_constraints_.publish(constraints);
+    2468             :   }
+    2469             : }
+    2470             : 
+    2471             : //}
+    2472             : 
+    2473             : /* //{ timerSafety() */
+    2474             : 
+    2475       17542 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+    2476             : 
+    2477       17542 :   mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+    2478             : 
+    2479       17542 :   if (!is_initialized_)
+    2480           0 :     return;
+    2481             : 
+    2482       35084 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+    2483       35084 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+    2484             : 
+    2485             :   // copy member variables
+    2486       17542 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2487       17542 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    2488       17542 :   auto [uav_state, uav_yaw]  = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+    2489       17542 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2490       17542 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2491             : 
+    2492       33210 :   if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+    2493       15668 :       active_tracker_idx == _null_tracker_idx_) {
+    2494        4827 :     return;
+    2495             :   }
+    2496             : 
+    2497       12715 :   if (odometry_switch_in_progress_) {
+    2498           0 :     ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+    2499           0 :     return;
+    2500             :   }
+    2501             : 
+    2502             :   // | ------------------------ timeouts ------------------------ |
+    2503             : 
+    2504       12715 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2505       12715 :     double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+    2506             : 
+    2507       12715 :     if (missing_for > _uav_state_max_missing_time_) {
+    2508           0 :       timeoutUavState(missing_for);
+    2509             :     }
+    2510             :   }
+    2511             : 
+    2512       12715 :   if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+    2513           0 :     double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+    2514             : 
+    2515           0 :     if (missing_for > _uav_state_max_missing_time_) {
+    2516           0 :       timeoutUavState(missing_for);
+    2517             :     }
+    2518             :   }
+    2519             : 
+    2520             :   // | -------------- eland and failsafe thresholds ------------- |
+    2521             : 
+    2522       12715 :   std::map<std::string, ControllerParams>::iterator it;
+    2523       12715 :   it = controllers_.find(_controller_names_[active_controller_idx]);
+    2524             : 
+    2525       12715 :   _eland_threshold_               = it->second.eland_threshold;
+    2526       12715 :   _failsafe_threshold_            = it->second.failsafe_threshold;
+    2527       12715 :   _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+    2528             : 
+    2529             :   // | --------- calculate control errors and tilt angle -------- |
+    2530             : 
+    2531             :   // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+    2532             :   // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+    2533       12715 :   if (!last_tracker_cmd || !last_control_output.control_output) {
+    2534          11 :     return;
+    2535             :   }
+    2536             : 
+    2537             :   {
+    2538       12704 :     std::scoped_lock lock(mutex_attitude_error_);
+    2539             : 
+    2540       12704 :     tilt_error_ = 0;
+    2541       12704 :     yaw_error_  = 0;
+    2542             :   }
+    2543             : 
+    2544             :   {
+    2545             :     // TODO this whole scope is very clumsy
+    2546             : 
+    2547       12704 :     position_error_ = {};
+    2548             : 
+    2549       12704 :     if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2550             : 
+    2551       12704 :       std::scoped_lock lock(mutex_position_error_);
+    2552             : 
+    2553       12704 :       if (!position_error_) {
+    2554       12704 :         position_error_ = Eigen::Vector3d::Zero(3);
+    2555             :       }
+    2556             : 
+    2557       12704 :       position_error_.value()[0] = last_tracker_cmd->position.x - uav_state.pose.position.x;
+    2558       12704 :       position_error_.value()[1] = last_tracker_cmd->position.y - uav_state.pose.position.y;
+    2559             :     }
+    2560             : 
+    2561       12704 :     if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2562             : 
+    2563       12704 :       std::scoped_lock lock(mutex_position_error_);
+    2564             : 
+    2565       12704 :       if (!position_error_) {
+    2566           0 :         position_error_ = Eigen::Vector3d::Zero(3);
+    2567             :       }
+    2568             : 
+    2569       12704 :       position_error_.value()[2] = last_tracker_cmd->position.z - uav_state.pose.position.z;
+    2570             :     }
+    2571             :   }
+    2572             : 
+    2573             :   // rotate the drone's z axis
+    2574       12704 :   tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2575       12704 :   tf2::Vector3   uav_z_in_world      = uav_state_transform * tf2::Vector3(0, 0, 1);
+    2576             : 
+    2577             :   // calculate the angle between the drone's z axis and the world's z axis
+    2578       12704 :   double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+    2579             : 
+    2580             :   // | ------------ calculate the tilt and yaw error ------------ |
+    2581             : 
+    2582             :   // | --------------------- the tilt error --------------------- |
+    2583             : 
+    2584       12704 :   if (last_control_output.desired_orientation) {
+    2585             : 
+    2586             :     // calculate the desired drone's z axis in the world frame
+    2587       11229 :     tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+    2588       11229 :     tf2::Vector3   uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+    2589             : 
+    2590             :     {
+    2591       11229 :       std::scoped_lock lock(mutex_attitude_error_);
+    2592             : 
+    2593             :       // calculate the angle between the drone's z axis and the world's z axis
+    2594       11229 :       tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+    2595             : 
+    2596             :       // calculate the yaw error
+    2597       11229 :       double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+    2598       11229 :       yaw_error_     = fabs(radians::diff(cmd_yaw, uav_yaw));
+    2599             :     }
+    2600             :   }
+    2601             : 
+    2602       12704 :   auto position_error          = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2603       12704 :   auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+    2604             : 
+    2605             :   // --------------------------------------------------------------
+    2606             :   // |   activate the failsafe controller in case of large error  |
+    2607             :   // --------------------------------------------------------------
+    2608             : 
+    2609       12704 :   if (position_error) {
+    2610             : 
+    2611       12704 :     if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+    2612             : 
+    2613           1 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2614             : 
+    2615           1 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2616             : 
+    2617           1 :         if (!failsafe_triggered_) {
+    2618             : 
+    2619           1 :           ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+    2620             :                     _failsafe_threshold_, position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2621             : 
+    2622           1 :           failsafe();
+    2623             :         }
+    2624             :       }
+    2625             :     }
+    2626             :   }
+    2627             : 
+    2628             :   // --------------------------------------------------------------
+    2629             :   // |     activate emergency land in case of large innovation    |
+    2630             :   // --------------------------------------------------------------
+    2631             : 
+    2632       12704 :   if (_odometry_innovation_check_enabled_) {
+    2633             :     {
+    2634       12704 :       auto [x, y, z] = mrs_lib::getPosition(sh_odometry_innovation_.getMsg());
+    2635             : 
+    2636       12704 :       double heading = 0;
+    2637             :       try {
+    2638       12704 :         heading = mrs_lib::getHeading(sh_odometry_innovation_.getMsg());
+    2639             :       }
+    2640           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    2641           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+    2642             :       }
+    2643             : 
+    2644       12704 :       double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+    2645             : 
+    2646       12704 :       if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+    2647             : 
+    2648          37 :         auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2649             : 
+    2650          37 :         if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2651             : 
+    2652          30 :           if (!failsafe_triggered_ && !eland_triggered_) {
+    2653             : 
+    2654           1 :             ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+    2655             :                       last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+    2656             : 
+    2657           1 :             eland();
+    2658             :           }
+    2659             :         }
+    2660             :       }
+    2661             :     }
+    2662             :   }
+    2663             : 
+    2664             :   // --------------------------------------------------------------
+    2665             :   // |   activate emergency land in case of medium control error  |
+    2666             :   // --------------------------------------------------------------
+    2667             : 
+    2668             :   // | ------------------- tilt control error ------------------- |
+    2669             : 
+    2670       12704 :   if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+    2671             : 
+    2672           0 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2673             : 
+    2674           0 :     if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2675             : 
+    2676           0 :       if (!failsafe_triggered_ && !eland_triggered_) {
+    2677             : 
+    2678           0 :         ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+    2679             :                   (180.0 / M_PI) * _tilt_limit_eland_);
+    2680             : 
+    2681           0 :         eland();
+    2682             :       }
+    2683             :     }
+    2684             :   }
+    2685             : 
+    2686             :   // | ----------------- position control error ----------------- |
+    2687             : 
+    2688       12704 :   if (position_error) {
+    2689             : 
+    2690       12704 :     double error_size = position_error->norm();
+    2691             : 
+    2692       12704 :     if (error_size > _eland_threshold_ / 2.0) {
+    2693             : 
+    2694         145 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2695             : 
+    2696         145 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2697             : 
+    2698         137 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2699             : 
+    2700         103 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+    2701             : 
+    2702         103 :           ungripSrv();
+    2703             :         }
+    2704             :       }
+    2705             :     }
+    2706             : 
+    2707       12704 :     if (error_size > _eland_threshold_) {
+    2708             : 
+    2709          42 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2710             : 
+    2711          42 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2712             : 
+    2713          35 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2714             : 
+    2715           1 :           ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+    2716             :                     position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2717             : 
+    2718           1 :           eland();
+    2719             :         }
+    2720             :       }
+    2721             :     }
+    2722             :   }
+    2723             : 
+    2724             :   // | -------------------- yaw control error ------------------- |
+    2725             :   // do not have to mutex the yaw_error_ here since I am filling it in this function
+    2726             : 
+    2727       12704 :   if (_yaw_error_eland_enabled_ && yaw_error) {
+    2728             : 
+    2729       12704 :     if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+    2730             : 
+    2731           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2732             : 
+    2733           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2734             : 
+    2735           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2736             : 
+    2737           0 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2738             :                              (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+    2739             : 
+    2740           0 :           ungripSrv();
+    2741             :         }
+    2742             :       }
+    2743             :     }
+    2744             : 
+    2745       12704 :     if (yaw_error.value() > _yaw_error_eland_) {
+    2746             : 
+    2747           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2748             : 
+    2749           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2750             : 
+    2751           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2752             : 
+    2753           0 :           ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2754             :                     (180.0 / M_PI) * _yaw_error_eland_);
+    2755             : 
+    2756           0 :           eland();
+    2757             :         }
+    2758             :       }
+    2759             :     }
+    2760             :   }
+    2761             : 
+    2762             :   // --------------------------------------------------------------
+    2763             :   // |      disarm the drone when the tilt exceeds the limit      |
+    2764             :   // --------------------------------------------------------------
+    2765       12704 :   if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+    2766             : 
+    2767           0 :     ROS_ERROR("[ControlManager]: tilt angle too large, disarming: tilt angle=%.2f/%.2f deg", (180.0 / M_PI) * tilt_angle, (180.0 / M_PI) * _tilt_limit_disarm_);
+    2768             : 
+    2769           0 :     arming(false);
+    2770             :   }
+    2771             : 
+    2772             :   // --------------------------------------------------------------
+    2773             :   // |     disarm the drone when tilt error exceeds the limit     |
+    2774             :   // --------------------------------------------------------------
+    2775             : 
+    2776       12704 :   if (_tilt_error_disarm_enabled_ && tilt_error) {
+    2777             : 
+    2778       12704 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2779             : 
+    2780             :     // the time from the last controller/tracker switch
+    2781             :     // fyi: we should not
+    2782       12704 :     double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+    2783             : 
+    2784             :     // if the tile error is over the threshold
+    2785             :     // && we are not ramping up during takeoff
+    2786       12704 :     if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+    2787             : 
+    2788             :       // only account for the error if some time passed from the last tracker/controller switch
+    2789           0 :       if (time_from_ctrl_tracker_switch > 1.0) {
+    2790             : 
+    2791             :         // if the threshold was not exceeded before
+    2792           0 :         if (!tilt_error_disarm_over_thr_) {
+    2793             : 
+    2794           0 :           tilt_error_disarm_over_thr_ = true;
+    2795           0 :           tilt_error_disarm_time_     = ros::Time::now();
+    2796             : 
+    2797           0 :           ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+    2798             :                    (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+    2799             : 
+    2800             :           // if it was exceeded before, just keep it
+    2801             :         } else {
+    2802             : 
+    2803           0 :           ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+    2804             :                             (ros::Time::now() - tilt_error_disarm_time_).toSec());
+    2805             :         }
+    2806             : 
+    2807             :         // if the tile error is bad, but the controller just switched,
+    2808             :         // don't think its bad anymore
+    2809             :       } else {
+    2810             : 
+    2811           0 :         tilt_error_disarm_over_thr_ = false;
+    2812           0 :         tilt_error_disarm_time_     = ros::Time::now();
+    2813             :       }
+    2814             : 
+    2815             :       // if the tilt error is fine
+    2816             :     } else {
+    2817             : 
+    2818             :       // make it fine
+    2819       12704 :       tilt_error_disarm_over_thr_ = false;
+    2820       12704 :       tilt_error_disarm_time_     = ros::Time::now();
+    2821             :     }
+    2822             : 
+    2823             :     // calculate the time over the threshold
+    2824       12704 :     double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+    2825             : 
+    2826             :     // if the tot exceeds the limit (and if we are actually over the threshold)
+    2827       12704 :     if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+    2828             : 
+    2829           0 :       bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+    2830             : 
+    2831             :       // only when flying and not in failsafe
+    2832           0 :       if (is_flying) {
+    2833             : 
+    2834           0 :         ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+    2835             : 
+    2836           0 :         toggleOutput(false);
+    2837           0 :         arming(false);
+    2838             :       }
+    2839             :     }
+    2840             :   }
+    2841             : 
+    2842             :   // | --------- dropping out of OFFBOARD in mid flight --------- |
+    2843             : 
+    2844             :   // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+    2845       12704 :   if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+    2846             : 
+    2847           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+    2848             : 
+    2849           0 :     toggleOutput(false);
+    2850             :   }
+    2851             : }  // namespace control_manager
+    2852             : 
+    2853             : //}
+    2854             : 
+    2855             : /* //{ timerEland() */
+    2856             : 
+    2857         235 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+    2858             : 
+    2859         235 :   if (!is_initialized_)
+    2860           0 :     return;
+    2861             : 
+    2862         470 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+    2863         470 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+    2864             : 
+    2865             :   // copy member variables
+    2866         235 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2867             : 
+    2868         235 :   if (!last_control_output.control_output) {
+    2869           0 :     return;
+    2870             :   }
+    2871             : 
+    2872         235 :   double throttle = 0;
+    2873             : 
+    2874         235 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeCmd>(last_control_output.control_output.value())) {
+    2875           0 :     throttle = std::get<mrs_msgs::HwApiAttitudeCmd>(last_control_output.control_output.value()).throttle;
+    2876         235 :   } else if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(last_control_output.control_output.value())) {
+    2877         235 :     throttle = std::get<mrs_msgs::HwApiAttitudeRateCmd>(last_control_output.control_output.value()).throttle;
+    2878           0 :   } else if (std::holds_alternative<mrs_msgs::HwApiControlGroupCmd>(last_control_output.control_output.value())) {
+    2879           0 :     throttle = std::get<mrs_msgs::HwApiControlGroupCmd>(last_control_output.control_output.value()).throttle;
+    2880             :   } else {
+    2881           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement eland timer for this control mode");
+    2882           0 :     return;
+    2883             :   }
+    2884             : 
+    2885         235 :   if (current_state_landing_ == IDLE_STATE) {
+    2886             : 
+    2887           0 :     return;
+    2888             : 
+    2889         235 :   } else if (current_state_landing_ == LANDING_STATE) {
+    2890             : 
+    2891         235 :     if (!last_control_output.control_output) {
+    2892           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+    2893           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+    2894           0 :       return;
+    2895             :     }
+    2896             : 
+    2897             :     // recalculate the mass based on the throttle
+    2898         235 :     throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle) / common_handlers_->g;
+    2899         235 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2900             : 
+    2901             :     // condition for automatic motor turn off
+    2902         235 :     if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+    2903          66 :       if (!throttle_under_threshold_) {
+    2904             : 
+    2905           3 :         throttle_mass_estimate_first_time_ = ros::Time::now();
+    2906           3 :         throttle_under_threshold_          = true;
+    2907             :       }
+    2908             : 
+    2909          66 :       ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2910             : 
+    2911             :     } else {
+    2912         169 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2913         169 :       throttle_under_threshold_          = false;
+    2914             :     }
+    2915             : 
+    2916         235 :     if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    2917             :       // enable callbacks? ... NO
+    2918             : 
+    2919           3 :       ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+    2920           3 :       toggleOutput(false);
+    2921             : 
+    2922             :       // disarm the drone
+    2923           3 :       if (_eland_disarm_enabled_) {
+    2924             : 
+    2925           3 :         ROS_INFO("[ControlManager]: calling for disarm");
+    2926           3 :         arming(false);
+    2927             :       }
+    2928             : 
+    2929           3 :       shutdown();
+    2930             : 
+    2931           3 :       changeLandingState(IDLE_STATE);
+    2932             : 
+    2933           3 :       ROS_WARN("[ControlManager]: emergency landing finished");
+    2934             : 
+    2935           3 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    2936           3 :       timer_eland_.stop();
+    2937           3 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    2938             : 
+    2939             :       // we should NOT set eland_triggered_=true
+    2940             :     }
+    2941             :   }
+    2942             : }
+    2943             : 
+    2944             : //}
+    2945             : 
+    2946             : /* //{ timerFailsafe() */
+    2947             : 
+    2948        1397 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+    2949             : 
+    2950        1397 :   if (!is_initialized_) {
+    2951           0 :     return;
+    2952             :   }
+    2953             : 
+    2954        1397 :   ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+    2955             : 
+    2956        2794 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+    2957        2794 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+    2958             : 
+    2959             :   // copy member variables
+    2960        1397 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2961             : 
+    2962        1397 :   updateControllers(uav_state);
+    2963             : 
+    2964        1397 :   publish();
+    2965             : 
+    2966        1397 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2967             : 
+    2968        1397 :   if (!last_control_output.control_output) {
+    2969           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+    2970           0 :     return;
+    2971             :   }
+    2972             : 
+    2973        1397 :   auto throttle = extractThrottle(last_control_output);
+    2974             : 
+    2975        1397 :   if (!throttle) {
+    2976           0 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+    2977           0 :     return;
+    2978             :   }
+    2979             : 
+    2980        1397 :   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2981        1397 :   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2982             : 
+    2983             :   // condition for automatic motor turn off
+    2984        1397 :   if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+    2985             : 
+    2986         202 :     if (!throttle_under_threshold_) {
+    2987             : 
+    2988           1 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2989           1 :       throttle_under_threshold_          = true;
+    2990             :     }
+    2991             : 
+    2992         202 :     ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2993             : 
+    2994             :   } else {
+    2995             : 
+    2996        1195 :     throttle_mass_estimate_first_time_ = ros::Time::now();
+    2997        1195 :     throttle_under_threshold_          = false;
+    2998             :   }
+    2999             : 
+    3000             :   // condition for automatic motor turn off
+    3001        1397 :   if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    3002             : 
+    3003           1 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+    3004             : 
+    3005           1 :     arming(false);
+    3006             :   }
+    3007             : }
+    3008             : 
+    3009             : //}
+    3010             : 
+    3011             : /* //{ timerJoystick() */
+    3012             : 
+    3013        3218 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+    3014             : 
+    3015        3218 :   if (!is_initialized_) {
+    3016           0 :     return;
+    3017             :   }
+    3018             : 
+    3019        9654 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+    3020        9654 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3021             : 
+    3022        6436 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3023             : 
+    3024             :   // if start was pressed and held for > 3.0 s
+    3025        3218 :   if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+    3026             : 
+    3027           0 :     joystick_start_press_time_ = ros::Time(0);
+    3028             : 
+    3029           0 :     ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+    3030             :              _joystick_controller_name_.c_str());
+    3031             : 
+    3032           0 :     joystick_start_pressed_ = false;
+    3033             : 
+    3034           0 :     switchTracker(_joystick_tracker_name_);
+    3035           0 :     switchController(_joystick_controller_name_);
+    3036             :   }
+    3037             : 
+    3038             :   // if RT+LT were pressed and held for > 0.1 s
+    3039        3218 :   if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+    3040             : 
+    3041           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3042             : 
+    3043           0 :     ROS_INFO("[ControlManager]: activating failsafe by joystick");
+    3044             : 
+    3045           0 :     joystick_failsafe_pressed_ = false;
+    3046             : 
+    3047           0 :     failsafe();
+    3048             :   }
+    3049             : 
+    3050             :   // if joypads were pressed and held for > 0.1 s
+    3051        3218 :   if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+    3052             : 
+    3053           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3054             : 
+    3055           0 :     ROS_INFO("[ControlManager]: activating eland by joystick");
+    3056             : 
+    3057           0 :     joystick_failsafe_pressed_ = false;
+    3058             : 
+    3059           0 :     eland();
+    3060             :   }
+    3061             : 
+    3062             :   // if back was pressed and held for > 0.1 s
+    3063        3218 :   if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+    3064             : 
+    3065           0 :     joystick_back_press_time_ = ros::Time(0);
+    3066             : 
+    3067             :     // activate/deactivate the joystick goto functionality
+    3068           0 :     joystick_goto_enabled_ = !joystick_goto_enabled_;
+    3069             : 
+    3070           0 :     ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+    3071             :   }
+    3072             : 
+    3073             :   // if the GOTO functionality is enabled...
+    3074        3218 :   if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+    3075             : 
+    3076           0 :     auto joystick_data = sh_joystick_.getMsg();
+    3077             : 
+    3078             :     // create the reference
+    3079             : 
+    3080           0 :     mrs_msgs::Vec4::Request request;
+    3081             : 
+    3082           0 :     if (fabs(joystick_data->axes[_channel_pitch_]) >= 0.05 || fabs(joystick_data->axes[_channel_roll_]) >= 0.05 ||
+    3083           0 :         fabs(joystick_data->axes[_channel_heading_]) >= 0.05 || fabs(joystick_data->axes[_channel_throttle_]) >= 0.05) {
+    3084             : 
+    3085           0 :       if (_joystick_mode_ == 0) {
+    3086             : 
+    3087           0 :         request.goal[REF_X]       = _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * _joystick_carrot_distance_;
+    3088           0 :         request.goal[REF_Y]       = _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * _joystick_carrot_distance_;
+    3089           0 :         request.goal[REF_Z]       = _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_];
+    3090           0 :         request.goal[REF_HEADING] = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3091             : 
+    3092           0 :         mrs_msgs::Vec4::Response response;
+    3093             : 
+    3094           0 :         callbackGotoFcu(request, response);
+    3095             : 
+    3096           0 :       } else if (_joystick_mode_ == 1) {
+    3097             : 
+    3098           0 :         mrs_msgs::TrajectoryReference trajectory;
+    3099             : 
+    3100           0 :         double dt = 0.2;
+    3101             : 
+    3102           0 :         trajectory.fly_now         = true;
+    3103           0 :         trajectory.header.frame_id = "fcu_untilted";
+    3104           0 :         trajectory.use_heading     = true;
+    3105           0 :         trajectory.dt              = dt;
+    3106             : 
+    3107           0 :         mrs_msgs::Reference point;
+    3108           0 :         point.position.x = 0;
+    3109           0 :         point.position.y = 0;
+    3110           0 :         point.position.z = 0;
+    3111           0 :         point.heading    = 0;
+    3112             : 
+    3113           0 :         trajectory.points.push_back(point);
+    3114             : 
+    3115           0 :         double speed = 1.0;
+    3116             : 
+    3117           0 :         for (int i = 0; i < 50; i++) {
+    3118             : 
+    3119           0 :           point.position.x += _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * (speed * dt);
+    3120           0 :           point.position.y += _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * (speed * dt);
+    3121           0 :           point.position.z += _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_] * (speed * dt);
+    3122           0 :           point.heading = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3123             : 
+    3124           0 :           trajectory.points.push_back(point);
+    3125             :         }
+    3126             : 
+    3127           0 :         setTrajectoryReference(trajectory);
+    3128             :       }
+    3129             :     }
+    3130             :   }
+    3131             : 
+    3132        3218 :   if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+    3133             : 
+    3134             :     // create the reference
+    3135           0 :     mrs_msgs::VelocityReferenceStampedSrv::Request request;
+    3136             : 
+    3137           0 :     double des_x       = 0;
+    3138           0 :     double des_y       = 0;
+    3139           0 :     double des_z       = 0;
+    3140           0 :     double des_heading = 0;
+    3141             : 
+    3142           0 :     bool nothing_to_do = true;
+    3143             : 
+    3144             :     // copy member variables
+    3145           0 :     mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+    3146             : 
+    3147           0 :     if (rc_channels->channels.size() < 4) {
+    3148             : 
+    3149           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+    3150             :                          int(rc_channels->channels.size()));
+    3151           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+    3152             : 
+    3153             :     } else {
+    3154             : 
+    3155           0 :       double tmp_x       = RCChannelToRange(rc_channels->channels[_rc_channel_pitch_], _rc_horizontal_speed_, 0.1);
+    3156           0 :       double tmp_y       = -RCChannelToRange(rc_channels->channels[_rc_channel_roll_], _rc_horizontal_speed_, 0.1);
+    3157           0 :       double tmp_z       = RCChannelToRange(rc_channels->channels[_rc_channel_throttle_], _rc_vertical_speed_, 0.3);
+    3158           0 :       double tmp_heading = -RCChannelToRange(rc_channels->channels[_rc_channel_heading_], _rc_heading_rate_, 0.1);
+    3159             : 
+    3160           0 :       if (abs(tmp_x) > 1e-3) {
+    3161           0 :         des_x         = tmp_x;
+    3162           0 :         nothing_to_do = false;
+    3163             :       }
+    3164             : 
+    3165           0 :       if (abs(tmp_y) > 1e-3) {
+    3166           0 :         des_y         = tmp_y;
+    3167           0 :         nothing_to_do = false;
+    3168             :       }
+    3169             : 
+    3170           0 :       if (abs(tmp_z) > 1e-3) {
+    3171           0 :         des_z         = tmp_z;
+    3172           0 :         nothing_to_do = false;
+    3173             :       }
+    3174             : 
+    3175           0 :       if (abs(tmp_heading) > 1e-3) {
+    3176           0 :         des_heading   = tmp_heading;
+    3177           0 :         nothing_to_do = false;
+    3178             :       }
+    3179             :     }
+    3180             : 
+    3181           0 :     if (!nothing_to_do) {
+    3182             : 
+    3183           0 :       request.reference.header.frame_id = "fcu_untilted";
+    3184             : 
+    3185           0 :       request.reference.reference.use_heading_rate = true;
+    3186             : 
+    3187           0 :       request.reference.reference.velocity.x   = des_x;
+    3188           0 :       request.reference.reference.velocity.y   = des_y;
+    3189           0 :       request.reference.reference.velocity.z   = des_z;
+    3190           0 :       request.reference.reference.heading_rate = des_heading;
+    3191             : 
+    3192           0 :       mrs_msgs::VelocityReferenceStampedSrv::Response response;
+    3193             : 
+    3194             :       // disable callbacks of all trackers
+    3195           0 :       std_srvs::SetBoolRequest req_enable_callbacks;
+    3196             : 
+    3197             :       // enable the callbacks for the active tracker
+    3198           0 :       req_enable_callbacks.data = true;
+    3199             :       {
+    3200           0 :         std::scoped_lock lock(mutex_tracker_list_);
+    3201             : 
+    3202           0 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3203           0 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3204             :       }
+    3205             : 
+    3206           0 :       callbacks_enabled_ = true;
+    3207             : 
+    3208           0 :       callbackVelocityReferenceService(request, response);
+    3209             : 
+    3210           0 :       callbacks_enabled_ = false;
+    3211             : 
+    3212           0 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: goto by RC with speed x=%.2f, y=%.2f, z=%.2f, heading_rate=%.2f", des_x, des_y, des_z, des_heading);
+    3213             : 
+    3214             :       // disable the callbacks back again
+    3215           0 :       req_enable_callbacks.data = false;
+    3216             :       {
+    3217           0 :         std::scoped_lock lock(mutex_tracker_list_);
+    3218             : 
+    3219           0 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3220           0 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3221             :       }
+    3222             :     }
+    3223             :   }
+    3224             : }
+    3225             : 
+    3226             : //}
+    3227             : 
+    3228             : /* //{ timerBumper() */
+    3229             : 
+    3230           0 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+    3231             : 
+    3232           0 :   if (!is_initialized_)
+    3233           0 :     return;
+    3234             : 
+    3235           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+    3236           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+    3237             : 
+    3238           0 :   if (!bumper_enabled_) {
+    3239           0 :     return;
+    3240             :   }
+    3241             : 
+    3242           0 :   if (!isFlyingNormally()) {
+    3243           0 :     return;
+    3244             :   }
+    3245             : 
+    3246           0 :   if (!got_uav_state_) {
+    3247           0 :     return;
+    3248             :   }
+    3249             : 
+    3250           0 :   if ((ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+    3251           0 :     return;
+    3252             :   }
+    3253             : 
+    3254             :   // --------------------------------------------------------------
+    3255             :   // |                      bumper repulsion                      |
+    3256             :   // --------------------------------------------------------------
+    3257             : 
+    3258           0 :   bumperPushFromObstacle();
+    3259             : }
+    3260             : 
+    3261             : //}
+    3262             : 
+    3263             : /* //{ timerPirouette() */
+    3264             : 
+    3265           0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+    3266             : 
+    3267           0 :   if (!is_initialized_)
+    3268           0 :     return;
+    3269             : 
+    3270           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+    3271           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+    3272             : 
+    3273           0 :   pirouette_iterator_++;
+    3274             : 
+    3275           0 :   double pirouette_duration  = (2 * M_PI) / _pirouette_speed_;
+    3276           0 :   double pirouette_n_steps   = pirouette_duration * _pirouette_timer_rate_;
+    3277           0 :   double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+    3278             : 
+    3279           0 :   if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+    3280             : 
+    3281           0 :     _pirouette_enabled_ = false;
+    3282           0 :     timer_pirouette_.stop();
+    3283             : 
+    3284           0 :     setCallbacks(true);
+    3285             : 
+    3286           0 :     return;
+    3287             :   }
+    3288             : 
+    3289             :   // set the reference
+    3290           0 :   mrs_msgs::ReferenceStamped reference_request;
+    3291             : 
+    3292           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3293             : 
+    3294           0 :   reference_request.header.frame_id      = "";
+    3295           0 :   reference_request.header.stamp         = ros::Time(0);
+    3296           0 :   reference_request.reference.position.x = last_tracker_cmd->position.x;
+    3297           0 :   reference_request.reference.position.y = last_tracker_cmd->position.y;
+    3298           0 :   reference_request.reference.position.z = last_tracker_cmd->position.z;
+    3299           0 :   reference_request.reference.heading    = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+    3300             : 
+    3301             :   // enable the callbacks for the active tracker
+    3302             :   {
+    3303           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3304             : 
+    3305           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3306           0 :     req_enable_callbacks.data = true;
+    3307             : 
+    3308           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3309             : 
+    3310           0 :     callbacks_enabled_ = true;
+    3311             :   }
+    3312             : 
+    3313           0 :   setReference(reference_request);
+    3314             : 
+    3315             :   {
+    3316           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3317             : 
+    3318             :     // disable the callbacks for the active tracker
+    3319           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3320           0 :     req_enable_callbacks.data = false;
+    3321             : 
+    3322           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3323             : 
+    3324           0 :     callbacks_enabled_ = false;
+    3325             :   }
+    3326             : }
+    3327             : 
+    3328             : //}
+    3329             : 
+    3330             : // --------------------------------------------------------------
+    3331             : // |                           asyncs                           |
+    3332             : // --------------------------------------------------------------
+    3333             : 
+    3334             : /* asyncControl() //{ */
+    3335             : 
+    3336        8056 : void ControlManager::asyncControl(void) {
+    3337             : 
+    3338        8056 :   if (!is_initialized_)
+    3339           0 :     return;
+    3340             : 
+    3341       16112 :   mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+    3342             : 
+    3343       24168 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("asyncControl");
+    3344       24168 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+    3345             : 
+    3346             :   // copy member variables
+    3347       16112 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3348        8056 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    3349             : 
+    3350        8056 :   if (!failsafe_triggered_) {  // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+    3351             : 
+    3352             :     // run the safety timer
+    3353             :     // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+    3354        6807 :     while (running_safety_timer_) {
+    3355             : 
+    3356          20 :       ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3357          20 :       ros::Duration wait(0.001);
+    3358          20 :       wait.sleep();
+    3359             : 
+    3360          20 :       if (!running_safety_timer_) {
+    3361          20 :         ROS_DEBUG("[ControlManager]: safety timer finished");
+    3362          20 :         break;
+    3363             :       }
+    3364             :     }
+    3365             : 
+    3366        6807 :     ros::TimerEvent safety_timer_event;
+    3367        6807 :     timerSafety(safety_timer_event);
+    3368             : 
+    3369        6807 :     updateTrackers();
+    3370             : 
+    3371        6807 :     updateControllers(uav_state);
+    3372             : 
+    3373        6807 :     if (got_constraints_) {
+    3374             : 
+    3375             :       // update the constraints to trackers, if need to
+    3376        6755 :       auto enforce = enforceControllersConstraints(current_constraints);
+    3377             : 
+    3378        6755 :       if (enforce && !constraints_being_enforced_) {
+    3379             : 
+    3380           5 :         setConstraintsToTrackers(enforce.value());
+    3381           5 :         mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+    3382             : 
+    3383           5 :         constraints_being_enforced_ = true;
+    3384             : 
+    3385           5 :         auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3386             : 
+    3387           5 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+    3388             :                           _controller_names_[active_controller_idx].c_str());
+    3389             : 
+    3390        6750 :       } else if (!enforce && constraints_being_enforced_) {
+    3391             : 
+    3392           5 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+    3393             : 
+    3394           5 :         constraints_being_enforced_ = false;
+    3395             : 
+    3396           5 :         mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+    3397             : 
+    3398           5 :         setConstraintsToTrackers(current_constraints);
+    3399             :       }
+    3400             :     }
+    3401             : 
+    3402        6807 :     publish();
+    3403             :   }
+    3404             : 
+    3405             :   // if odometry switch happened, we finish it here and turn the safety timer back on
+    3406        8056 :   if (odometry_switch_in_progress_) {
+    3407             : 
+    3408           0 :     ROS_DEBUG("[ControlManager]: starting safety timer");
+    3409           0 :     timer_safety_.start();
+    3410           0 :     ROS_DEBUG("[ControlManager]: safety timer started");
+    3411           0 :     odometry_switch_in_progress_ = false;
+    3412             : 
+    3413             :     {
+    3414           0 :       std::scoped_lock lock(mutex_uav_state_);
+    3415             : 
+    3416           0 :       ROS_INFO("[ControlManager]: odometry after switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+    3417             :                uav_state.pose.position.z, uav_heading_);
+    3418             :     }
+    3419             :   }
+    3420             : }
+    3421             : 
+    3422             : //}
+    3423             : 
+    3424             : // --------------------------------------------------------------
+    3425             : // |                          callbacks                         |
+    3426             : // --------------------------------------------------------------
+    3427             : 
+    3428             : // | --------------------- topic callbacks -------------------- |
+    3429             : 
+    3430             : /* //{ callbackOdometry() */
+    3431             : 
+    3432           0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    3433             : 
+    3434           0 :   if (!is_initialized_)
+    3435           0 :     return;
+    3436             : 
+    3437           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackOdometry");
+    3438           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+    3439             : 
+    3440           0 :   nav_msgs::OdometryConstPtr odom = msg;
+    3441             : 
+    3442             :   // | ------------------ check for time stamp ------------------ |
+    3443             : 
+    3444             :   {
+    3445           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3446             : 
+    3447           0 :     if (uav_state_.header.stamp == odom->header.stamp) {
+    3448           0 :       return;
+    3449             :     }
+    3450             :   }
+    3451             : 
+    3452             :   // | --------------------- check for nans --------------------- |
+    3453             : 
+    3454           0 :   if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+    3455           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+    3456           0 :     return;
+    3457             :   }
+    3458             : 
+    3459             :   // | ---------------------- frame switch ---------------------- |
+    3460             : 
+    3461             :   /* Odometry frame switch //{ */
+    3462             : 
+    3463             :   // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+    3464             : 
+    3465           0 :   mrs_msgs::UavState uav_state_odom;
+    3466             : 
+    3467           0 :   uav_state_odom.header   = odom->header;
+    3468           0 :   uav_state_odom.pose     = odom->pose.pose;
+    3469           0 :   uav_state_odom.velocity = odom->twist.twist;
+    3470             : 
+    3471             :   // | ----- check for change in odometry frame of reference ---- |
+    3472             : 
+    3473           0 :   if (got_uav_state_) {
+    3474             : 
+    3475           0 :     if (odom->header.frame_id != uav_state_.header.frame_id) {
+    3476             : 
+    3477           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3478             :       {
+    3479           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3480             : 
+    3481           0 :         ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+    3482             :                  uav_state_.pose.position.z, uav_heading_);
+    3483             :       }
+    3484             : 
+    3485           0 :       odometry_switch_in_progress_ = true;
+    3486             : 
+    3487             :       // we have to stop safety timer, otherwise it will interfere
+    3488           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3489           0 :       timer_safety_.stop();
+    3490           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3491             : 
+    3492             :       // wait for the safety timer to stop if its running
+    3493           0 :       while (running_safety_timer_) {
+    3494             : 
+    3495           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3496           0 :         ros::Duration wait(0.001);
+    3497           0 :         wait.sleep();
+    3498             : 
+    3499           0 :         if (!running_safety_timer_) {
+    3500           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3501           0 :           break;
+    3502             :         }
+    3503             :       }
+    3504             : 
+    3505             :       // we have to also for the oneshot control timer to finish
+    3506           0 :       while (running_async_control_) {
+    3507             : 
+    3508           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3509           0 :         ros::Duration wait(0.001);
+    3510           0 :         wait.sleep();
+    3511             : 
+    3512           0 :         if (!running_async_control_) {
+    3513           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3514           0 :           break;
+    3515             :         }
+    3516             :       }
+    3517             : 
+    3518             :       {
+    3519           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3520             : 
+    3521           0 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(uav_state_odom);
+    3522           0 :         controller_list_[active_controller_idx_]->switchOdometrySource(uav_state_odom);
+    3523             :       }
+    3524             :     }
+    3525             :   }
+    3526             : 
+    3527             :   //}
+    3528             : 
+    3529             :   // | ----------- copy the odometry to the uav_state ----------- |
+    3530             : 
+    3531             :   {
+    3532           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3533             : 
+    3534           0 :     previous_uav_state_ = uav_state_;
+    3535             : 
+    3536           0 :     uav_state_ = mrs_msgs::UavState();
+    3537             : 
+    3538           0 :     uav_state_.header           = odom->header;
+    3539           0 :     uav_state_.pose             = odom->pose.pose;
+    3540           0 :     uav_state_.velocity.angular = odom->twist.twist.angular;
+    3541             : 
+    3542             :     // transform the twist into the header's frame
+    3543             :     {
+    3544             :       // the velocity from the odometry
+    3545           0 :       geometry_msgs::Vector3Stamped speed_child_frame;
+    3546           0 :       speed_child_frame.header.frame_id = odom->child_frame_id;
+    3547           0 :       speed_child_frame.header.stamp    = odom->header.stamp;
+    3548           0 :       speed_child_frame.vector.x        = odom->twist.twist.linear.x;
+    3549           0 :       speed_child_frame.vector.y        = odom->twist.twist.linear.y;
+    3550           0 :       speed_child_frame.vector.z        = odom->twist.twist.linear.z;
+    3551             : 
+    3552           0 :       auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+    3553             : 
+    3554           0 :       if (res) {
+    3555           0 :         uav_state_.velocity.linear.x = res.value().vector.x;
+    3556           0 :         uav_state_.velocity.linear.y = res.value().vector.y;
+    3557           0 :         uav_state_.velocity.linear.z = res.value().vector.z;
+    3558             :       } else {
+    3559           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+    3560             :                            odom->header.frame_id.c_str());
+    3561           0 :         return;
+    3562             :       }
+    3563             :     }
+    3564             : 
+    3565             :     // calculate the euler angles
+    3566           0 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+    3567             : 
+    3568             :     try {
+    3569           0 :       uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+    3570             :     }
+    3571           0 :     catch (...) {
+    3572           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+    3573             :     }
+    3574             : 
+    3575           0 :     transformer_->setDefaultFrame(odom->header.frame_id);
+    3576             : 
+    3577           0 :     got_uav_state_ = true;
+    3578             :   }
+    3579             : 
+    3580             :   // run the control loop asynchronously in an OneShotTimer
+    3581             :   // but only if its not already running
+    3582           0 :   if (!running_async_control_) {
+    3583             : 
+    3584           0 :     running_async_control_ = true;
+    3585             : 
+    3586           0 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3587             :   }
+    3588             : }
+    3589             : 
+    3590             : //}
+    3591             : 
+    3592             : /* //{ callbackUavState() */
+    3593             : 
+    3594        8866 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    3595             : 
+    3596        8866 :   if (!is_initialized_)
+    3597         809 :     return;
+    3598             : 
+    3599       17732 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackUavState");
+    3600       17732 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+    3601             : 
+    3602        8866 :   mrs_msgs::UavStateConstPtr uav_state = msg;
+    3603             : 
+    3604             :   // | ------------------ check for time stamp ------------------ |
+    3605             : 
+    3606             :   {
+    3607        8866 :     std::scoped_lock lock(mutex_uav_state_);
+    3608             : 
+    3609        8866 :     if (uav_state_.header.stamp == uav_state->header.stamp) {
+    3610         809 :       return;
+    3611             :     }
+    3612             :   }
+    3613             : 
+    3614             :   // | --------------------- check for nans --------------------- |
+    3615             : 
+    3616        8057 :   if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+    3617           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+    3618           0 :     return;
+    3619             :   }
+    3620             : 
+    3621             :   // | -------------------- check for hiccups ------------------- |
+    3622             : 
+    3623             :   /* hickup detection //{ */
+    3624             : 
+    3625        8057 :   double alpha               = 0.99;
+    3626        8057 :   double alpha2              = 0.666;
+    3627        8057 :   double uav_state_count_lim = 1000;
+    3628             : 
+    3629        8057 :   double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+    3630             : 
+    3631             :   // belive only reasonable numbers
+    3632        8057 :   if (uav_state_dt <= 1.0) {
+    3633             : 
+    3634        8043 :     uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+    3635             : 
+    3636        8043 :     if (uav_state_count_ < uav_state_count_lim) {
+    3637        6851 :       uav_state_count_++;
+    3638             :     }
+    3639             :   }
+    3640             : 
+    3641        8057 :   if (uav_state_count_ == uav_state_count_lim) {
+    3642             : 
+    3643             :     /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+    3644             : 
+    3645        1198 :     if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+    3646             : 
+    3647         685 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+    3648             : 
+    3649         513 :     } else if (uav_state_avg_dt_ > 0.0001) {
+    3650             : 
+    3651         513 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+    3652             :     }
+    3653             : 
+    3654        1198 :     if (uav_state_hiccup_factor_ > 3.141592653) {
+    3655             : 
+    3656             :       /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+    3657             : 
+    3658           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3659           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3660           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3661           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |            UAV_STATE has a large hiccup factor!            |");
+    3662           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           hint, hint: you are probably rosbagging          |");
+    3663           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           lot of data or publishing lot of large           |");
+    3664           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |          messages without mutual nodelet managers.         |");
+    3665           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3666           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3667           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3668             :     }
+    3669             :   }
+    3670             : 
+    3671             :   //}
+    3672             : 
+    3673             :   // | ---------------------- frame switch ---------------------- |
+    3674             : 
+    3675             :   /* frame switch //{ */
+    3676             : 
+    3677             :   // | ----- check for change in odometry frame of reference ---- |
+    3678             : 
+    3679        8057 :   if (got_uav_state_) {
+    3680             : 
+    3681        8050 :     if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+    3682             : 
+    3683           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3684             :       {
+    3685           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3686             : 
+    3687           0 :         ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+    3688             :                  uav_state_.pose.position.z, uav_heading_);
+    3689             :       }
+    3690             : 
+    3691           0 :       odometry_switch_in_progress_ = true;
+    3692             : 
+    3693             :       // we have to stop safety timer, otherwise it will interfere
+    3694           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3695           0 :       timer_safety_.stop();
+    3696           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3697             : 
+    3698             :       // wait for the safety timer to stop if its running
+    3699           0 :       while (running_safety_timer_) {
+    3700             : 
+    3701           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3702           0 :         ros::Duration wait(0.001);
+    3703           0 :         wait.sleep();
+    3704             : 
+    3705           0 :         if (!running_safety_timer_) {
+    3706           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3707           0 :           break;
+    3708             :         }
+    3709             :       }
+    3710             : 
+    3711             :       // we have to also for the oneshot control timer to finish
+    3712           0 :       while (running_async_control_) {
+    3713             : 
+    3714           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3715           0 :         ros::Duration wait(0.001);
+    3716           0 :         wait.sleep();
+    3717             : 
+    3718           0 :         if (!running_async_control_) {
+    3719           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3720           0 :           break;
+    3721             :         }
+    3722             :       }
+    3723             : 
+    3724             :       {
+    3725           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3726             : 
+    3727           0 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(*uav_state);
+    3728           0 :         controller_list_[active_controller_idx_]->switchOdometrySource(*uav_state);
+    3729             :       }
+    3730             :     }
+    3731             :   }
+    3732             : 
+    3733             :   //}
+    3734             : 
+    3735             :   // --------------------------------------------------------------
+    3736             :   // |           copy the UavState message for later use          |
+    3737             :   // --------------------------------------------------------------
+    3738             : 
+    3739             :   {
+    3740        8057 :     std::scoped_lock lock(mutex_uav_state_);
+    3741             : 
+    3742        8057 :     previous_uav_state_ = uav_state_;
+    3743             : 
+    3744        8057 :     uav_state_ = *uav_state;
+    3745             : 
+    3746        8057 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+    3747             : 
+    3748             :     try {
+    3749        8057 :       uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+    3750             :     }
+    3751           0 :     catch (...) {
+    3752           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+    3753             :     }
+    3754             : 
+    3755        8057 :     transformer_->setDefaultFrame(uav_state->header.frame_id);
+    3756             : 
+    3757        8057 :     got_uav_state_ = true;
+    3758             :   }
+    3759             : 
+    3760             :   // run the control loop asynchronously in an OneShotTimer
+    3761             :   // but only if its not already running
+    3762        8057 :   if (!running_async_control_) {
+    3763             : 
+    3764        8056 :     running_async_control_ = true;
+    3765             : 
+    3766        8056 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3767             :   }
+    3768             : }
+    3769             : 
+    3770             : //}
+    3771             : 
+    3772             : /* //{ callbackGNSS() */
+    3773             : 
+    3774        5273 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    3775             : 
+    3776        5273 :   if (!is_initialized_)
+    3777          10 :     return;
+    3778             : 
+    3779       15789 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGNSS");
+    3780       15789 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+    3781             : 
+    3782        5263 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    3783             : }
+    3784             : 
+    3785             : //}
+    3786             : 
+    3787             : /* callbackJoystick() //{ */
+    3788             : 
+    3789           0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+    3790             : 
+    3791           0 :   if (!is_initialized_)
+    3792           0 :     return;
+    3793             : 
+    3794           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackJoystick");
+    3795           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3796             : 
+    3797             :   // copy member variables
+    3798           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    3799           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3800             : 
+    3801           0 :   sensor_msgs::JoyConstPtr joystick_data = msg;
+    3802             : 
+    3803             :   // TODO check if the array is smaller than the largest idx
+    3804           0 :   if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+    3805           0 :     return;
+    3806             :   }
+    3807             : 
+    3808             :   // | ---- switching back to fallback tracker and controller --- |
+    3809             : 
+    3810             :   // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+    3811           0 :   if ((joystick_data->buttons[_channel_A_] == 1 || joystick_data->buttons[_channel_B_] == 1 || joystick_data->buttons[_channel_X_] == 1 ||
+    3812           0 :        joystick_data->buttons[_channel_Y_] == 1) &&
+    3813           0 :       active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+    3814             : 
+    3815           0 :     ROS_INFO("[ControlManager]: switching from joystick to normal control");
+    3816             : 
+    3817           0 :     switchTracker(_joystick_fallback_tracker_name_);
+    3818           0 :     switchController(_joystick_fallback_controller_name_);
+    3819             : 
+    3820           0 :     joystick_goto_enabled_ = false;
+    3821             :   }
+    3822             : 
+    3823             :   // | ------- joystick control activation ------- |
+    3824             : 
+    3825             :   // if start button was pressed
+    3826           0 :   if (joystick_data->buttons[_channel_start_] == 1) {
+    3827             : 
+    3828           0 :     if (!joystick_start_pressed_) {
+    3829             : 
+    3830           0 :       ROS_INFO("[ControlManager]: joystick start button pressed");
+    3831             : 
+    3832           0 :       joystick_start_pressed_    = true;
+    3833           0 :       joystick_start_press_time_ = ros::Time::now();
+    3834             :     }
+    3835             : 
+    3836           0 :   } else if (joystick_start_pressed_) {
+    3837             : 
+    3838           0 :     ROS_INFO("[ControlManager]: joystick start button released");
+    3839             : 
+    3840           0 :     joystick_start_pressed_    = false;
+    3841           0 :     joystick_start_press_time_ = ros::Time(0);
+    3842             :   }
+    3843             : 
+    3844             :   // | ---------------- Joystick goto activation ---------------- |
+    3845             : 
+    3846             :   // if back button was pressed
+    3847           0 :   if (joystick_data->buttons[_channel_back_] == 1) {
+    3848             : 
+    3849           0 :     if (!joystick_back_pressed_) {
+    3850             : 
+    3851           0 :       ROS_INFO("[ControlManager]: joystick back button pressed");
+    3852             : 
+    3853           0 :       joystick_back_pressed_    = true;
+    3854           0 :       joystick_back_press_time_ = ros::Time::now();
+    3855             :     }
+    3856             : 
+    3857           0 :   } else if (joystick_back_pressed_) {
+    3858             : 
+    3859           0 :     ROS_INFO("[ControlManager]: joystick back button released");
+    3860             : 
+    3861           0 :     joystick_back_pressed_    = false;
+    3862           0 :     joystick_back_press_time_ = ros::Time(0);
+    3863             :   }
+    3864             : 
+    3865             :   // | ------------------------ Failsafes ----------------------- |
+    3866             : 
+    3867             :   // if LT and RT buttons are both pressed down
+    3868           0 :   if (joystick_data->axes[_channel_LT_] < -0.99 && joystick_data->axes[_channel_RT_] < -0.99) {
+    3869             : 
+    3870           0 :     if (!joystick_failsafe_pressed_) {
+    3871             : 
+    3872           0 :       ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+    3873             : 
+    3874           0 :       joystick_failsafe_pressed_    = true;
+    3875           0 :       joystick_failsafe_press_time_ = ros::Time::now();
+    3876             :     }
+    3877             : 
+    3878           0 :   } else if (joystick_failsafe_pressed_) {
+    3879             : 
+    3880           0 :     ROS_INFO("[ControlManager]: joystick Failsafe released");
+    3881             : 
+    3882           0 :     joystick_failsafe_pressed_    = false;
+    3883           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3884             :   }
+    3885             : 
+    3886             :   // if left and right joypads are both pressed down
+    3887           0 :   if (joystick_data->buttons[_channel_L_joy_] == 1 && joystick_data->buttons[_channel_R_joy_] == 1) {
+    3888             : 
+    3889           0 :     if (!joystick_eland_pressed_) {
+    3890             : 
+    3891           0 :       ROS_INFO("[ControlManager]: joystick eland pressed");
+    3892             : 
+    3893           0 :       joystick_eland_pressed_    = true;
+    3894           0 :       joystick_eland_press_time_ = ros::Time::now();
+    3895             :     }
+    3896             : 
+    3897           0 :   } else if (joystick_eland_pressed_) {
+    3898             : 
+    3899           0 :     ROS_INFO("[ControlManager]: joystick eland released");
+    3900             : 
+    3901           0 :     joystick_eland_pressed_    = false;
+    3902           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3903             :   }
+    3904             : }
+    3905             : 
+    3906             : //}
+    3907             : 
+    3908             : /* //{ callbackHwApiStatus() */
+    3909             : 
+    3910       37558 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+    3911             : 
+    3912       37558 :   if (!is_initialized_)
+    3913          30 :     return;
+    3914             : 
+    3915      112584 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+    3916      112584 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+    3917             : 
+    3918       75056 :   mrs_msgs::HwApiStatusConstPtr state = msg;
+    3919             : 
+    3920             :   // | ------ detect and print the changes in offboard mode ----- |
+    3921       37528 :   if (state->offboard) {
+    3922             : 
+    3923       27905 :     if (!offboard_mode_) {
+    3924           7 :       offboard_mode_          = true;
+    3925           7 :       offboard_mode_was_true_ = true;
+    3926           7 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+    3927             :     }
+    3928             : 
+    3929             :   } else {
+    3930             : 
+    3931        9623 :     if (offboard_mode_) {
+    3932           0 :       offboard_mode_ = false;
+    3933           0 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+    3934             :     }
+    3935             :   }
+    3936             : 
+    3937             :   // | --------- detect and print the changes in arming --------- |
+    3938       37528 :   if (state->armed == true) {
+    3939             : 
+    3940       27913 :     if (!armed_) {
+    3941           7 :       armed_ = true;
+    3942           7 :       ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+    3943             :     }
+    3944             : 
+    3945             :   } else {
+    3946             : 
+    3947        9615 :     if (armed_) {
+    3948           5 :       armed_ = false;
+    3949           5 :       ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+    3950             :     }
+    3951             :   }
+    3952             : }
+    3953             : 
+    3954             : //}
+    3955             : 
+    3956             : /* //{ callbackRC() */
+    3957             : 
+    3958        1064 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+    3959             : 
+    3960        1064 :   if (!is_initialized_)
+    3961           0 :     return;
+    3962             : 
+    3963        3192 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackRC");
+    3964        3192 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+    3965             : 
+    3966        2128 :   mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+    3967             : 
+    3968        1064 :   ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+    3969             : 
+    3970             :   // | ------------------- rc joystic control ------------------- |
+    3971             : 
+    3972             :   // when the switch change its position
+    3973        1064 :   if (_rc_goto_enabled_) {
+    3974             : 
+    3975        1064 :     if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+    3976             : 
+    3977           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+    3978             :                          int(rc->channels.size()));
+    3979             : 
+    3980             :     } else {
+    3981             : 
+    3982        1064 :       bool channel_low  = rc->channels[_rc_joystick_channel_] < (0.5 - RC_DEADBAND) ? true : false;
+    3983        1064 :       bool channel_high = rc->channels[_rc_joystick_channel_] > (0.5 + RC_DEADBAND) ? true : false;
+    3984             : 
+    3985        1064 :       if (channel_low) {
+    3986        1064 :         rc_joystick_channel_was_low_ = true;
+    3987             :       }
+    3988             : 
+    3989             :       // rc control activation
+    3990        1064 :       if (!rc_goto_active_) {
+    3991             : 
+    3992        1064 :         if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+    3993             : 
+    3994           0 :           if (isFlyingNormally()) {
+    3995             : 
+    3996           0 :             ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+    3997             : 
+    3998           0 :             callbacks_enabled_ = false;
+    3999             : 
+    4000           0 :             std_srvs::SetBoolRequest req_goto_out;
+    4001           0 :             req_goto_out.data = false;
+    4002             : 
+    4003           0 :             std_srvs::SetBoolRequest req_enable_callbacks;
+    4004           0 :             req_enable_callbacks.data = callbacks_enabled_;
+    4005             : 
+    4006             :             {
+    4007           0 :               std::scoped_lock lock(mutex_tracker_list_);
+    4008             : 
+    4009             :               // disable callbacks of all trackers
+    4010           0 :               for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4011           0 :                 tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4012             :               }
+    4013             :             }
+    4014             : 
+    4015           0 :             rc_goto_active_ = true;
+    4016             : 
+    4017             :           } else {
+    4018             : 
+    4019           0 :             ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+    4020           0 :           }
+    4021             : 
+    4022        1064 :         } else if (channel_high && !rc_joystick_channel_was_low_) {
+    4023             : 
+    4024           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+    4025             :         }
+    4026             :       }
+    4027             : 
+    4028             :       // rc control deactivation
+    4029        1064 :       if (rc_goto_active_ && channel_low) {
+    4030             : 
+    4031           0 :         ROS_INFO("[ControlManager]: deactivating RC joystick");
+    4032             : 
+    4033           0 :         callbacks_enabled_ = true;
+    4034             : 
+    4035           0 :         std_srvs::SetBoolRequest req_goto_out;
+    4036           0 :         req_goto_out.data = true;
+    4037             : 
+    4038           0 :         std_srvs::SetBoolRequest req_enable_callbacks;
+    4039           0 :         req_enable_callbacks.data = callbacks_enabled_;
+    4040             : 
+    4041             :         {
+    4042           0 :           std::scoped_lock lock(mutex_tracker_list_);
+    4043             : 
+    4044             :           // enable callbacks of all trackers
+    4045           0 :           for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4046           0 :             tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4047             :           }
+    4048             :         }
+    4049             : 
+    4050           0 :         rc_goto_active_ = false;
+    4051             :       }
+    4052             : 
+    4053             :       // do not forget to update the last... variable
+    4054             :       // only do that if its out of the deadband
+    4055        1064 :       if (channel_high || channel_low) {
+    4056        1064 :         rc_joystick_channel_last_value_ = rc->channels[_rc_joystick_channel_];
+    4057             :       }
+    4058             :     }
+    4059             :   }
+    4060             : 
+    4061             :   // | ------------------------ rc eland ------------------------ |
+    4062        1064 :   if (_rc_escalating_failsafe_enabled_) {
+    4063             : 
+    4064        1064 :     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+    4065             : 
+    4066           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+    4067             :                          int(rc->channels.size()));
+    4068             : 
+    4069             :     } else {
+    4070             : 
+    4071        1064 :       if (rc->channels[_rc_escalating_failsafe_channel_] >= _rc_escalating_failsafe_threshold_) {
+    4072             : 
+    4073           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+    4074             : 
+    4075           0 :         auto [success, message] = escalatingFailsafe();
+    4076             : 
+    4077           0 :         if (success) {
+    4078           0 :           rc_escalating_failsafe_triggered_ = true;
+    4079             :         }
+    4080             :       }
+    4081             :     }
+    4082             :   }
+    4083             : }
+    4084             : 
+    4085             : //}
+    4086             : 
+    4087             : // | --------------------- topic timeouts --------------------- |
+    4088             : 
+    4089             : /* timeoutUavState() //{ */
+    4090             : 
+    4091           0 : void ControlManager::timeoutUavState(const double& missing_for) {
+    4092             : 
+    4093           0 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    4094             : 
+    4095           0 :   if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+    4096             : 
+    4097             :     // We need to fire up timerFailsafe, which will regularly trigger the controllers
+    4098             :     // in place of the callbackUavState/callbackOdometry().
+    4099             : 
+    4100           0 :     ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+    4101             : 
+    4102           0 :     failsafe();
+    4103             :   }
+    4104           0 : }
+    4105             : 
+    4106             : //}
+    4107             : 
+    4108             : // | -------------------- service callbacks ------------------- |
+    4109             : 
+    4110             : /* //{ callbackSwitchTracker() */
+    4111             : 
+    4112          16 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4113             : 
+    4114          16 :   if (!is_initialized_)
+    4115           0 :     return false;
+    4116             : 
+    4117          16 :   if (failsafe_triggered_ || eland_triggered_) {
+    4118             : 
+    4119           0 :     std::stringstream ss;
+    4120           0 :     ss << "can not switch tracker, eland or failsafe active";
+    4121             : 
+    4122           0 :     res.message = ss.str();
+    4123           0 :     res.success = false;
+    4124             : 
+    4125           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4126             : 
+    4127           0 :     return true;
+    4128             :   }
+    4129             : 
+    4130          16 :   auto [success, response] = switchTracker(req.value);
+    4131             : 
+    4132          16 :   res.success = success;
+    4133          16 :   res.message = response;
+    4134             : 
+    4135          16 :   return true;
+    4136             : }
+    4137             : 
+    4138             : //}
+    4139             : 
+    4140             : /* callbackSwitchController() //{ */
+    4141             : 
+    4142          15 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4143             : 
+    4144          15 :   if (!is_initialized_)
+    4145           0 :     return false;
+    4146             : 
+    4147          15 :   if (failsafe_triggered_ || eland_triggered_) {
+    4148             : 
+    4149           0 :     std::stringstream ss;
+    4150           0 :     ss << "can not switch controller, eland or failsafe active";
+    4151             : 
+    4152           0 :     res.message = ss.str();
+    4153           0 :     res.success = false;
+    4154             : 
+    4155           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4156             : 
+    4157           0 :     return true;
+    4158             :   }
+    4159             : 
+    4160          15 :   auto [success, response] = switchController(req.value);
+    4161             : 
+    4162          15 :   res.success = success;
+    4163          15 :   res.message = response;
+    4164             : 
+    4165          15 :   return true;
+    4166             : }
+    4167             : 
+    4168             : //}
+    4169             : 
+    4170             : /* //{ callbackSwitchTracker() */
+    4171             : 
+    4172           0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4173             : 
+    4174           0 :   if (!is_initialized_)
+    4175           0 :     return false;
+    4176             : 
+    4177           0 :   std::stringstream message;
+    4178             : 
+    4179           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4180             : 
+    4181           0 :     message << "can not reset tracker, eland or failsafe active";
+    4182             : 
+    4183           0 :     res.message = message.str();
+    4184           0 :     res.success = false;
+    4185             : 
+    4186           0 :     ROS_WARN_STREAM("[ControlManager]: " << message.str());
+    4187             : 
+    4188           0 :     return true;
+    4189             :   }
+    4190             : 
+    4191             :   // reactivate the current tracker
+    4192             :   {
+    4193           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4194             : 
+    4195           0 :     std::string tracker_name = _tracker_names_[active_tracker_idx_];
+    4196             : 
+    4197           0 :     bool succ = tracker_list_[active_tracker_idx_]->resetStatic();
+    4198             : 
+    4199           0 :     if (succ) {
+    4200           0 :       message << "the tracker '" << tracker_name << "' was reset";
+    4201           0 :       ROS_INFO_STREAM("[ControlManager]: " << message.str());
+    4202             :     } else {
+    4203           0 :       message << "the tracker '" << tracker_name << "' reset failed!";
+    4204           0 :       ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+    4205             :     }
+    4206             :   }
+    4207             : 
+    4208           0 :   res.message = message.str();
+    4209           0 :   res.success = true;
+    4210             : 
+    4211           0 :   return true;
+    4212             : }
+    4213             : 
+    4214             : //}
+    4215             : 
+    4216             : /* //{ callbackEHover() */
+    4217             : 
+    4218           0 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4219             : 
+    4220           0 :   if (!is_initialized_)
+    4221           0 :     return false;
+    4222             : 
+    4223           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4224             : 
+    4225           0 :     std::stringstream ss;
+    4226           0 :     ss << "can not switch controller, eland or failsafe active";
+    4227             : 
+    4228           0 :     res.message = ss.str();
+    4229           0 :     res.success = false;
+    4230             : 
+    4231           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4232             : 
+    4233           0 :     return true;
+    4234             :   }
+    4235             : 
+    4236           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+    4237             : 
+    4238           0 :   auto [success, message] = ehover();
+    4239             : 
+    4240           0 :   res.success = success;
+    4241           0 :   res.message = message;
+    4242             : 
+    4243           0 :   return true;
+    4244             : }
+    4245             : 
+    4246             : //}
+    4247             : 
+    4248             : /* callbackFailsafe() //{ */
+    4249             : 
+    4250           0 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4251             : 
+    4252           0 :   if (!is_initialized_)
+    4253           0 :     return false;
+    4254             : 
+    4255           0 :   if (failsafe_triggered_) {
+    4256             : 
+    4257           0 :     std::stringstream ss;
+    4258           0 :     ss << "can not activate failsafe, it is already active";
+    4259             : 
+    4260           0 :     res.message = ss.str();
+    4261           0 :     res.success = false;
+    4262             : 
+    4263           0 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4264             : 
+    4265           0 :     return true;
+    4266             :   }
+    4267             : 
+    4268           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+    4269             : 
+    4270           0 :   auto [success, message] = failsafe();
+    4271             : 
+    4272           0 :   res.success = success;
+    4273           0 :   res.message = message;
+    4274             : 
+    4275           0 :   return true;
+    4276             : }
+    4277             : 
+    4278             : //}
+    4279             : 
+    4280             : /* callbackFailsafeEscalating() //{ */
+    4281             : 
+    4282           0 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4283             : 
+    4284           0 :   if (!is_initialized_)
+    4285           0 :     return false;
+    4286             : 
+    4287           0 :   if (_service_escalating_failsafe_enabled_) {
+    4288             : 
+    4289           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+    4290             : 
+    4291           0 :     auto [success, message] = escalatingFailsafe();
+    4292             : 
+    4293           0 :     res.success = success;
+    4294           0 :     res.message = message;
+    4295             : 
+    4296             :   } else {
+    4297             : 
+    4298           0 :     std::stringstream ss;
+    4299           0 :     ss << "escalating failsafe is disabled";
+    4300             : 
+    4301           0 :     res.success = false;
+    4302           0 :     res.message = ss.str();
+    4303             : 
+    4304           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+    4305             :   }
+    4306             : 
+    4307           0 :   return true;
+    4308             : }
+    4309             : 
+    4310             : //}
+    4311             : 
+    4312             : /* //{ callbackELand() */
+    4313             : 
+    4314           1 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4315             : 
+    4316           1 :   if (!is_initialized_)
+    4317           0 :     return false;
+    4318             : 
+    4319           1 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+    4320             : 
+    4321           1 :   auto [success, message] = eland();
+    4322             : 
+    4323           1 :   res.success = success;
+    4324           1 :   res.message = message;
+    4325             : 
+    4326           1 :   return true;
+    4327             : }
+    4328             : 
+    4329             : //}
+    4330             : 
+    4331             : /* //{ callbackParachute() */
+    4332             : 
+    4333           0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4334             : 
+    4335           0 :   if (!is_initialized_)
+    4336           0 :     return false;
+    4337             : 
+    4338           0 :   if (!_parachute_enabled_) {
+    4339             : 
+    4340           0 :     std::stringstream ss;
+    4341           0 :     ss << "parachute disabled";
+    4342           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4343           0 :     res.message = ss.str();
+    4344           0 :     res.success = false;
+    4345             :   }
+    4346             : 
+    4347           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+    4348             : 
+    4349           0 :   auto [success, message] = deployParachute();
+    4350             : 
+    4351           0 :   res.success = success;
+    4352           0 :   res.message = message;
+    4353             : 
+    4354           0 :   return true;
+    4355             : }
+    4356             : 
+    4357             : //}
+    4358             : 
+    4359             : /* //{ callbackToggleOutput() */
+    4360             : 
+    4361           7 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4362             : 
+    4363           7 :   if (!is_initialized_)
+    4364           0 :     return false;
+    4365             : 
+    4366           7 :   ROS_INFO("[ControlManager]: toggling output by service");
+    4367             : 
+    4368             :   // copy member variables
+    4369          14 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4370             : 
+    4371          14 :   std::stringstream ss;
+    4372             : 
+    4373           7 :   bool prereq_check = true;
+    4374             : 
+    4375             :   {
+    4376          14 :     mrs_msgs::ReferenceStamped current_coord;
+    4377           7 :     current_coord.header.frame_id      = uav_state.header.frame_id;
+    4378           7 :     current_coord.reference.position.x = uav_state.pose.position.x;
+    4379           7 :     current_coord.reference.position.y = uav_state.pose.position.y;
+    4380             : 
+    4381           7 :     if (!isPointInSafetyArea2d(current_coord)) {
+    4382           0 :       ss << "cannot toggle output, the UAV is outside of the safety area!";
+    4383           0 :       prereq_check = false;
+    4384             :     }
+    4385             :   }
+    4386             : 
+    4387           7 :   if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+    4388           0 :     ss << "cannot toggle output ON, we landed in emergency";
+    4389           0 :     prereq_check = false;
+    4390             :   }
+    4391             : 
+    4392           7 :   if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+    4393           0 :     ss << "cannot toggle output ON, missing HW API status!";
+    4394           0 :     prereq_check = false;
+    4395             :   }
+    4396             : 
+    4397           7 :   if (bumper_enabled_ && !sh_bumper_.hasMsg()) {
+    4398           0 :     ss << "cannot toggle output ON, missing bumper data!";
+    4399           0 :     prereq_check = false;
+    4400             :   }
+    4401             : 
+    4402           7 :   if (!prereq_check) {
+    4403             : 
+    4404           0 :     res.message = ss.str();
+    4405           0 :     res.success = false;
+    4406             : 
+    4407           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4408             : 
+    4409           0 :     return false;
+    4410             : 
+    4411             :   } else {
+    4412             : 
+    4413           7 :     toggleOutput(req.data);
+    4414             : 
+    4415           7 :     ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+    4416           7 :     res.message = ss.str();
+    4417           7 :     res.success = true;
+    4418             : 
+    4419           7 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4420             : 
+    4421           7 :     publishDiagnostics();
+    4422             : 
+    4423           7 :     return true;
+    4424             :   }
+    4425             : }
+    4426             : 
+    4427             : //}
+    4428             : 
+    4429             : /* callbackArm() //{ */
+    4430             : 
+    4431           1 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4432             : 
+    4433           1 :   if (!is_initialized_)
+    4434           0 :     return false;
+    4435             : 
+    4436           1 :   ROS_INFO("[ControlManager]: arming by service");
+    4437             : 
+    4438           2 :   std::stringstream ss;
+    4439             : 
+    4440           1 :   if (failsafe_triggered_ || eland_triggered_) {
+    4441             : 
+    4442           0 :     ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+    4443             : 
+    4444           0 :     res.message = ss.str();
+    4445           0 :     res.success = false;
+    4446             : 
+    4447           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4448             : 
+    4449           0 :     return true;
+    4450             :   }
+    4451             : 
+    4452           1 :   if (req.data) {
+    4453             : 
+    4454           0 :     ss << "this service is not allowed to arm the UAV";
+    4455           0 :     res.success = false;
+    4456           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4457             : 
+    4458             :   } else {
+    4459             : 
+    4460           2 :     auto [success, message] = arming(false);
+    4461             : 
+    4462           1 :     if (success) {
+    4463             : 
+    4464           1 :       ss << "disarmed";
+    4465           1 :       res.success = true;
+    4466           1 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4467             : 
+    4468             :     } else {
+    4469             : 
+    4470           0 :       ss << "could not disarm: " << message;
+    4471           0 :       res.success = false;
+    4472           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4473             :     }
+    4474             :   }
+    4475             : 
+    4476           1 :   res.message = ss.str();
+    4477             : 
+    4478           1 :   return true;
+    4479             : }
+    4480             : 
+    4481             : //}
+    4482             : 
+    4483             : /* //{ callbackEnableCallbacks() */
+    4484             : 
+    4485           1 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4486             : 
+    4487           1 :   if (!is_initialized_)
+    4488           0 :     return false;
+    4489             : 
+    4490           1 :   setCallbacks(req.data);
+    4491             : 
+    4492           1 :   std::stringstream ss;
+    4493             : 
+    4494           1 :   ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+    4495             : 
+    4496           1 :   res.message = ss.str();
+    4497           1 :   res.success = true;
+    4498             : 
+    4499           1 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4500             : 
+    4501           1 :   return true;
+    4502             : }
+    4503             : 
+    4504             : //}
+    4505             : 
+    4506             : /* callbackSetConstraints() //{ */
+    4507             : 
+    4508           7 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+    4509             : 
+    4510           7 :   if (!is_initialized_) {
+    4511           0 :     res.message = "not initialized";
+    4512           0 :     res.success = false;
+    4513           0 :     return true;
+    4514             :   }
+    4515             : 
+    4516             :   {
+    4517          14 :     std::scoped_lock lock(mutex_constraints_);
+    4518             : 
+    4519           7 :     current_constraints_ = req;
+    4520             : 
+    4521           7 :     auto enforced = enforceControllersConstraints(current_constraints_);
+    4522             : 
+    4523           7 :     if (enforced) {
+    4524           0 :       sanitized_constraints_      = enforced.value();
+    4525           0 :       constraints_being_enforced_ = true;
+    4526             :     } else {
+    4527           7 :       sanitized_constraints_      = req;
+    4528           7 :       constraints_being_enforced_ = false;
+    4529             :     }
+    4530             : 
+    4531           7 :     got_constraints_ = true;
+    4532             : 
+    4533           7 :     setConstraintsToControllers(current_constraints_);
+    4534           7 :     setConstraintsToTrackers(sanitized_constraints_);
+    4535             :   }
+    4536             : 
+    4537           7 :   res.message = "setting constraints";
+    4538           7 :   res.success = true;
+    4539             : 
+    4540           7 :   return true;
+    4541             : }
+    4542             : 
+    4543             : //}
+    4544             : 
+    4545             : /* //{ callbackEmergencyReference() */
+    4546             : 
+    4547           0 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    4548             : 
+    4549           0 :   if (!is_initialized_)
+    4550           0 :     return false;
+    4551             : 
+    4552           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4553             : 
+    4554           0 :   callbacks_enabled_ = false;
+    4555             : 
+    4556           0 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    4557             : 
+    4558           0 :   std::stringstream ss;
+    4559             : 
+    4560             :   // transform the reference to the current frame
+    4561           0 :   mrs_msgs::ReferenceStamped original_reference;
+    4562           0 :   original_reference.header    = req.header;
+    4563           0 :   original_reference.reference = req.reference;
+    4564             : 
+    4565           0 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    4566             : 
+    4567           0 :   if (!ret) {
+    4568             : 
+    4569           0 :     ss << "the emergency reference could not be transformed";
+    4570             : 
+    4571           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4572           0 :     res.message = ss.str();
+    4573           0 :     res.success = false;
+    4574           0 :     return true;
+    4575             :   }
+    4576             : 
+    4577           0 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    4578             : 
+    4579           0 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    4580             : 
+    4581           0 :   mrs_msgs::ReferenceSrvRequest req_goto_out;
+    4582           0 :   req_goto_out.reference = transformed_reference.reference;
+    4583             : 
+    4584             :   {
+    4585           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4586             : 
+    4587             :     // disable callbacks of all trackers
+    4588           0 :     req_enable_callbacks.data = false;
+    4589           0 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4590           0 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4591             :     }
+    4592             : 
+    4593             :     // enable the callbacks for the active tracker
+    4594           0 :     req_enable_callbacks.data = true;
+    4595           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4596             : 
+    4597             :     // call the setReference()
+    4598           0 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    4599           0 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    4600             : 
+    4601             :     // disable the callbacks back again
+    4602           0 :     req_enable_callbacks.data = false;
+    4603           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4604             : 
+    4605           0 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    4606           0 :       res.message = tracker_response->message;
+    4607           0 :       res.success = tracker_response->success;
+    4608             :     } else {
+    4609           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    4610           0 :       res.message = ss.str();
+    4611           0 :       res.success = false;
+    4612             :     }
+    4613             :   }
+    4614             : 
+    4615           0 :   return true;
+    4616             : }
+    4617             : 
+    4618             : //}
+    4619             : 
+    4620             : /* callbackPirouette() //{ */
+    4621             : 
+    4622           0 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4623             : 
+    4624           0 :   if (!is_initialized_)
+    4625           0 :     return false;
+    4626             : 
+    4627             :   // copy member variables
+    4628           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4629             : 
+    4630             :   double uav_heading;
+    4631             :   try {
+    4632           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    4633             :   }
+    4634           0 :   catch (...) {
+    4635           0 :     std::stringstream ss;
+    4636           0 :     ss << "could not calculate the UAV heading to initialize the pirouette";
+    4637             : 
+    4638           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4639             : 
+    4640           0 :     res.message = ss.str();
+    4641           0 :     res.success = false;
+    4642             : 
+    4643           0 :     return false;
+    4644             :   }
+    4645             : 
+    4646           0 :   if (_pirouette_enabled_) {
+    4647           0 :     res.success = false;
+    4648           0 :     res.message = "already active";
+    4649           0 :     return true;
+    4650             :   }
+    4651             : 
+    4652           0 :   if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+    4653             : 
+    4654           0 :     std::stringstream ss;
+    4655           0 :     ss << "can not activate the pirouette, eland or failsafe active";
+    4656             : 
+    4657           0 :     res.message = ss.str();
+    4658           0 :     res.success = false;
+    4659             : 
+    4660           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4661             : 
+    4662           0 :     return true;
+    4663             :   }
+    4664             : 
+    4665           0 :   _pirouette_enabled_ = true;
+    4666             : 
+    4667           0 :   setCallbacks(false);
+    4668             : 
+    4669           0 :   pirouette_initial_heading_ = uav_heading;
+    4670           0 :   pirouette_iterator_        = 0;
+    4671           0 :   timer_pirouette_.start();
+    4672             : 
+    4673           0 :   res.success = true;
+    4674           0 :   res.message = "activated";
+    4675             : 
+    4676           0 :   return true;
+    4677             : }
+    4678             : 
+    4679             : //}
+    4680             : 
+    4681             : /* callbackUseJoystick() //{ */
+    4682             : 
+    4683           0 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4684             : 
+    4685           0 :   if (!is_initialized_) {
+    4686           0 :     return false;
+    4687             :   }
+    4688             : 
+    4689           0 :   std::stringstream ss;
+    4690             : 
+    4691             :   {
+    4692           0 :     auto [success, response] = switchTracker(_joystick_tracker_name_);
+    4693             : 
+    4694           0 :     if (!success) {
+    4695             : 
+    4696           0 :       ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+    4697           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4698             : 
+    4699           0 :       res.success = false;
+    4700           0 :       res.message = ss.str();
+    4701             : 
+    4702           0 :       return true;
+    4703             :     }
+    4704             :   }
+    4705             : 
+    4706           0 :   auto [success, response] = switchController(_joystick_controller_name_);
+    4707             : 
+    4708           0 :   if (!success) {
+    4709             : 
+    4710           0 :     ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+    4711           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4712             : 
+    4713           0 :     res.success = false;
+    4714           0 :     res.message = ss.str();
+    4715             : 
+    4716             :     // switch back to hover tracker
+    4717           0 :     switchTracker(_ehover_tracker_name_);
+    4718             : 
+    4719             :     // switch back to safety controller
+    4720           0 :     switchController(_eland_controller_name_);
+    4721             : 
+    4722           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4723             : 
+    4724           0 :     return true;
+    4725             :   }
+    4726             : 
+    4727           0 :   ss << "switched to joystick control";
+    4728             : 
+    4729           0 :   res.success = true;
+    4730           0 :   res.message = ss.str();
+    4731             : 
+    4732           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4733             : 
+    4734           0 :   return true;
+    4735             : }
+    4736             : 
+    4737             : //}
+    4738             : 
+    4739             : /* //{ callbackHover() */
+    4740             : 
+    4741           0 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4742             : 
+    4743           0 :   if (!is_initialized_)
+    4744           0 :     return false;
+    4745             : 
+    4746           0 :   auto [success, message] = hover();
+    4747             : 
+    4748           0 :   res.success = success;
+    4749           0 :   res.message = message;
+    4750             : 
+    4751           0 :   return true;
+    4752             : }
+    4753             : 
+    4754             : //}
+    4755             : 
+    4756             : /* //{ callbackStartTrajectoryTracking() */
+    4757             : 
+    4758           0 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4759             : 
+    4760           0 :   if (!is_initialized_)
+    4761           0 :     return false;
+    4762             : 
+    4763           0 :   auto [success, message] = startTrajectoryTracking();
+    4764             : 
+    4765           0 :   res.success = success;
+    4766           0 :   res.message = message;
+    4767             : 
+    4768           0 :   return true;
+    4769             : }
+    4770             : 
+    4771             : //}
+    4772             : 
+    4773             : /* //{ callbackStopTrajectoryTracking() */
+    4774             : 
+    4775           0 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4776             : 
+    4777           0 :   if (!is_initialized_)
+    4778           0 :     return false;
+    4779             : 
+    4780           0 :   auto [success, message] = stopTrajectoryTracking();
+    4781             : 
+    4782           0 :   res.success = success;
+    4783           0 :   res.message = message;
+    4784             : 
+    4785           0 :   return true;
+    4786             : }
+    4787             : 
+    4788             : //}
+    4789             : 
+    4790             : /* //{ callbackResumeTrajectoryTracking() */
+    4791             : 
+    4792           0 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4793             : 
+    4794           0 :   if (!is_initialized_)
+    4795           0 :     return false;
+    4796             : 
+    4797           0 :   auto [success, message] = resumeTrajectoryTracking();
+    4798             : 
+    4799           0 :   res.success = success;
+    4800           0 :   res.message = message;
+    4801             : 
+    4802           0 :   return true;
+    4803             : }
+    4804             : 
+    4805             : //}
+    4806             : 
+    4807             : /* //{ callbackGotoTrajectoryStart() */
+    4808             : 
+    4809           0 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4810             : 
+    4811           0 :   if (!is_initialized_)
+    4812           0 :     return false;
+    4813             : 
+    4814           0 :   auto [success, message] = gotoTrajectoryStart();
+    4815             : 
+    4816           0 :   res.success = success;
+    4817           0 :   res.message = message;
+    4818             : 
+    4819           0 :   return true;
+    4820             : }
+    4821             : 
+    4822             : //}
+    4823             : 
+    4824             : /* //{ callbackTransformReference() */
+    4825             : 
+    4826           0 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+    4827             : 
+    4828           0 :   if (!is_initialized_)
+    4829           0 :     return false;
+    4830             : 
+    4831             :   // transform the reference to the current frame
+    4832           0 :   mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+    4833             : 
+    4834           0 :   if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+    4835             : 
+    4836           0 :     res.reference = ret.value();
+    4837           0 :     res.message   = "transformation successful";
+    4838           0 :     res.success   = true;
+    4839           0 :     return true;
+    4840             : 
+    4841             :   } else {
+    4842             : 
+    4843           0 :     res.message = "the reference could not be transformed";
+    4844           0 :     res.success = false;
+    4845           0 :     return true;
+    4846             :   }
+    4847             : 
+    4848             :   return true;
+    4849             : }
+    4850             : 
+    4851             : //}
+    4852             : 
+    4853             : /* //{ callbackTransformPose() */
+    4854             : 
+    4855           0 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+    4856             : 
+    4857           0 :   if (!is_initialized_)
+    4858           0 :     return false;
+    4859             : 
+    4860             :   // transform the reference to the current frame
+    4861           0 :   geometry_msgs::PoseStamped transformed_pose = req.pose;
+    4862             : 
+    4863           0 :   if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+    4864             : 
+    4865           0 :     res.pose    = ret.value();
+    4866           0 :     res.message = "transformation successful";
+    4867           0 :     res.success = true;
+    4868           0 :     return true;
+    4869             : 
+    4870             :   } else {
+    4871             : 
+    4872           0 :     res.message = "the pose could not be transformed";
+    4873           0 :     res.success = false;
+    4874           0 :     return true;
+    4875             :   }
+    4876             : 
+    4877             :   return true;
+    4878             : }
+    4879             : 
+    4880             : //}
+    4881             : 
+    4882             : /* //{ callbackTransformVector3() */
+    4883             : 
+    4884           0 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+    4885             : 
+    4886           0 :   if (!is_initialized_)
+    4887           0 :     return false;
+    4888             : 
+    4889             :   // transform the reference to the current frame
+    4890           0 :   geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+    4891             : 
+    4892           0 :   if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+    4893             : 
+    4894           0 :     res.vector  = ret.value();
+    4895           0 :     res.message = "transformation successful";
+    4896           0 :     res.success = true;
+    4897           0 :     return true;
+    4898             : 
+    4899             :   } else {
+    4900             : 
+    4901           0 :     res.message = "the twist could not be transformed";
+    4902           0 :     res.success = false;
+    4903           0 :     return true;
+    4904             :   }
+    4905             : 
+    4906             :   return true;
+    4907             : }
+    4908             : 
+    4909             : //}
+    4910             : 
+    4911             : /* //{ callbackEnableBumper() */
+    4912             : 
+    4913           0 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4914             : 
+    4915           0 :   if (!is_initialized_)
+    4916           0 :     return false;
+    4917             : 
+    4918           0 :   bumper_enabled_ = req.data;
+    4919             : 
+    4920           0 :   std::stringstream ss;
+    4921             : 
+    4922           0 :   ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+    4923             : 
+    4924           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4925             : 
+    4926           0 :   res.success = true;
+    4927           0 :   res.message = ss.str();
+    4928             : 
+    4929           0 :   return true;
+    4930             : }
+    4931             : 
+    4932             : //}
+    4933             : 
+    4934             : /* //{ callbackUseSafetyArea() */
+    4935             : 
+    4936           0 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4937             : 
+    4938           0 :   if (!is_initialized_)
+    4939           0 :     return false;
+    4940             : 
+    4941           0 :   use_safety_area_ = req.data;
+    4942             : 
+    4943           0 :   std::stringstream ss;
+    4944             : 
+    4945           0 :   ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+    4946             : 
+    4947           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4948             : 
+    4949           0 :   res.success = true;
+    4950           0 :   res.message = ss.str();
+    4951             : 
+    4952           0 :   return true;
+    4953             : }
+    4954             : 
+    4955             : //}
+    4956             : 
+    4957             : /* //{ callbackGetMinZ() */
+    4958             : 
+    4959           0 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+    4960             : 
+    4961           0 :   if (!is_initialized_) {
+    4962           0 :     return false;
+    4963             :   }
+    4964             : 
+    4965           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4966             : 
+    4967           0 :   res.success = true;
+    4968           0 :   res.value   = getMinZ(uav_state.header.frame_id);
+    4969             : 
+    4970           0 :   return true;
+    4971             : }
+    4972             : 
+    4973             : //}
+    4974             : 
+    4975             : /* //{ callbackValidateReference() */
+    4976             : 
+    4977           0 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    4978             : 
+    4979           0 :   if (!is_initialized_) {
+    4980           0 :     res.message = "not initialized";
+    4981           0 :     res.success = false;
+    4982           0 :     return true;
+    4983             :   }
+    4984             : 
+    4985           0 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    4986           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    4987           0 :     res.message = "NaNs/infs in input!";
+    4988           0 :     res.success = false;
+    4989           0 :     return true;
+    4990             :   }
+    4991             : 
+    4992             :   // copy member variables
+    4993           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4994           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    4995             : 
+    4996             :   // transform the reference to the current frame
+    4997           0 :   mrs_msgs::ReferenceStamped original_reference;
+    4998           0 :   original_reference.header    = req.reference.header;
+    4999           0 :   original_reference.reference = req.reference.reference;
+    5000             : 
+    5001           0 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5002             : 
+    5003           0 :   if (!ret) {
+    5004             : 
+    5005           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5006           0 :     res.message = "the reference could not be transformed";
+    5007           0 :     res.success = false;
+    5008           0 :     return true;
+    5009             :   }
+    5010             : 
+    5011           0 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5012             : 
+    5013           0 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5014           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5015           0 :     res.message = "the point is outside of the safety area";
+    5016           0 :     res.success = false;
+    5017           0 :     return true;
+    5018             :   }
+    5019             : 
+    5020           0 :   if (last_tracker_cmd) {
+    5021             : 
+    5022           0 :     mrs_msgs::ReferenceStamped from_point;
+    5023           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5024           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5025           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5026           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5027             : 
+    5028           0 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5029           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5030           0 :       res.message = "the path is going outside the safety area";
+    5031           0 :       res.success = false;
+    5032           0 :       return true;
+    5033             :     }
+    5034             :   }
+    5035             : 
+    5036           0 :   res.message = "the reference is ok";
+    5037           0 :   res.success = true;
+    5038           0 :   return true;
+    5039             : }
+    5040             : 
+    5041             : //}
+    5042             : 
+    5043             : /* //{ callbackValidateReference2d() */
+    5044             : 
+    5045         187 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5046             : 
+    5047         187 :   if (!is_initialized_) {
+    5048           0 :     res.message = "not initialized";
+    5049           0 :     res.success = false;
+    5050           0 :     return true;
+    5051             :   }
+    5052             : 
+    5053         187 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5054           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5055           0 :     res.message = "NaNs/infs in input!";
+    5056           0 :     res.success = false;
+    5057           0 :     return true;
+    5058             :   }
+    5059             : 
+    5060             :   // copy member variables
+    5061         374 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5062         374 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5063             : 
+    5064             :   // transform the reference to the current frame
+    5065         374 :   mrs_msgs::ReferenceStamped original_reference;
+    5066         187 :   original_reference.header    = req.reference.header;
+    5067         187 :   original_reference.reference = req.reference.reference;
+    5068             : 
+    5069         374 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5070             : 
+    5071         187 :   if (!ret) {
+    5072             : 
+    5073           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5074           0 :     res.message = "the reference could not be transformed";
+    5075           0 :     res.success = false;
+    5076           0 :     return true;
+    5077             :   }
+    5078             : 
+    5079         374 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5080             : 
+    5081         187 :   if (!isPointInSafetyArea2d(transformed_reference)) {
+    5082           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5083           0 :     res.message = "the point is outside of the safety area";
+    5084           0 :     res.success = false;
+    5085           0 :     return true;
+    5086             :   }
+    5087             : 
+    5088         187 :   if (last_tracker_cmd) {
+    5089             : 
+    5090           0 :     mrs_msgs::ReferenceStamped from_point;
+    5091           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5092           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5093           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5094           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5095             : 
+    5096           0 :     if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+    5097           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5098           0 :       res.message = "the path is going outside the safety area";
+    5099           0 :       res.success = false;
+    5100           0 :       return true;
+    5101             :     }
+    5102             :   }
+    5103             : 
+    5104         187 :   res.message = "the reference is ok";
+    5105         187 :   res.success = true;
+    5106         187 :   return true;
+    5107             : }
+    5108             : 
+    5109             : //}
+    5110             : 
+    5111             : /* //{ callbackValidateReferenceList() */
+    5112             : 
+    5113           0 : bool ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res) {
+    5114             : 
+    5115           0 :   if (!is_initialized_) {
+    5116           0 :     res.message = "not initialized";
+    5117           0 :     return false;
+    5118             :   }
+    5119             : 
+    5120             :   // copy member variables
+    5121           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5122           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5123             : 
+    5124             :   // get the transformer
+    5125           0 :   auto ret = transformer_->getTransform(uav_state.header.frame_id, req.list.header.frame_id, req.list.header.stamp);
+    5126             : 
+    5127           0 :   if (!ret) {
+    5128             : 
+    5129           0 :     ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+    5130           0 :     res.message = "could not find transform";
+    5131           0 :     return false;
+    5132             :   }
+    5133             : 
+    5134           0 :   geometry_msgs::TransformStamped tf = ret.value();
+    5135             : 
+    5136           0 :   for (int i = 0; i < int(req.list.list.size()); i++) {
+    5137             : 
+    5138           0 :     res.success.push_back(true);
+    5139             : 
+    5140           0 :     mrs_msgs::ReferenceStamped original_reference;
+    5141           0 :     original_reference.header    = req.list.header;
+    5142           0 :     original_reference.reference = req.list.list[i];
+    5143             : 
+    5144           0 :     res.success[i] = validateReference(original_reference.reference, "ControlManager", "reference_list");
+    5145             : 
+    5146           0 :     auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5147             : 
+    5148           0 :     if (!ret) {
+    5149             : 
+    5150           0 :       ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+    5151           0 :       res.success[i] = false;
+    5152             :     }
+    5153             : 
+    5154           0 :     mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5155             : 
+    5156           0 :     if (!isPointInSafetyArea3d(transformed_reference)) {
+    5157           0 :       res.success[i] = false;
+    5158             :     }
+    5159             : 
+    5160           0 :     if (last_tracker_cmd) {
+    5161             : 
+    5162           0 :       mrs_msgs::ReferenceStamped from_point;
+    5163           0 :       from_point.header.frame_id      = uav_state.header.frame_id;
+    5164           0 :       from_point.reference.position.x = last_tracker_cmd->position.x;
+    5165           0 :       from_point.reference.position.y = last_tracker_cmd->position.y;
+    5166           0 :       from_point.reference.position.z = last_tracker_cmd->position.z;
+    5167             : 
+    5168           0 :       if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5169           0 :         res.success[i] = false;
+    5170             :       }
+    5171             :     }
+    5172             :   }
+    5173             : 
+    5174           0 :   res.message = "references were checked";
+    5175           0 :   return true;
+    5176             : }
+    5177             : 
+    5178             : //}
+    5179             : 
+    5180             : // | -------------- setpoint topics and services -------------- |
+    5181             : 
+    5182             : /* //{ callbackReferenceService() */
+    5183             : 
+    5184           0 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    5185             : 
+    5186           0 :   if (!is_initialized_) {
+    5187           0 :     res.message = "not initialized";
+    5188           0 :     res.success = false;
+    5189           0 :     return true;
+    5190             :   }
+    5191             : 
+    5192           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceService");
+    5193           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5194             : 
+    5195           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5196           0 :   des_reference.header    = req.header;
+    5197           0 :   des_reference.reference = req.reference;
+    5198             : 
+    5199           0 :   auto [success, message] = setReference(des_reference);
+    5200             : 
+    5201           0 :   res.success = success;
+    5202           0 :   res.message = message;
+    5203             : 
+    5204           0 :   return true;
+    5205             : }
+    5206             : 
+    5207             : //}
+    5208             : 
+    5209             : /* //{ callbackReferenceTopic() */
+    5210             : 
+    5211           0 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+    5212             : 
+    5213           0 :   if (!is_initialized_)
+    5214           0 :     return;
+    5215             : 
+    5216           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+    5217           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5218             : 
+    5219           0 :   setReference(*msg);
+    5220             : }
+    5221             : 
+    5222             : //}
+    5223             : 
+    5224             : /* //{ callbackVelocityReferenceService() */
+    5225             : 
+    5226           0 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request&  req,
+    5227             :                                                       mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+    5228             : 
+    5229           0 :   if (!is_initialized_) {
+    5230           0 :     res.message = "not initialized";
+    5231           0 :     res.success = false;
+    5232           0 :     return true;
+    5233             :   }
+    5234             : 
+    5235           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+    5236           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5237             : 
+    5238           0 :   mrs_msgs::VelocityReferenceStamped des_reference;
+    5239           0 :   des_reference = req.reference;
+    5240             : 
+    5241           0 :   auto [success, message] = setVelocityReference(des_reference);
+    5242             : 
+    5243           0 :   res.success = success;
+    5244           0 :   res.message = message;
+    5245             : 
+    5246           0 :   return true;
+    5247             : }
+    5248             : 
+    5249             : //}
+    5250             : 
+    5251             : /* //{ callbackVelocityReferenceTopic() */
+    5252             : 
+    5253           0 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+    5254             : 
+    5255           0 :   if (!is_initialized_)
+    5256           0 :     return;
+    5257             : 
+    5258           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+    5259           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5260             : 
+    5261           0 :   setVelocityReference(*msg);
+    5262             : }
+    5263             : 
+    5264             : //}
+    5265             : 
+    5266             : /* //{ callbackTrajectoryReferenceService() */
+    5267             : 
+    5268           0 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+    5269             : 
+    5270           0 :   if (!is_initialized_) {
+    5271           0 :     res.message = "not initialized";
+    5272           0 :     res.success = false;
+    5273           0 :     return true;
+    5274             :   }
+    5275             : 
+    5276           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+    5277           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5278             : 
+    5279           0 :   auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+    5280             : 
+    5281           0 :   res.success          = success;
+    5282           0 :   res.message          = message;
+    5283           0 :   res.modified         = modified;
+    5284           0 :   res.tracker_names    = tracker_names;
+    5285           0 :   res.tracker_messages = tracker_messages;
+    5286             : 
+    5287           0 :   for (size_t i = 0; i < tracker_successes.size(); i++) {
+    5288           0 :     res.tracker_successes.push_back(tracker_successes[i]);
+    5289             :   }
+    5290             : 
+    5291           0 :   return true;
+    5292             : }
+    5293             : 
+    5294             : //}
+    5295             : 
+    5296             : /* //{ callbackTrajectoryReferenceTopic() */
+    5297             : 
+    5298           0 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+    5299             : 
+    5300           0 :   if (!is_initialized_)
+    5301           0 :     return;
+    5302             : 
+    5303           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+    5304           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5305             : 
+    5306           0 :   setTrajectoryReference(*msg);
+    5307             : }
+    5308             : 
+    5309             : //}
+    5310             : 
+    5311             : // | ------------- human-callable "goto" services ------------- |
+    5312             : 
+    5313             : /* //{ callbackGoto() */
+    5314             : 
+    5315           1 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5316             : 
+    5317           1 :   if (!is_initialized_) {
+    5318           0 :     res.message = "not initialized";
+    5319           0 :     res.success = false;
+    5320           0 :     return true;
+    5321             :   }
+    5322             : 
+    5323           3 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGoto");
+    5324           3 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+    5325             : 
+    5326           2 :   mrs_msgs::ReferenceStamped des_reference;
+    5327           1 :   des_reference.header.frame_id      = "";
+    5328           1 :   des_reference.header.stamp         = ros::Time(0);
+    5329           1 :   des_reference.reference.position.x = req.goal[REF_X];
+    5330           1 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5331           1 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5332           1 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5333             : 
+    5334           2 :   auto [success, message] = setReference(des_reference);
+    5335             : 
+    5336           1 :   res.success = success;
+    5337           1 :   res.message = message;
+    5338             : 
+    5339           1 :   return true;
+    5340             : }
+    5341             : 
+    5342             : //}
+    5343             : 
+    5344             : /* //{ callbackGotoFcu() */
+    5345             : 
+    5346           0 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5347             : 
+    5348           0 :   if (!is_initialized_) {
+    5349           0 :     res.message = "not initialized";
+    5350           0 :     res.success = false;
+    5351           0 :     return true;
+    5352             :   }
+    5353             : 
+    5354           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+    5355           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+    5356             : 
+    5357           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5358           0 :   des_reference.header.frame_id      = "fcu_untilted";
+    5359           0 :   des_reference.header.stamp         = ros::Time(0);
+    5360           0 :   des_reference.reference.position.x = req.goal[REF_X];
+    5361           0 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5362           0 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5363           0 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5364             : 
+    5365           0 :   auto [success, message] = setReference(des_reference);
+    5366             : 
+    5367           0 :   res.success = success;
+    5368           0 :   res.message = message;
+    5369             : 
+    5370           0 :   return true;
+    5371             : }
+    5372             : 
+    5373             : //}
+    5374             : 
+    5375             : /* //{ callbackGotoRelative() */
+    5376             : 
+    5377           2 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5378             : 
+    5379           2 :   if (!is_initialized_) {
+    5380           0 :     res.message = "not initialized";
+    5381           0 :     res.success = false;
+    5382           0 :     return true;
+    5383             :   }
+    5384             : 
+    5385           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+    5386           6 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+    5387             : 
+    5388           4 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5389             : 
+    5390           2 :   if (!last_tracker_cmd) {
+    5391           0 :     res.message = "not flying";
+    5392           0 :     res.success = false;
+    5393           0 :     return true;
+    5394             :   }
+    5395             : 
+    5396           4 :   mrs_msgs::ReferenceStamped des_reference;
+    5397           2 :   des_reference.header.frame_id      = "";
+    5398           2 :   des_reference.header.stamp         = ros::Time(0);
+    5399           2 :   des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal[REF_X];
+    5400           2 :   des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal[REF_Y];
+    5401           2 :   des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal[REF_Z];
+    5402           2 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal[REF_HEADING];
+    5403             : 
+    5404           4 :   auto [success, message] = setReference(des_reference);
+    5405             : 
+    5406           2 :   res.success = success;
+    5407           2 :   res.message = message;
+    5408             : 
+    5409           2 :   return true;
+    5410             : }
+    5411             : 
+    5412             : //}
+    5413             : 
+    5414             : /* //{ callbackGotoAltitude() */
+    5415             : 
+    5416           0 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5417             : 
+    5418           0 :   if (!is_initialized_) {
+    5419           0 :     res.message = "not initialized";
+    5420           0 :     res.success = false;
+    5421           0 :     return true;
+    5422             :   }
+    5423             : 
+    5424           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+    5425           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+    5426             : 
+    5427           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5428             : 
+    5429           0 :   if (!last_tracker_cmd) {
+    5430           0 :     res.message = "not flying";
+    5431           0 :     res.success = false;
+    5432           0 :     return true;
+    5433             :   }
+    5434             : 
+    5435           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5436           0 :   des_reference.header.frame_id      = "";
+    5437           0 :   des_reference.header.stamp         = ros::Time(0);
+    5438           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5439           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5440           0 :   des_reference.reference.position.z = req.goal;
+    5441           0 :   des_reference.reference.heading    = last_tracker_cmd->heading;
+    5442             : 
+    5443           0 :   auto [success, message] = setReference(des_reference);
+    5444             : 
+    5445           0 :   res.success = success;
+    5446           0 :   res.message = message;
+    5447             : 
+    5448           0 :   return true;
+    5449             : }
+    5450             : 
+    5451             : //}
+    5452             : 
+    5453             : /* //{ callbackSetHeading() */
+    5454             : 
+    5455           0 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5456             : 
+    5457           0 :   if (!is_initialized_) {
+    5458           0 :     res.message = "not initialized";
+    5459           0 :     res.success = false;
+    5460           0 :     return true;
+    5461             :   }
+    5462             : 
+    5463           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeading");
+    5464           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+    5465             : 
+    5466           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5467             : 
+    5468           0 :   if (!last_tracker_cmd) {
+    5469           0 :     res.message = "not flying";
+    5470           0 :     res.success = false;
+    5471           0 :     return true;
+    5472             :   }
+    5473             : 
+    5474           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5475           0 :   des_reference.header.frame_id      = "";
+    5476           0 :   des_reference.header.stamp         = ros::Time(0);
+    5477           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5478           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5479           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5480           0 :   des_reference.reference.heading    = req.goal;
+    5481             : 
+    5482           0 :   auto [success, message] = setReference(des_reference);
+    5483             : 
+    5484           0 :   res.success = success;
+    5485           0 :   res.message = message;
+    5486             : 
+    5487           0 :   return true;
+    5488             : }
+    5489             : 
+    5490             : //}
+    5491             : 
+    5492             : /* //{ callbackSetHeadingRelative() */
+    5493             : 
+    5494           0 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5495             : 
+    5496           0 :   if (!is_initialized_) {
+    5497           0 :     res.message = "not initialized";
+    5498           0 :     res.success = false;
+    5499           0 :     return true;
+    5500             :   }
+    5501             : 
+    5502           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+    5503           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+    5504             : 
+    5505           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5506             : 
+    5507           0 :   if (!last_tracker_cmd) {
+    5508           0 :     res.message = "not flying";
+    5509           0 :     res.success = false;
+    5510           0 :     return true;
+    5511             :   }
+    5512             : 
+    5513           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5514           0 :   des_reference.header.frame_id      = "";
+    5515           0 :   des_reference.header.stamp         = ros::Time(0);
+    5516           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5517           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5518           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5519           0 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal;
+    5520             : 
+    5521           0 :   auto [success, message] = setReference(des_reference);
+    5522             : 
+    5523           0 :   res.success = success;
+    5524           0 :   res.message = message;
+    5525             : 
+    5526           0 :   return true;
+    5527             : }
+    5528             : 
+    5529             : //}
+    5530             : 
+    5531             : // --------------------------------------------------------------
+    5532             : // |                          routines                          |
+    5533             : // --------------------------------------------------------------
+    5534             : 
+    5535             : /* setReference() //{ */
+    5536             : 
+    5537           3 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+    5538             : 
+    5539           6 :   std::stringstream ss;
+    5540             : 
+    5541           3 :   if (!callbacks_enabled_) {
+    5542           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5543           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5544           0 :     return std::tuple(false, ss.str());
+    5545             :   }
+    5546             : 
+    5547           3 :   if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+    5548           0 :     ss << "incoming reference is not finite!!!";
+    5549           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5550           0 :     return std::tuple(false, ss.str());
+    5551             :   }
+    5552             : 
+    5553             :   // copy member variables
+    5554           6 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5555           6 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5556             : 
+    5557             :   // transform the reference to the current frame
+    5558           6 :   auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+    5559             : 
+    5560           3 :   if (!ret) {
+    5561             : 
+    5562           0 :     ss << "the reference could not be transformed";
+    5563           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5564           0 :     return std::tuple(false, ss.str());
+    5565             :   }
+    5566             : 
+    5567           6 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5568             : 
+    5569             :   // safety area check
+    5570           3 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5571           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5572           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5573           0 :     return std::tuple(false, ss.str());
+    5574             :   }
+    5575             : 
+    5576           3 :   if (last_tracker_cmd) {
+    5577             : 
+    5578           3 :     mrs_msgs::ReferenceStamped from_point;
+    5579           3 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5580           3 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5581           3 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5582           3 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5583             : 
+    5584           3 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5585           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5586           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5587           0 :       return std::tuple(false, ss.str());
+    5588             :     }
+    5589             :   }
+    5590             : 
+    5591           3 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    5592             : 
+    5593             :   // prepare the message for current tracker
+    5594           3 :   mrs_msgs::ReferenceSrvRequest reference_request;
+    5595           3 :   reference_request.reference = transformed_reference.reference;
+    5596             : 
+    5597             :   {
+    5598           6 :     std::scoped_lock lock(mutex_tracker_list_);
+    5599             : 
+    5600           9 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    5601           9 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+    5602             : 
+    5603           3 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    5604             : 
+    5605           6 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5606             : 
+    5607             :     } else {
+    5608             : 
+    5609           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    5610           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+    5611           0 :       return std::tuple(false, ss.str());
+    5612             :     }
+    5613             :   }
+    5614             : }
+    5615             : 
+    5616             : //}
+    5617             : 
+    5618             : /* setVelocityReference() //{ */
+    5619             : 
+    5620           0 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+    5621             : 
+    5622           0 :   std::stringstream ss;
+    5623             : 
+    5624           0 :   if (!callbacks_enabled_) {
+    5625           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5626           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5627           0 :     return std::tuple(false, ss.str());
+    5628             :   }
+    5629             : 
+    5630           0 :   if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+    5631           0 :     ss << "velocity command is not valid!";
+    5632           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5633           0 :     return std::tuple(false, ss.str());
+    5634             :   }
+    5635             : 
+    5636             :   {
+    5637           0 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    5638             : 
+    5639           0 :     if (!last_tracker_cmd_) {
+    5640           0 :       ss << "could not set velocity command, not flying!";
+    5641           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5642           0 :       return std::tuple(false, ss.str());
+    5643             :     }
+    5644             :   }
+    5645             : 
+    5646             :   // copy member variables
+    5647           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5648           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5649             : 
+    5650             :   // | -- transform the velocity reference to the current frame - |
+    5651             : 
+    5652           0 :   mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+    5653             : 
+    5654           0 :   auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+    5655             : 
+    5656           0 :   geometry_msgs::TransformStamped tf;
+    5657             : 
+    5658           0 :   if (!ret) {
+    5659           0 :     ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+    5660           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5661           0 :     return std::tuple(false, ss.str());
+    5662             :   } else {
+    5663           0 :     tf = ret.value();
+    5664             :   }
+    5665             : 
+    5666             :   // transform the velocity
+    5667             :   {
+    5668           0 :     geometry_msgs::Vector3Stamped velocity;
+    5669           0 :     velocity.header   = reference_in.header;
+    5670           0 :     velocity.vector.x = reference_in.reference.velocity.x;
+    5671           0 :     velocity.vector.y = reference_in.reference.velocity.y;
+    5672           0 :     velocity.vector.z = reference_in.reference.velocity.z;
+    5673             : 
+    5674           0 :     auto ret = transformer_->transform(velocity, tf);
+    5675             : 
+    5676           0 :     if (!ret) {
+    5677             : 
+    5678           0 :       ss << "the velocity reference could not be transformed";
+    5679           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5680           0 :       return std::tuple(false, ss.str());
+    5681             : 
+    5682             :     } else {
+    5683           0 :       transformed_reference.reference.velocity.x = ret->vector.x;
+    5684           0 :       transformed_reference.reference.velocity.y = ret->vector.y;
+    5685           0 :       transformed_reference.reference.velocity.z = ret->vector.z;
+    5686             :     }
+    5687             :   }
+    5688             : 
+    5689             :   // transform the z and the heading
+    5690             :   {
+    5691           0 :     geometry_msgs::PoseStamped pose;
+    5692           0 :     pose.header           = reference_in.header;
+    5693           0 :     pose.pose.position.x  = 0;
+    5694           0 :     pose.pose.position.y  = 0;
+    5695           0 :     pose.pose.position.z  = reference_in.reference.altitude;
+    5696           0 :     pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+    5697             : 
+    5698           0 :     auto ret = transformer_->transform(pose, tf);
+    5699             : 
+    5700           0 :     if (!ret) {
+    5701             : 
+    5702           0 :       ss << "the velocity reference could not be transformed";
+    5703           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5704           0 :       return std::tuple(false, ss.str());
+    5705             : 
+    5706             :     } else {
+    5707           0 :       transformed_reference.reference.altitude = ret->pose.position.z;
+    5708           0 :       transformed_reference.reference.heading  = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+    5709             :     }
+    5710             :   }
+    5711             : 
+    5712             :   // the heading rate doees not need to be transformed
+    5713           0 :   transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+    5714             : 
+    5715           0 :   transformed_reference.header.stamp    = tf.header.stamp;
+    5716           0 :   transformed_reference.header.frame_id = transformer_->frame_to(tf);
+    5717             : 
+    5718           0 :   mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+    5719             : 
+    5720           0 :   ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+    5721             :             eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+    5722             : 
+    5723             :   // safety area check
+    5724           0 :   if (!isPointInSafetyArea3d(eqivalent_reference)) {
+    5725           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5726           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5727           0 :     return std::tuple(false, ss.str());
+    5728             :   }
+    5729             : 
+    5730           0 :   if (last_tracker_cmd) {
+    5731             : 
+    5732           0 :     mrs_msgs::ReferenceStamped from_point;
+    5733           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5734           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5735           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5736           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5737             : 
+    5738           0 :     if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+    5739           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5740           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5741           0 :       return std::tuple(false, ss.str());
+    5742             :     }
+    5743             :   }
+    5744             : 
+    5745           0 :   mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+    5746             : 
+    5747             :   // prepare the message for current tracker
+    5748           0 :   mrs_msgs::VelocityReferenceSrvRequest reference_request;
+    5749           0 :   reference_request.reference = transformed_reference.reference;
+    5750             : 
+    5751             :   {
+    5752           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    5753             : 
+    5754           0 :     tracker_response = tracker_list_[active_tracker_idx_]->setVelocityReference(
+    5755           0 :         mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+    5756             : 
+    5757           0 :     if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+    5758             : 
+    5759           0 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5760             : 
+    5761             :     } else {
+    5762             : 
+    5763           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setVelocityReference()' function!";
+    5764           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+    5765           0 :       return std::tuple(false, ss.str());
+    5766             :     }
+    5767             :   }
+    5768             : }
+    5769             : 
+    5770             : //}
+    5771             : 
+    5772             : /* setTrajectoryReference() //{ */
+    5773             : 
+    5774           0 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+    5775             :     const mrs_msgs::TrajectoryReference trajectory_in) {
+    5776             : 
+    5777           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5778           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5779             : 
+    5780           0 :   std::stringstream ss;
+    5781             : 
+    5782           0 :   if (!callbacks_enabled_) {
+    5783           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5784           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5785           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5786             :   }
+    5787             : 
+    5788             :   /* validate the size and check for NaNs //{ */
+    5789             : 
+    5790             :   // check for the size 0, which is invalid
+    5791           0 :   if (trajectory_in.points.size() == 0) {
+    5792             : 
+    5793           0 :     ss << "can not load trajectory with size 0";
+    5794           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5795           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5796             :   }
+    5797             : 
+    5798           0 :   for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+    5799             : 
+    5800             :     // check the point for NaN/inf
+    5801           0 :     bool valid = validateReference(trajectory_in.points[i], "ControlManager", "trajectory_in.points[]");
+    5802             : 
+    5803           0 :     if (!valid) {
+    5804             : 
+    5805           0 :       ss << "trajectory contains NaNs/infs.";
+    5806           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5807           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5808             :     }
+    5809             :   }
+    5810             : 
+    5811             :   //}
+    5812             : 
+    5813             :   /* publish the debugging topics of the original trajectory //{ */
+    5814             : 
+    5815             :   {
+    5816             : 
+    5817           0 :     geometry_msgs::PoseArray debug_trajectory_out;
+    5818           0 :     debug_trajectory_out.header = trajectory_in.header;
+    5819             : 
+    5820           0 :     debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+    5821             : 
+    5822           0 :     if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+    5823           0 :       debug_trajectory_out.header.stamp = ros::Time::now();
+    5824             :     }
+    5825             : 
+    5826           0 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5827             : 
+    5828           0 :       geometry_msgs::Pose new_pose;
+    5829             : 
+    5830           0 :       new_pose.position.x = trajectory_in.points[i].position.x;
+    5831           0 :       new_pose.position.y = trajectory_in.points[i].position.y;
+    5832           0 :       new_pose.position.z = trajectory_in.points[i].position.z;
+    5833             : 
+    5834           0 :       new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points[i].heading);
+    5835             : 
+    5836           0 :       debug_trajectory_out.poses.push_back(new_pose);
+    5837             :     }
+    5838             : 
+    5839           0 :     pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+    5840             : 
+    5841           0 :     visualization_msgs::MarkerArray msg_out;
+    5842             : 
+    5843           0 :     visualization_msgs::Marker marker;
+    5844             : 
+    5845           0 :     marker.header = trajectory_in.header;
+    5846             : 
+    5847           0 :     marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+    5848             : 
+    5849           0 :     if (marker.header.frame_id == "") {
+    5850           0 :       marker.header.frame_id = uav_state.header.frame_id;
+    5851             :     }
+    5852             : 
+    5853           0 :     if (marker.header.stamp == ros::Time(0)) {
+    5854           0 :       marker.header.stamp = ros::Time::now();
+    5855             :     }
+    5856             : 
+    5857           0 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    5858           0 :     marker.color.a          = 1;
+    5859           0 :     marker.scale.x          = 0.05;
+    5860           0 :     marker.color.r          = 0;
+    5861           0 :     marker.color.g          = 1;
+    5862           0 :     marker.color.b          = 0;
+    5863           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    5864             : 
+    5865           0 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5866             : 
+    5867           0 :       geometry_msgs::Point point1;
+    5868             : 
+    5869           0 :       point1.x = trajectory_in.points[i].position.x;
+    5870           0 :       point1.y = trajectory_in.points[i].position.y;
+    5871           0 :       point1.z = trajectory_in.points[i].position.z;
+    5872             : 
+    5873           0 :       marker.points.push_back(point1);
+    5874             : 
+    5875           0 :       geometry_msgs::Point point2;
+    5876             : 
+    5877           0 :       point2.x = trajectory_in.points[i + 1].position.x;
+    5878           0 :       point2.y = trajectory_in.points[i + 1].position.y;
+    5879           0 :       point2.z = trajectory_in.points[i + 1].position.z;
+    5880             : 
+    5881           0 :       marker.points.push_back(point2);
+    5882             :     }
+    5883             : 
+    5884           0 :     msg_out.markers.push_back(marker);
+    5885             : 
+    5886           0 :     pub_debug_original_trajectory_markers_.publish(msg_out);
+    5887             :   }
+    5888             : 
+    5889             :   //}
+    5890             : 
+    5891           0 :   mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+    5892             : 
+    5893           0 :   int trajectory_size = int(processed_trajectory.points.size());
+    5894             : 
+    5895           0 :   bool trajectory_modified = false;
+    5896             : 
+    5897             :   /* safety area check //{ */
+    5898             : 
+    5899           0 :   if (use_safety_area_) {
+    5900             : 
+    5901           0 :     int last_valid_idx    = 0;
+    5902           0 :     int first_invalid_idx = -1;
+    5903             : 
+    5904           0 :     double min_z = getMinZ(processed_trajectory.header.frame_id);
+    5905           0 :     double max_z = getMaxZ(processed_trajectory.header.frame_id);
+    5906             : 
+    5907           0 :     for (int i = 0; i < trajectory_size; i++) {
+    5908             : 
+    5909           0 :       if (_snap_trajectory_to_safety_area_) {
+    5910             : 
+    5911             :         // saturate the trajectory to min and max Z
+    5912           0 :         if (processed_trajectory.points[i].position.z < min_z) {
+    5913             : 
+    5914           0 :           processed_trajectory.points[i].position.z = min_z;
+    5915           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+    5916           0 :           trajectory_modified = true;
+    5917             :         }
+    5918             : 
+    5919           0 :         if (processed_trajectory.points[i].position.z > max_z) {
+    5920             : 
+    5921           0 :           processed_trajectory.points[i].position.z = max_z;
+    5922           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+    5923           0 :           trajectory_modified = true;
+    5924             :         }
+    5925             :       }
+    5926             : 
+    5927             :       // check the point against the safety area
+    5928           0 :       mrs_msgs::ReferenceStamped des_reference;
+    5929           0 :       des_reference.header    = processed_trajectory.header;
+    5930           0 :       des_reference.reference = processed_trajectory.points[i];
+    5931             : 
+    5932           0 :       if (!isPointInSafetyArea3d(des_reference)) {
+    5933             : 
+    5934           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+    5935           0 :         trajectory_modified = true;
+    5936             : 
+    5937             :         // the first invalid point
+    5938           0 :         if (first_invalid_idx == -1) {
+    5939             : 
+    5940           0 :           first_invalid_idx = i;
+    5941             : 
+    5942           0 :           last_valid_idx = i - 1;
+    5943             :         }
+    5944             : 
+    5945             :         // the point is ok
+    5946             :       } else {
+    5947             : 
+    5948             :         // we found a point, which is ok, after finding a point which was not ok
+    5949           0 :         if (first_invalid_idx != -1) {
+    5950             : 
+    5951             :           // special case, we had no valid point so far
+    5952           0 :           if (last_valid_idx == -1) {
+    5953             : 
+    5954           0 :             ss << "the trajectory starts outside of the safety area!";
+    5955           0 :             ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5956           0 :             return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5957             : 
+    5958             :             // we have a valid point in the past
+    5959             :           } else {
+    5960             : 
+    5961           0 :             if (!_snap_trajectory_to_safety_area_) {
+    5962           0 :               break;
+    5963             :             }
+    5964             : 
+    5965           0 :             bool interpolation_success = true;
+    5966             : 
+    5967             :             // iterpolate between the last valid point and this new valid point
+    5968           0 :             double angle = atan2((processed_trajectory.points[i].position.y - processed_trajectory.points[last_valid_idx].position.y),
+    5969           0 :                                  (processed_trajectory.points[i].position.x - processed_trajectory.points[last_valid_idx].position.x));
+    5970             : 
+    5971             :             double dist_two_points =
+    5972           0 :                 mrs_lib::geometry::dist(vec2_t(processed_trajectory.points[i].position.x, processed_trajectory.points[i].position.y),
+    5973           0 :                                         vec2_t(processed_trajectory.points[last_valid_idx].position.x, processed_trajectory.points[last_valid_idx].position.y));
+    5974           0 :             double step = dist_two_points / (i - last_valid_idx);
+    5975             : 
+    5976           0 :             for (int j = last_valid_idx; j < i; j++) {
+    5977             : 
+    5978           0 :               mrs_msgs::ReferenceStamped temp_point;
+    5979           0 :               temp_point.header.frame_id      = processed_trajectory.header.frame_id;
+    5980           0 :               temp_point.reference.position.x = processed_trajectory.points[last_valid_idx].position.x + (j - last_valid_idx) * cos(angle) * step;
+    5981           0 :               temp_point.reference.position.y = processed_trajectory.points[last_valid_idx].position.y + (j - last_valid_idx) * sin(angle) * step;
+    5982             : 
+    5983           0 :               if (!isPointInSafetyArea2d(temp_point)) {
+    5984             : 
+    5985           0 :                 interpolation_success = false;
+    5986           0 :                 break;
+    5987             : 
+    5988             :               } else {
+    5989             : 
+    5990           0 :                 processed_trajectory.points[j].position.x = temp_point.reference.position.x;
+    5991           0 :                 processed_trajectory.points[j].position.y = temp_point.reference.position.y;
+    5992             :               }
+    5993             :             }
+    5994             : 
+    5995           0 :             if (!interpolation_success) {
+    5996           0 :               break;
+    5997             :             }
+    5998             :           }
+    5999             : 
+    6000           0 :           first_invalid_idx = -1;
+    6001             :         }
+    6002             :       }
+    6003             :     }
+    6004             : 
+    6005             :     // special case, the trajectory does not end with a valid point
+    6006           0 :     if (first_invalid_idx != -1) {
+    6007             : 
+    6008             :       // super special case, the whole trajectory is invalid
+    6009           0 :       if (first_invalid_idx == 0) {
+    6010             : 
+    6011           0 :         ss << "the whole trajectory is outside of the safety area!";
+    6012           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6013           0 :         return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6014             : 
+    6015             :         // there is a good portion of the trajectory in the beginning
+    6016             :       } else {
+    6017             : 
+    6018           0 :         trajectory_size = last_valid_idx + 1;
+    6019           0 :         processed_trajectory.points.resize(trajectory_size);
+    6020           0 :         trajectory_modified = true;
+    6021             :       }
+    6022             :     }
+    6023             :   }
+    6024             : 
+    6025           0 :   if (trajectory_size == 0) {
+    6026             : 
+    6027           0 :     ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+    6028           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6029           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6030             :   }
+    6031             : 
+    6032             :   //}
+    6033             : 
+    6034             :   /* transform the trajectory to the current control frame //{ */
+    6035             : 
+    6036           0 :   std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+    6037             : 
+    6038           0 :   if (processed_trajectory.header.stamp > ros::Time::now()) {
+    6039           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+    6040             :   } else {
+    6041           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+    6042             :   }
+    6043             : 
+    6044           0 :   if (!tf_traj_state) {
+    6045           0 :     ss << "could not create TF transformer for the trajectory";
+    6046           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6047           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6048             :   }
+    6049             : 
+    6050           0 :   processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    6051             : 
+    6052           0 :   for (int i = 0; i < trajectory_size; i++) {
+    6053             : 
+    6054           0 :     mrs_msgs::ReferenceStamped trajectory_point;
+    6055           0 :     trajectory_point.header    = processed_trajectory.header;
+    6056           0 :     trajectory_point.reference = processed_trajectory.points[i];
+    6057             : 
+    6058           0 :     auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    6059             : 
+    6060           0 :     if (!ret) {
+    6061             : 
+    6062           0 :       ss << "trajectory cannnot be transformed";
+    6063           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6064           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6065             : 
+    6066             :     } else {
+    6067             : 
+    6068             :       // transform the points in the trajectory to the current frame
+    6069           0 :       processed_trajectory.points[i] = ret.value().reference;
+    6070             :     }
+    6071             :   }
+    6072             : 
+    6073             :   //}
+    6074             : 
+    6075           0 :   mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+    6076           0 :   mrs_msgs::TrajectoryReferenceSrvRequest            request;
+    6077             : 
+    6078             :   // check for empty trajectory
+    6079           0 :   if (processed_trajectory.points.size() == 0) {
+    6080           0 :     ss << "reference trajectory was processing and it is now empty, this should not happen!";
+    6081           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6082           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6083             :   }
+    6084             : 
+    6085             :   // prepare the message for current tracker
+    6086           0 :   request.trajectory = processed_trajectory;
+    6087             : 
+    6088             :   bool                     success;
+    6089           0 :   std::string              message;
+    6090             :   bool                     modified;
+    6091           0 :   std::vector<std::string> tracker_names;
+    6092           0 :   std::vector<bool>        tracker_successes;
+    6093           0 :   std::vector<std::string> tracker_messages;
+    6094             : 
+    6095             :   {
+    6096           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    6097             : 
+    6098             :     // set the trajectory to the currently active tracker
+    6099           0 :     response = tracker_list_[active_tracker_idx_]->setTrajectoryReference(
+    6100           0 :         mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6101             : 
+    6102           0 :     tracker_names.push_back(_tracker_names_[active_tracker_idx_]);
+    6103             : 
+    6104           0 :     if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6105             : 
+    6106           0 :       success  = response->success;
+    6107           0 :       message  = response->message;
+    6108           0 :       modified = response->modified || trajectory_modified;
+    6109           0 :       tracker_successes.push_back(response->success);
+    6110           0 :       tracker_messages.push_back(response->message);
+    6111             : 
+    6112             :     } else {
+    6113             : 
+    6114           0 :       ss << "the active tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setTrajectoryReference()' function!";
+    6115           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the trajectory: " << ss.str());
+    6116             : 
+    6117           0 :       success  = false;
+    6118           0 :       message  = ss.str();
+    6119           0 :       modified = false;
+    6120           0 :       tracker_successes.push_back(false);
+    6121           0 :       tracker_messages.push_back(ss.str());
+    6122             :     }
+    6123             : 
+    6124             :     // set the trajectory to the non-active trackers
+    6125           0 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6126             : 
+    6127           0 :       if (i != active_tracker_idx_) {
+    6128             : 
+    6129           0 :         tracker_names.push_back(_tracker_names_[i]);
+    6130             : 
+    6131           0 :         response = tracker_list_[i]->setTrajectoryReference(
+    6132           0 :             mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6133             : 
+    6134           0 :         if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6135             : 
+    6136           0 :           tracker_successes.push_back(response->success);
+    6137           0 :           tracker_messages.push_back(response->message);
+    6138             : 
+    6139           0 :           if (response->success) {
+    6140           0 :             std::stringstream ss;
+    6141           0 :             ss << "trajectory loaded to non-active tracker '" << _tracker_names_[i];
+    6142           0 :             ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6143             :           }
+    6144             : 
+    6145             :         } else {
+    6146             : 
+    6147           0 :           std::stringstream ss;
+    6148           0 :           ss << "the tracker \"" << _tracker_names_[i] << "\" does not implement setTrajectoryReference()";
+    6149           0 :           tracker_successes.push_back(false);
+    6150           0 :           tracker_messages.push_back(ss.str());
+    6151             :         }
+    6152             :       }
+    6153             :     }
+    6154             :   }
+    6155             : 
+    6156           0 :   return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+    6157             : }
+    6158             : 
+    6159             : //}
+    6160             : 
+    6161             : /* isOffboard() //{ */
+    6162             : 
+    6163           5 : bool ControlManager::isOffboard(void) {
+    6164             : 
+    6165           5 :   if (!sh_hw_api_status_.hasMsg()) {
+    6166           0 :     return false;
+    6167             :   }
+    6168             : 
+    6169           5 :   mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+    6170             : 
+    6171           5 :   return hw_api_status->connected && hw_api_status->offboard;
+    6172             : }
+    6173             : 
+    6174             : //}
+    6175             : 
+    6176             : /* setCallbacks() //{ */
+    6177             : 
+    6178           1 : void ControlManager::setCallbacks(bool in) {
+    6179             : 
+    6180           1 :   callbacks_enabled_ = in;
+    6181             : 
+    6182           1 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    6183           1 :   req_enable_callbacks.data = callbacks_enabled_;
+    6184             : 
+    6185             :   {
+    6186           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    6187             : 
+    6188             :     // set callbacks to all trackers
+    6189           7 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6190           6 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6191             :     }
+    6192             :   }
+    6193           1 : }
+    6194             : 
+    6195             : //}
+    6196             : 
+    6197             : /* publishDiagnostics() //{ */
+    6198             : 
+    6199        1078 : void ControlManager::publishDiagnostics(void) {
+    6200             : 
+    6201        1078 :   if (!is_initialized_) {
+    6202           0 :     return;
+    6203             :   }
+    6204             : 
+    6205        3234 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publishDiagnostics");
+    6206        3234 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    6207             : 
+    6208        2156 :   std::scoped_lock lock(mutex_diagnostics_);
+    6209             : 
+    6210        2156 :   mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+    6211             : 
+    6212        1078 :   diagnostics_msg.stamp    = ros::Time::now();
+    6213        1078 :   diagnostics_msg.uav_name = _uav_name_;
+    6214             : 
+    6215        1078 :   diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+    6216             : 
+    6217        1078 :   diagnostics_msg.output_enabled = output_enabled_;
+    6218             : 
+    6219        1078 :   diagnostics_msg.rc_mode = rc_goto_active_;
+    6220             : 
+    6221             :   {
+    6222        1078 :     std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+    6223             : 
+    6224        1078 :     diagnostics_msg.flying_normally = isFlyingNormally();
+    6225             :   }
+    6226             : 
+    6227             :   // | ----------------- fill the tracker status ---------------- |
+    6228             : 
+    6229             :   {
+    6230        2156 :     std::scoped_lock lock(mutex_tracker_list_);
+    6231             : 
+    6232        1078 :     mrs_msgs::TrackerStatus tracker_status;
+    6233             : 
+    6234        1078 :     diagnostics_msg.active_tracker = _tracker_names_[active_tracker_idx_];
+    6235        1078 :     diagnostics_msg.tracker_status = tracker_list_[active_tracker_idx_]->getStatus();
+    6236             :   }
+    6237             : 
+    6238             :   // | --------------- fill the controller status --------------- |
+    6239             : 
+    6240             :   {
+    6241        2156 :     std::scoped_lock lock(mutex_controller_list_);
+    6242             : 
+    6243        1078 :     mrs_msgs::ControllerStatus controller_status;
+    6244             : 
+    6245        1078 :     diagnostics_msg.active_controller = _controller_names_[active_controller_idx_];
+    6246        1078 :     diagnostics_msg.controller_status = controller_list_[active_controller_idx_]->getStatus();
+    6247             :   }
+    6248             : 
+    6249             :   // | ------------ fill in the available controllers ----------- |
+    6250             : 
+    6251        6468 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    6252        5390 :     if ((_controller_names_[i] != _failsafe_controller_name_) && (_controller_names_[i] != _eland_controller_name_)) {
+    6253        3234 :       diagnostics_msg.available_controllers.push_back(_controller_names_[i]);
+    6254        3234 :       diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_[i]).human_switchable);
+    6255             :     }
+    6256             :   }
+    6257             : 
+    6258             :   // | ------------- fill in the available trackers ------------- |
+    6259             : 
+    6260        7546 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    6261        6468 :     if (_tracker_names_[i] != _null_tracker_name_) {
+    6262        5390 :       diagnostics_msg.available_trackers.push_back(_tracker_names_[i]);
+    6263        5390 :       diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_[i]).human_switchable);
+    6264             :     }
+    6265             :   }
+    6266             : 
+    6267             :   // | ------------------------- publish ------------------------ |
+    6268             : 
+    6269        1078 :   ph_diagnostics_.publish(diagnostics_msg);
+    6270             : }
+    6271             : 
+    6272             : //}
+    6273             : 
+    6274             : /* setConstraintsToTrackers() //{ */
+    6275             : 
+    6276          24 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6277             : 
+    6278          72 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+    6279          72 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+    6280             : 
+    6281          24 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6282             : 
+    6283             :   {
+    6284          48 :     std::scoped_lock lock(mutex_tracker_list_);
+    6285             : 
+    6286             :     // for each tracker
+    6287         168 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6288             : 
+    6289             :       // if it is the active one, update and retrieve the command
+    6290         432 :       response = tracker_list_[i]->setConstraints(
+    6291         432 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6292             :     }
+    6293             :   }
+    6294          24 : }
+    6295             : 
+    6296             : //}
+    6297             : 
+    6298             : /* setConstraintsToControllers() //{ */
+    6299             : 
+    6300          32 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6301             : 
+    6302          96 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+    6303          96 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+    6304             : 
+    6305          32 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6306             : 
+    6307             :   {
+    6308          64 :     std::scoped_lock lock(mutex_controller_list_);
+    6309             : 
+    6310             :     // for each controller
+    6311         192 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    6312             : 
+    6313             :       // if it is the active one, update and retrieve the command
+    6314         480 :       response = controller_list_[i]->setConstraints(
+    6315         480 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6316             :     }
+    6317             :   }
+    6318          32 : }
+    6319             : 
+    6320             : //}
+    6321             : 
+    6322             : /* setConstraints() //{ */
+    6323             : 
+    6324           7 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6325             : 
+    6326          21 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraints");
+    6327          21 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+    6328             : 
+    6329           7 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6330             : 
+    6331           7 :   setConstraintsToTrackers(constraints);
+    6332             : 
+    6333           7 :   setConstraintsToControllers(constraints);
+    6334           7 : }
+    6335             : 
+    6336             : //}
+    6337             : 
+    6338             : 
+    6339             : /* enforceControllerConstraints() //{ */
+    6340             : 
+    6341        6762 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+    6342             :     const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6343             : 
+    6344             :   // copy member variables
+    6345       13524 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6346             : 
+    6347        6762 :   if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+    6348        3263 :     return {};
+    6349             :   }
+    6350             : 
+    6351        3499 :   bool enforcing = false;
+    6352             : 
+    6353        3499 :   auto constraints_out = constraints;
+    6354             : 
+    6355        6998 :   std::scoped_lock lock(mutex_tracker_list_);
+    6356             : 
+    6357             :   // enforce horizontal speed
+    6358        3499 :   if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+    6359        1146 :     constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+    6360             : 
+    6361        1146 :     enforcing = true;
+    6362             :   }
+    6363             : 
+    6364             :   // enforce horizontal acceleration
+    6365        3499 :   if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+    6366        1905 :     constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+    6367             : 
+    6368        1905 :     enforcing = true;
+    6369             :   }
+    6370             : 
+    6371             :   // enforce vertical ascending speed
+    6372        3499 :   if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+    6373        1146 :     constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+    6374             : 
+    6375        1146 :     enforcing = true;
+    6376             :   }
+    6377             : 
+    6378             :   // enforce vertical ascending acceleration
+    6379        3499 :   if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+    6380           0 :     constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+    6381             : 
+    6382           0 :     enforcing = true;
+    6383             :   }
+    6384             : 
+    6385             :   // enforce vertical descending speed
+    6386        3499 :   if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+    6387        1146 :     constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+    6388             : 
+    6389        1146 :     enforcing = true;
+    6390             :   }
+    6391             : 
+    6392             :   // enforce vertical descending acceleration
+    6393        3499 :   if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+    6394           0 :     constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+    6395             : 
+    6396           0 :     enforcing = true;
+    6397             :   }
+    6398             : 
+    6399        3499 :   if (enforcing) {
+    6400        1905 :     return {constraints_out};
+    6401             :   } else {
+    6402        1594 :     return {};
+    6403             :   }
+    6404             : }
+    6405             : 
+    6406             : //}
+    6407             : 
+    6408             : /* isFlyingNormally() //{ */
+    6409             : 
+    6410        1078 : bool ControlManager::isFlyingNormally(void) {
+    6411             : 
+    6412        1870 :   return (output_enabled_) && (offboard_mode_) && (armed_) &&
+    6413         781 :          (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+    6414         921 :            (active_controller_idx_ != _failsafe_controller_idx_)) ||
+    6415        2429 :           _controller_names_.size() == 1) &&
+    6416        1300 :          (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+    6417             : }
+    6418             : 
+    6419             : //}
+    6420             : 
+    6421             : /* //{ getMass() */
+    6422             : 
+    6423          39 : double ControlManager::getMass(void) {
+    6424             : 
+    6425          78 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6426             : 
+    6427          39 :   if (last_control_output.diagnostics.mass_estimator) {
+    6428           4 :     return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    6429             :   } else {
+    6430          35 :     return _uav_mass_;
+    6431             :   }
+    6432             : }
+    6433             : 
+    6434             : //}
+    6435             : 
+    6436             : /* loadConfigFile() //{ */
+    6437             : 
+    6438           0 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+    6439             : 
+    6440           0 :   const std::string name_space = nh_.getNamespace() + "/" + ns;
+    6441             : 
+    6442           0 :   ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+    6443             : 
+    6444             :   // load the user-requested file
+    6445             :   {
+    6446           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    6447           0 :     int         result  = std::system(command.c_str());
+    6448             : 
+    6449           0 :     if (result != 0) {
+    6450           0 :       ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+    6451           0 :       return false;
+    6452             :     }
+    6453             :   }
+    6454             : 
+    6455             :   // load the platform config
+    6456           0 :   if (_platform_config_ != "") {
+    6457           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    6458           0 :     int         result  = std::system(command.c_str());
+    6459             : 
+    6460           0 :     if (result != 0) {
+    6461           0 :       ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+    6462           0 :       return false;
+    6463             :     }
+    6464             :   }
+    6465             : 
+    6466             :   // load the custom config
+    6467           0 :   if (_custom_config_ != "") {
+    6468           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    6469           0 :     int         result  = std::system(command.c_str());
+    6470             : 
+    6471           0 :     if (result != 0) {
+    6472           0 :       ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+    6473           0 :       return false;
+    6474             :     }
+    6475             :   }
+    6476             : 
+    6477           0 :   return true;
+    6478             : }
+    6479             : 
+    6480             : //}
+    6481             : 
+    6482             : // | ----------------------- safety area ---------------------- |
+    6483             : 
+    6484             : /* //{ isPointInSafetyArea3d() */
+    6485             : 
+    6486           5 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+    6487             : 
+    6488           5 :   if (!use_safety_area_) {
+    6489           2 :     return true;
+    6490             :   }
+    6491             : 
+    6492           6 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6493             : 
+    6494           3 :   if (!tfed_horizontal) {
+    6495           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6496           0 :     return false;
+    6497             :   }
+    6498             : 
+    6499           3 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6500           0 :     return false;
+    6501             :   }
+    6502             : 
+    6503           3 :   if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+    6504           0 :     return false;
+    6505             :   }
+    6506             : 
+    6507           3 :   return true;
+    6508             : }
+    6509             : 
+    6510             : //}
+    6511             : 
+    6512             : /* //{ isPointInSafetyArea2d() */
+    6513             : 
+    6514         194 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+    6515             : 
+    6516         194 :   if (!use_safety_area_) {
+    6517           3 :     return true;
+    6518             :   }
+    6519             : 
+    6520         382 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6521             : 
+    6522         191 :   if (!tfed_horizontal) {
+    6523           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6524           0 :     return false;
+    6525             :   }
+    6526             : 
+    6527         191 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6528           0 :     return false;
+    6529             :   }
+    6530             : 
+    6531         191 :   return true;
+    6532             : }
+    6533             : 
+    6534             : //}
+    6535             : 
+    6536             : /* //{ isPathToPointInSafetyArea3d() */
+    6537             : 
+    6538           3 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6539             : 
+    6540           3 :   if (!use_safety_area_) {
+    6541           2 :     return true;
+    6542             :   }
+    6543             : 
+    6544           1 :   if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+    6545           0 :     return false;
+    6546             :   }
+    6547             : 
+    6548           2 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6549             : 
+    6550             :   {
+    6551           1 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6552             : 
+    6553           1 :     if (!ret) {
+    6554             : 
+    6555           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6556             : 
+    6557           0 :       return false;
+    6558             :     }
+    6559             : 
+    6560           1 :     start_transformed = ret.value();
+    6561             :   }
+    6562             : 
+    6563             :   {
+    6564           1 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6565             : 
+    6566           1 :     if (!ret) {
+    6567             : 
+    6568           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6569             : 
+    6570           0 :       return false;
+    6571             :     }
+    6572             : 
+    6573           1 :     end_transformed = ret.value();
+    6574             :   }
+    6575             : 
+    6576           1 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6577           1 :                                    end_transformed.reference.position.y);
+    6578             : }
+    6579             : 
+    6580             : //}
+    6581             : 
+    6582             : /* //{ isPathToPointInSafetyArea2d() */
+    6583             : 
+    6584           0 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6585           0 :   if (!use_safety_area_) {
+    6586           0 :     return true;
+    6587             :   }
+    6588             : 
+    6589           0 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6590             : 
+    6591           0 :   if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+    6592           0 :     return false;
+    6593             :   }
+    6594             : 
+    6595             :   {
+    6596           0 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6597             : 
+    6598           0 :     if (!ret) {
+    6599             : 
+    6600           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6601             : 
+    6602           0 :       return false;
+    6603             :     }
+    6604             : 
+    6605           0 :     start_transformed = ret.value();
+    6606             :   }
+    6607             : 
+    6608             :   {
+    6609           0 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6610             : 
+    6611           0 :     if (!ret) {
+    6612             : 
+    6613           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6614             : 
+    6615           0 :       return false;
+    6616             :     }
+    6617             : 
+    6618           0 :     end_transformed = ret.value();
+    6619             :   }
+    6620             : 
+    6621           0 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6622           0 :                                    end_transformed.reference.position.y);
+    6623             : }
+    6624             : 
+    6625             : //}
+    6626             : 
+    6627             : /* //{ getMaxZ() */
+    6628             : 
+    6629         478 : double ControlManager::getMaxZ(const std::string& frame_id) {
+    6630             : 
+    6631             :   // | ------- first, calculate max_z from the safety area ------ |
+    6632             : 
+    6633         478 :   double safety_area_max_z = std::numeric_limits<float>::max();
+    6634             : 
+    6635             :   {
+    6636             : 
+    6637         956 :     geometry_msgs::PointStamped point;
+    6638             : 
+    6639         478 :     point.header.frame_id = _safety_area_vertical_frame_;
+    6640         478 :     point.point.x         = 0;
+    6641         478 :     point.point.y         = 0;
+    6642         478 :     point.point.z         = _safety_area_max_z_;
+    6643             : 
+    6644         478 :     auto ret = transformer_->transformSingle(point, frame_id);
+    6645             : 
+    6646         478 :     if (!ret) {
+    6647           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+    6648             :     }
+    6649             : 
+    6650         478 :     safety_area_max_z = ret->point.z;
+    6651             :   }
+    6652             : 
+    6653             :   // | ------------ overwrite from estimation manager ----------- |
+    6654             : 
+    6655         478 :   double estimation_manager_max_z = std::numeric_limits<float>::max();
+    6656             : 
+    6657             :   {
+    6658             :     // if possible, override it with max z from the estimation manager
+    6659         478 :     if (sh_max_z_.hasMsg()) {
+    6660             : 
+    6661         956 :       auto msg = sh_max_z_.getMsg();
+    6662             : 
+    6663             :       // transform it into the safety area frame
+    6664         956 :       geometry_msgs::PointStamped point;
+    6665         478 :       point.header  = msg->header;
+    6666         478 :       point.point.x = 0;
+    6667         478 :       point.point.y = 0;
+    6668         478 :       point.point.z = msg->value;
+    6669             : 
+    6670         478 :       auto ret = transformer_->transformSingle(point, frame_id);
+    6671             : 
+    6672         478 :       if (!ret) {
+    6673           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+    6674             :       }
+    6675             : 
+    6676         478 :       estimation_manager_max_z = ret->point.z;
+    6677             :     }
+    6678             :   }
+    6679             : 
+    6680         478 :   if (estimation_manager_max_z < safety_area_max_z) {
+    6681           0 :     return estimation_manager_max_z;
+    6682             :   } else {
+    6683         478 :     return safety_area_max_z;
+    6684             :   }
+    6685             : }
+    6686             : 
+    6687             : //}
+    6688             : 
+    6689             : /* //{ getMinZ() */
+    6690             : 
+    6691        3371 : double ControlManager::getMinZ(const std::string& frame_id) {
+    6692             : 
+    6693        3371 :   if (!use_safety_area_) {
+    6694        1693 :     return std::numeric_limits<double>::lowest();
+    6695             :   }
+    6696             : 
+    6697        3356 :   geometry_msgs::PointStamped point;
+    6698             : 
+    6699        1678 :   point.header.frame_id = _safety_area_vertical_frame_;
+    6700        1678 :   point.point.x         = 0;
+    6701        1678 :   point.point.y         = 0;
+    6702        1678 :   point.point.z         = _safety_area_min_z_;
+    6703             : 
+    6704        3356 :   auto ret = transformer_->transformSingle(point, frame_id);
+    6705             : 
+    6706        1678 :   if (!ret) {
+    6707           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+    6708           0 :     return std::numeric_limits<double>::lowest();
+    6709             :   }
+    6710             : 
+    6711        1678 :   return ret->point.z;
+    6712             : }
+    6713             : 
+    6714             : //}
+    6715             : 
+    6716             : // | --------------------- obstacle bumper -------------------- |
+    6717             : 
+    6718             : /* bumperPushFromObstacle() //{ */
+    6719             : 
+    6720           0 : bool ControlManager::bumperPushFromObstacle(void) {
+    6721           0 :   if (!bumper_enabled_) {
+    6722           0 :     return true;
+    6723             :   }
+    6724             : 
+    6725           0 :   if (!sh_bumper_.hasMsg()) {
+    6726           0 :     return true;
+    6727             :   }
+    6728             : 
+    6729             :   // copy member variables
+    6730           0 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6731           0 :   auto                              uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    6732             : 
+    6733           0 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    6734             : 
+    6735           0 :   double direction                     = 0;
+    6736           0 :   double repulsion_distance            = std::numeric_limits<double>::max();
+    6737           0 :   bool   horizontal_collision_detected = false;
+    6738             : 
+    6739           0 :   bool vertical_collision_detected = false;
+    6740             : 
+    6741           0 :   for (int i = 0; i < int(bumper_data->n_horizontal_sectors); i++) {
+    6742             : 
+    6743           0 :     if (bumper_data->sectors[i] < 0) {
+    6744           0 :       continue;
+    6745             :     }
+    6746             : 
+    6747           0 :     bool wall_locked_horizontal = false;
+    6748             : 
+    6749             :     // if the sector is under critical distance
+    6750           0 :     if (bumper_data->sectors[i] <= _bumper_horizontal_distance_ && bumper_data->sectors[i] < repulsion_distance) {
+    6751             : 
+    6752             :       // check for locking between the oposite walls
+    6753             :       // get the desired direction of motion
+    6754           0 :       double oposite_direction  = double(i) * sector_size + M_PI;
+    6755           0 :       int    oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+    6756             : 
+    6757           0 :       if (bumper_data->sectors[oposite_sector_idx] > 0 &&
+    6758           0 :           ((bumper_data->sectors[i] + bumper_data->sectors[oposite_sector_idx]) <= (2 * _bumper_horizontal_distance_ + 2 * _bumper_horizontal_overshoot_))) {
+    6759             : 
+    6760           0 :         wall_locked_horizontal = true;
+    6761             : 
+    6762           0 :         if (fabs(bumper_data->sectors[i] - bumper_data->sectors[oposite_sector_idx]) <= 2 * _bumper_horizontal_overshoot_) {
+    6763             : 
+    6764           0 :           ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: locked between two walls");
+    6765           0 :           continue;
+    6766             :         }
+    6767             :       }
+    6768             : 
+    6769             :       // get the id of the oposite sector
+    6770           0 :       direction = oposite_direction;
+    6771             : 
+    6772             :       /* int oposite_sector_idx = (i + bumper_data->n_horizontal_sectors / 2) % bumper_data->n_horizontal_sectors; */
+    6773             : 
+    6774           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %d vs. %d), obstacle distance: %.2f, repulsing", i,
+    6775             :                         oposite_sector_idx, bumper_data->sectors[i]);
+    6776             : 
+    6777           0 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: oposite direction: %.2f", oposite_direction);
+    6778             : 
+    6779           0 :       if (wall_locked_horizontal) {
+    6780           0 :         if (bumper_data->sectors[i] < bumper_data->sectors[oposite_sector_idx]) {
+    6781           0 :           repulsion_distance = _bumper_horizontal_overshoot_;
+    6782             :         } else {
+    6783           0 :           repulsion_distance = -_bumper_horizontal_overshoot_;
+    6784             :         }
+    6785             :       } else {
+    6786           0 :         repulsion_distance = _bumper_horizontal_distance_ + _bumper_horizontal_overshoot_ - bumper_data->sectors[i];
+    6787             :       }
+    6788             : 
+    6789           0 :       horizontal_collision_detected = true;
+    6790             :     }
+    6791             :   }
+    6792             : 
+    6793           0 :   bool   collision_above             = false;
+    6794           0 :   bool   collision_below             = false;
+    6795           0 :   double vertical_repulsion_distance = 0;
+    6796             : 
+    6797             :   // check for vertical collision down
+    6798           0 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors] > 0 && bumper_data->sectors[bumper_data->n_horizontal_sectors] <= _bumper_vertical_distance_) {
+    6799             : 
+    6800           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+    6801           0 :     collision_above             = true;
+    6802           0 :     vertical_collision_detected = true;
+    6803           0 :     vertical_repulsion_distance = _bumper_vertical_distance_ - bumper_data->sectors[bumper_data->n_horizontal_sectors];
+    6804             :   }
+    6805             : 
+    6806             :   // check for vertical collision up
+    6807           0 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] > 0 &&
+    6808           0 :       bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] <= _bumper_vertical_distance_) {
+    6809             : 
+    6810           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+    6811           0 :     collision_below             = true;
+    6812           0 :     vertical_collision_detected = true;
+    6813           0 :     vertical_repulsion_distance = -(_bumper_vertical_distance_ - bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]);
+    6814             :   }
+    6815             : 
+    6816             :   // check the up/down wall locking
+    6817           0 :   if (collision_above && collision_below) {
+    6818             : 
+    6819           0 :     if (((bumper_data->sectors[bumper_data->n_horizontal_sectors] + bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) <=
+    6820           0 :          (2 * _bumper_vertical_distance_ + 2 * _bumper_vertical_overshoot_))) {
+    6821             : 
+    6822           0 :       vertical_repulsion_distance =
+    6823           0 :           (-bumper_data->sectors[bumper_data->n_horizontal_sectors] + bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) / 2.0;
+    6824             : 
+    6825           0 :       if (fabs(bumper_data->sectors[bumper_data->n_horizontal_sectors] - bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) <=
+    6826           0 :           2 * _bumper_vertical_overshoot_) {
+    6827             : 
+    6828           0 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: locked between the floor and ceiling");
+    6829           0 :         vertical_collision_detected = false;
+    6830             :       }
+    6831             :     }
+    6832             :   }
+    6833             : 
+    6834             :   // if potential collision was detected and we should start the repulsing_
+    6835           0 :   if (horizontal_collision_detected || vertical_collision_detected) {
+    6836             : 
+    6837           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: repulsion was initiated");
+    6838             : 
+    6839           0 :     if (!repulsing_) {
+    6840             : 
+    6841           0 :       if (_bumper_switch_tracker_) {
+    6842             : 
+    6843           0 :         auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6844           0 :         std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    6845             : 
+    6846             :         // remember the previously active tracker
+    6847           0 :         bumper_previous_tracker_ = active_tracker_name;
+    6848             : 
+    6849           0 :         if (active_tracker_name != _bumper_tracker_name_) {
+    6850             : 
+    6851           0 :           switchTracker(_bumper_tracker_name_);
+    6852             :         }
+    6853             :       }
+    6854             : 
+    6855           0 :       if (_bumper_switch_controller_) {
+    6856             : 
+    6857           0 :         auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6858           0 :         std::string active_controller_name = _controller_names_[active_controller_idx];
+    6859             : 
+    6860             :         // remember the previously active controller
+    6861           0 :         bumper_previous_controller_ = active_controller_name;
+    6862             : 
+    6863           0 :         if (active_controller_name != _bumper_controller_name_) {
+    6864             : 
+    6865           0 :           switchController(_bumper_controller_name_);
+    6866             :         }
+    6867             :       }
+    6868             :     }
+    6869             : 
+    6870           0 :     repulsing_ = true;
+    6871             : 
+    6872           0 :     mrs_msgs::BumperStatus bumper_status;
+    6873           0 :     bumper_status.repulsing = repulsing_;
+    6874             : 
+    6875           0 :     ph_bumper_status_.publish(bumper_status);
+    6876             : 
+    6877           0 :     callbacks_enabled_ = false;
+    6878             : 
+    6879           0 :     mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    6880             : 
+    6881           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6882             : 
+    6883             :     // create the reference in the fcu_untilted frame
+    6884           0 :     mrs_msgs::ReferenceStamped reference_fcu_untilted;
+    6885             : 
+    6886           0 :     reference_fcu_untilted.header.frame_id = "fcu_untilted";
+    6887             : 
+    6888           0 :     if (horizontal_collision_detected) {
+    6889           0 :       reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+    6890           0 :       reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+    6891             :     } else {
+    6892           0 :       reference_fcu_untilted.reference.position.x = 0;
+    6893           0 :       reference_fcu_untilted.reference.position.y = 0;
+    6894             :     }
+    6895             : 
+    6896           0 :     reference_fcu_untilted.reference.heading = 0;
+    6897             : 
+    6898           0 :     if (vertical_collision_detected) {
+    6899           0 :       reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+    6900             :     } else {
+    6901           0 :       reference_fcu_untilted.reference.position.z = 0;
+    6902             :     }
+    6903             : 
+    6904             :     {
+    6905           0 :       std::scoped_lock lock(mutex_tracker_list_);
+    6906             : 
+    6907             :       // transform the reference into the currently used frame
+    6908             :       // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+    6909             :       // to the tracker before we actually call the goto service
+    6910             : 
+    6911           0 :       auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+    6912             : 
+    6913           0 :       if (!ret) {
+    6914             : 
+    6915           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+    6916           0 :         return false;
+    6917             :       }
+    6918             : 
+    6919           0 :       reference_fcu_untilted = ret.value();
+    6920             : 
+    6921             :       // copy the reference into the service type message
+    6922           0 :       mrs_msgs::ReferenceSrvRequest req_goto_out;
+    6923           0 :       req_goto_out.reference = reference_fcu_untilted.reference;
+    6924             : 
+    6925             :       // disable callbacks of all trackers
+    6926           0 :       req_enable_callbacks.data = false;
+    6927           0 :       for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6928           0 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6929             :       }
+    6930             : 
+    6931             :       // enable the callbacks for the active tracker
+    6932           0 :       req_enable_callbacks.data = true;
+    6933           0 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6934             : 
+    6935             :       // call the goto
+    6936           0 :       tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    6937           0 :           mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    6938             : 
+    6939             :       // disable the callbacks back again
+    6940           0 :       req_enable_callbacks.data = false;
+    6941           0 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6942             :     }
+    6943             :   }
+    6944             : 
+    6945             :   // if repulsing_ and the distance is safe once again
+    6946           0 :   if ((repulsing_ && !horizontal_collision_detected && !vertical_collision_detected)) {
+    6947             : 
+    6948           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: repulsion was stopped");
+    6949             : 
+    6950           0 :     if (_bumper_switch_tracker_) {
+    6951             : 
+    6952           0 :       auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6953           0 :       std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    6954             : 
+    6955           0 :       if (active_tracker_name != bumper_previous_tracker_) {
+    6956             : 
+    6957           0 :         switchTracker(bumper_previous_tracker_);
+    6958             :       }
+    6959             :     }
+    6960             : 
+    6961           0 :     if (_bumper_switch_controller_) {
+    6962             : 
+    6963           0 :       auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6964           0 :       std::string active_controller_name = _controller_names_[active_controller_idx];
+    6965             : 
+    6966           0 :       if (active_controller_name != bumper_previous_controller_) {
+    6967             : 
+    6968           0 :         switchController(bumper_previous_controller_);
+    6969             :       }
+    6970             :     }
+    6971             : 
+    6972           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6973             : 
+    6974             :     {
+    6975           0 :       std::scoped_lock lock(mutex_tracker_list_);
+    6976             : 
+    6977             :       // enable callbacks of all trackers
+    6978           0 :       req_enable_callbacks.data = true;
+    6979           0 :       for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6980           0 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6981             :       }
+    6982             :     }
+    6983             : 
+    6984           0 :     callbacks_enabled_ = true;
+    6985             : 
+    6986           0 :     repulsing_ = false;
+    6987             :   }
+    6988             : 
+    6989           0 :   return false;
+    6990             : }
+    6991             : 
+    6992             : //}
+    6993             : 
+    6994             : /* bumperGetSectorId() //{ */
+    6995             : 
+    6996           0 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+    6997             :   // copy member variables
+    6998           0 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6999             : 
+    7000             :   // heading of the point in drone frame
+    7001           0 :   double point_heading_horizontal = atan2(y, x);
+    7002             : 
+    7003           0 :   point_heading_horizontal += TAU;
+    7004             : 
+    7005             :   // if point_heading_horizontal is greater then 2*M_PI mod it
+    7006           0 :   if (fabs(point_heading_horizontal) >= TAU) {
+    7007           0 :     point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+    7008             :   }
+    7009             : 
+    7010             :   // heading of the right edge of the first sector
+    7011           0 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    7012             : 
+    7013             :   // calculate the idx
+    7014           0 :   int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+    7015             : 
+    7016           0 :   if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+    7017           0 :     idx -= bumper_data->n_horizontal_sectors;
+    7018             :   }
+    7019             : 
+    7020           0 :   return idx;
+    7021             : }
+    7022             : 
+    7023             : //}
+    7024             : 
+    7025             : // | ------------------------- safety ------------------------- |
+    7026             : 
+    7027             : /* //{ changeLandingState() */
+    7028             : 
+    7029           6 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+    7030             :   // copy member variables
+    7031          12 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7032             : 
+    7033             :   {
+    7034           6 :     std::scoped_lock lock(mutex_landing_state_machine_);
+    7035             : 
+    7036           6 :     previous_state_landing_ = current_state_landing_;
+    7037           6 :     current_state_landing_  = new_state;
+    7038             :   }
+    7039             : 
+    7040           6 :   switch (current_state_landing_) {
+    7041             : 
+    7042           3 :     case IDLE_STATE:
+    7043           3 :       break;
+    7044           3 :     case LANDING_STATE: {
+    7045             : 
+    7046           3 :       ROS_DEBUG("[ControlManager]: starting eland timer");
+    7047           3 :       timer_eland_.start();
+    7048           3 :       ROS_DEBUG("[ControlManager]: eland timer started");
+    7049           3 :       eland_triggered_ = true;
+    7050           3 :       bumper_enabled_  = false;
+    7051             : 
+    7052           3 :       landing_uav_mass_ = getMass();
+    7053             :     }
+    7054             : 
+    7055           3 :     break;
+    7056             :   }
+    7057             : 
+    7058           6 :   ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+    7059           6 : }
+    7060             : 
+    7061             : //}
+    7062             : 
+    7063             : /* hover() //{ */
+    7064             : 
+    7065           0 : std::tuple<bool, std::string> ControlManager::hover(void) {
+    7066           0 :   if (!is_initialized_)
+    7067           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7068             : 
+    7069           0 :   if (eland_triggered_)
+    7070           0 :     return std::tuple(false, "cannot hover, eland already triggered");
+    7071             : 
+    7072           0 :   if (failsafe_triggered_)
+    7073           0 :     return std::tuple(false, "cannot hover, failsafe already triggered");
+    7074             : 
+    7075             :   {
+    7076           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7077             : 
+    7078           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7079           0 :     std_srvs::TriggerRequest            request;
+    7080             : 
+    7081           0 :     response = tracker_list_[active_tracker_idx_]->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7082             : 
+    7083           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7084             : 
+    7085           0 :       return std::tuple(response->success, response->message);
+    7086             : 
+    7087             :     } else {
+    7088             : 
+    7089           0 :       std::stringstream ss;
+    7090           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'hover()' function!";
+    7091             : 
+    7092           0 :       return std::tuple(false, ss.str());
+    7093             :     }
+    7094             :   }
+    7095             : }
+    7096             : 
+    7097             : //}
+    7098             : 
+    7099             : /* //{ ehover() */
+    7100             : 
+    7101           0 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+    7102           0 :   if (!is_initialized_)
+    7103           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7104             : 
+    7105           0 :   if (eland_triggered_)
+    7106           0 :     return std::tuple(false, "cannot ehover, eland already triggered");
+    7107             : 
+    7108           0 :   if (failsafe_triggered_)
+    7109           0 :     return std::tuple(false, "cannot ehover, failsafe already triggered");
+    7110             : 
+    7111             :   // copy the member variables
+    7112           0 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7113           0 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7114           0 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7115             : 
+    7116           0 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7117             : 
+    7118           0 :     std::stringstream ss;
+    7119           0 :     ss << "can not trigger ehover while not flying";
+    7120           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7121             : 
+    7122           0 :     return std::tuple(false, ss.str());
+    7123             :   }
+    7124             : 
+    7125           0 :   ungripSrv();
+    7126             : 
+    7127             :   {
+    7128             : 
+    7129           0 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7130             : 
+    7131             :     // check if the tracker was successfully switched
+    7132             :     // this is vital, that is the core of the hover
+    7133           0 :     if (!success) {
+    7134             : 
+    7135           0 :       std::stringstream ss;
+    7136           0 :       ss << "error during switching to ehover tracker: '" << message << "'";
+    7137           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7138             : 
+    7139           0 :       return std::tuple(success, ss.str());
+    7140             :     }
+    7141             :   }
+    7142             : 
+    7143             :   {
+    7144           0 :     auto [success, message] = switchController(_eland_controller_name_);
+    7145             : 
+    7146             :     // check if the controller was successfully switched
+    7147             :     // this is not vital, we can continue without that
+    7148           0 :     if (!success) {
+    7149             : 
+    7150           0 :       std::stringstream ss;
+    7151           0 :       ss << "error during switching to ehover controller: '" << message << "'";
+    7152           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7153             :     }
+    7154             :   }
+    7155             : 
+    7156           0 :   std::stringstream ss;
+    7157           0 :   ss << "ehover activated";
+    7158           0 :   ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7159             : 
+    7160           0 :   callbacks_enabled_ = false;
+    7161             : 
+    7162           0 :   return std::tuple(true, ss.str());
+    7163             : }
+    7164             : 
+    7165             : //}
+    7166             : 
+    7167             : /* eland() //{ */
+    7168             : 
+    7169           3 : std::tuple<bool, std::string> ControlManager::eland(void) {
+    7170           3 :   if (!is_initialized_)
+    7171           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7172             : 
+    7173           3 :   if (eland_triggered_)
+    7174           0 :     return std::tuple(false, "cannot eland, eland already triggered");
+    7175             : 
+    7176           3 :   if (failsafe_triggered_)
+    7177           0 :     return std::tuple(false, "cannot eland, failsafe already triggered");
+    7178             : 
+    7179             :   // copy member variables
+    7180           6 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7181           6 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7182           3 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7183             : 
+    7184           3 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7185             : 
+    7186           0 :     std::stringstream ss;
+    7187           0 :     ss << "can not trigger eland while not flying";
+    7188           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7189             : 
+    7190           0 :     return std::tuple(false, ss.str());
+    7191             :   }
+    7192             : 
+    7193           3 :   if (_rc_emergency_handoff_) {
+    7194             : 
+    7195           0 :     toggleOutput(false);
+    7196             : 
+    7197           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7198             :   }
+    7199             : 
+    7200             :   {
+    7201           3 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7202             : 
+    7203             :     // check if the tracker was successfully switched
+    7204             :     // this is vital
+    7205           3 :     if (!success) {
+    7206             : 
+    7207           0 :       std::stringstream ss;
+    7208           0 :       ss << "error during switching to eland tracker: '" << message << "'";
+    7209           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7210             : 
+    7211           0 :       return std::tuple(success, ss.str());
+    7212             :     }
+    7213             :   }
+    7214             : 
+    7215             :   {
+    7216           6 :     auto [success, message] = switchController(_eland_controller_name_);
+    7217             : 
+    7218             :     // check if the controller was successfully switched
+    7219             :     // this is not vital, we can continue without it
+    7220           3 :     if (!success) {
+    7221             : 
+    7222           0 :       std::stringstream ss;
+    7223           0 :       ss << "error during switching to eland controller: '" << message << "'";
+    7224           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7225             :     }
+    7226             :   }
+    7227             : 
+    7228             :   // | ----------------- call the eland service ----------------- |
+    7229             : 
+    7230           3 :   std::stringstream ss;
+    7231             :   bool              success;
+    7232             : 
+    7233           3 :   if (elandSrv()) {
+    7234             : 
+    7235           3 :     changeLandingState(LANDING_STATE);
+    7236             : 
+    7237           3 :     odometryCallbacksSrv(false);
+    7238             : 
+    7239           3 :     ss << "eland activated";
+    7240           3 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7241             : 
+    7242           3 :     success = true;
+    7243             : 
+    7244           3 :     callbacks_enabled_ = false;
+    7245             : 
+    7246             :   } else {
+    7247             : 
+    7248           0 :     ss << "error during activation of eland";
+    7249           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7250             : 
+    7251           0 :     success = false;
+    7252             :   }
+    7253             : 
+    7254           6 :   return std::tuple(success, ss.str());
+    7255             : }
+    7256             : 
+    7257             : //}
+    7258             : 
+    7259             : /* failsafe() //{ */
+    7260             : 
+    7261           1 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+    7262             :   // copy member variables
+    7263           2 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7264           2 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7265           1 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7266           1 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7267             : 
+    7268           1 :   if (!is_initialized_) {
+    7269           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7270             :   }
+    7271             : 
+    7272           1 :   if (failsafe_triggered_) {
+    7273           0 :     return std::tuple(false, "cannot, failsafe already triggered");
+    7274             :   }
+    7275             : 
+    7276           1 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7277             : 
+    7278           0 :     std::stringstream ss;
+    7279           0 :     ss << "can not trigger failsafe while not flying";
+    7280           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7281           0 :     return std::tuple(false, ss.str());
+    7282             :   }
+    7283             : 
+    7284           1 :   if (_rc_emergency_handoff_) {
+    7285             : 
+    7286           0 :     toggleOutput(false);
+    7287             : 
+    7288           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7289             :   }
+    7290             : 
+    7291           1 :   if (_parachute_enabled_) {
+    7292             : 
+    7293           0 :     auto [success, message] = deployParachute();
+    7294             : 
+    7295           0 :     if (success) {
+    7296             : 
+    7297           0 :       std::stringstream ss;
+    7298           0 :       ss << "failsafe activated (parachute): '" << message << "'";
+    7299           0 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7300             : 
+    7301           0 :       return std::tuple(true, ss.str());
+    7302             : 
+    7303             :     } else {
+    7304             : 
+    7305           0 :       std::stringstream ss;
+    7306           0 :       ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+    7307           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7308             :     }
+    7309             :   }
+    7310             : 
+    7311           1 :   if (_failsafe_controller_idx_ != active_controller_idx) {
+    7312             : 
+    7313             :     try {
+    7314             : 
+    7315           2 :       std::scoped_lock lock(mutex_controller_list_);
+    7316             : 
+    7317           1 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+    7318           1 :       controller_list_[_failsafe_controller_idx_]->activate(last_control_output);
+    7319             : 
+    7320             :       {
+    7321           2 :         std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7322             : 
+    7323             :         // update the time (used in failsafe)
+    7324           1 :         controller_tracker_switch_time_ = ros::Time::now();
+    7325             :       }
+    7326             : 
+    7327           1 :       failsafe_triggered_ = true;
+    7328           1 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    7329           1 :       timer_eland_.stop();
+    7330           1 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7331             : 
+    7332           1 :       landing_uav_mass_ = getMass();
+    7333             : 
+    7334           1 :       eland_triggered_ = false;
+    7335           1 :       ROS_DEBUG("[ControlManager]: starting failsafe timer");
+    7336           1 :       timer_failsafe_.start();
+    7337           1 :       ROS_DEBUG("[ControlManager]: failsafe timer started");
+    7338             : 
+    7339           1 :       bumper_enabled_ = false;
+    7340             : 
+    7341           1 :       odometryCallbacksSrv(false);
+    7342             : 
+    7343           1 :       callbacks_enabled_ = false;
+    7344             : 
+    7345           1 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+    7346             : 
+    7347             :       // super important, switch the active controller idx
+    7348             :       try {
+    7349           1 :         controller_list_[active_controller_idx_]->deactivate();
+    7350           1 :         active_controller_idx_ = _failsafe_controller_idx_;
+    7351             :       }
+    7352           0 :       catch (std::runtime_error& exrun) {
+    7353           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    7354             :       }
+    7355             :     }
+    7356           0 :     catch (std::runtime_error& exrun) {
+    7357           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+    7358           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    7359             :     }
+    7360             :   }
+    7361             : 
+    7362           2 :   return std::tuple(true, "failsafe activated");
+    7363             : }
+    7364             : 
+    7365             : //}
+    7366             : 
+    7367             : /* escalatingFailsafe() //{ */
+    7368             : 
+    7369           0 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+    7370           0 :   std::stringstream ss;
+    7371             : 
+    7372           0 :   if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+    7373             : 
+    7374           0 :     ss << "too soon for escalating failsafe";
+    7375           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7376             : 
+    7377           0 :     return std::tuple(false, ss.str());
+    7378             :   }
+    7379             : 
+    7380           0 :   if (!output_enabled_) {
+    7381             : 
+    7382           0 :     ss << "not escalating failsafe, output is disabled";
+    7383           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7384             : 
+    7385           0 :     return std::tuple(false, ss.str());
+    7386             :   }
+    7387             : 
+    7388           0 :   ROS_WARN("[ControlManager]: escalating failsafe triggered");
+    7389             : 
+    7390           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7391           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7392             : 
+    7393           0 :   std::string active_tracker_name    = _tracker_names_[active_tracker_idx];
+    7394           0 :   std::string active_controller_name = _controller_names_[active_controller_idx];
+    7395             : 
+    7396           0 :   EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+    7397             : 
+    7398           0 :   escalating_failsafe_time_ = ros::Time::now();
+    7399             : 
+    7400           0 :   switch (next_state) {
+    7401             : 
+    7402           0 :     case ESC_NONE_STATE: {
+    7403             : 
+    7404           0 :       ss << "escalating failsafe has run to impossible situation";
+    7405           0 :       ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7406             : 
+    7407           0 :       return std::tuple(false, "escalating failsafe has run to impossible situation");
+    7408             : 
+    7409             :       break;
+    7410             :     }
+    7411             : 
+    7412           0 :     case ESC_EHOVER_STATE: {
+    7413             : 
+    7414           0 :       ss << "escalating failsafe escalates to ehover";
+    7415           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7416             : 
+    7417           0 :       auto [success, message] = ehover();
+    7418             : 
+    7419           0 :       if (success) {
+    7420           0 :         state_escalating_failsafe_ = ESC_EHOVER_STATE;
+    7421             :       }
+    7422             : 
+    7423           0 :       return {success, message};
+    7424             : 
+    7425             :       break;
+    7426             :     }
+    7427             : 
+    7428           0 :     case ESC_ELAND_STATE: {
+    7429             : 
+    7430           0 :       ss << "escalating failsafe escalates to eland";
+    7431           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7432             : 
+    7433           0 :       auto [success, message] = eland();
+    7434             : 
+    7435           0 :       if (success) {
+    7436           0 :         state_escalating_failsafe_ = ESC_ELAND_STATE;
+    7437             :       }
+    7438             : 
+    7439           0 :       return {success, message};
+    7440             : 
+    7441             :       break;
+    7442             :     }
+    7443             : 
+    7444           0 :     case ESC_FAILSAFE_STATE: {
+    7445             : 
+    7446           0 :       escalating_failsafe_time_ = ros::Time::now();
+    7447             : 
+    7448           0 :       ss << "escalating failsafe escalates to failsafe";
+    7449           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7450             : 
+    7451           0 :       auto [success, message] = failsafe();
+    7452             : 
+    7453           0 :       if (success) {
+    7454           0 :         state_escalating_failsafe_ = ESC_FINISHED_STATE;
+    7455             :       }
+    7456             : 
+    7457           0 :       return {success, message};
+    7458             : 
+    7459             :       break;
+    7460             :     }
+    7461             : 
+    7462           0 :     case ESC_FINISHED_STATE: {
+    7463             : 
+    7464           0 :       escalating_failsafe_time_ = ros::Time::now();
+    7465             : 
+    7466           0 :       ss << "escalating failsafe has nothing more to do";
+    7467           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7468             : 
+    7469           0 :       return std::tuple(false, "escalating failsafe has nothing more to do");
+    7470             : 
+    7471             :       break;
+    7472             :     }
+    7473             : 
+    7474           0 :     default: {
+    7475             : 
+    7476           0 :       break;
+    7477             :     }
+    7478             :   }
+    7479             : 
+    7480           0 :   ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+    7481             : 
+    7482           0 :   return std::tuple(false, "escalating failsafe exception");
+    7483             : }
+    7484             : 
+    7485             : //}
+    7486             : 
+    7487             : /* getNextEscFailsafeState() //{ */
+    7488             : 
+    7489           0 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+    7490           0 :   EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+    7491             : 
+    7492           0 :   switch (current_state) {
+    7493             : 
+    7494           0 :     case ESC_FINISHED_STATE: {
+    7495             : 
+    7496           0 :       return ESC_FINISHED_STATE;
+    7497             : 
+    7498             :       break;
+    7499             :     }
+    7500             : 
+    7501           0 :     case ESC_NONE_STATE: {
+    7502             : 
+    7503           0 :       if (_escalating_failsafe_ehover_) {
+    7504           0 :         return ESC_EHOVER_STATE;
+    7505           0 :       } else if (_escalating_failsafe_eland_) {
+    7506           0 :         return ESC_ELAND_STATE;
+    7507           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7508           0 :         return ESC_FAILSAFE_STATE;
+    7509             :       } else {
+    7510           0 :         return ESC_FINISHED_STATE;
+    7511             :       }
+    7512             : 
+    7513             :       break;
+    7514             :     }
+    7515             : 
+    7516           0 :     case ESC_EHOVER_STATE: {
+    7517             : 
+    7518           0 :       if (_escalating_failsafe_eland_) {
+    7519           0 :         return ESC_ELAND_STATE;
+    7520           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7521           0 :         return ESC_FAILSAFE_STATE;
+    7522             :       } else {
+    7523           0 :         return ESC_FINISHED_STATE;
+    7524             :       }
+    7525             : 
+    7526             :       break;
+    7527             :     }
+    7528             : 
+    7529           0 :     case ESC_ELAND_STATE: {
+    7530             : 
+    7531           0 :       if (_escalating_failsafe_failsafe_) {
+    7532           0 :         return ESC_FAILSAFE_STATE;
+    7533             :       } else {
+    7534           0 :         return ESC_FINISHED_STATE;
+    7535             :       }
+    7536             : 
+    7537             :       break;
+    7538             :     }
+    7539             : 
+    7540           0 :     case ESC_FAILSAFE_STATE: {
+    7541             : 
+    7542           0 :       return ESC_FINISHED_STATE;
+    7543             : 
+    7544             :       break;
+    7545             :     }
+    7546             :   }
+    7547             : 
+    7548           0 :   ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+    7549             : 
+    7550           0 :   return ESC_NONE_STATE;
+    7551             : }
+    7552             : 
+    7553             : //}
+    7554             : 
+    7555             : // | ------------------- trajectory tracking ------------------ |
+    7556             : 
+    7557             : /* startTrajectoryTracking() //{ */
+    7558             : 
+    7559           0 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+    7560           0 :   if (!is_initialized_)
+    7561           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7562             : 
+    7563             :   {
+    7564           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7565             : 
+    7566           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7567           0 :     std_srvs::TriggerRequest            request;
+    7568             : 
+    7569             :     response =
+    7570           0 :         tracker_list_[active_tracker_idx_]->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7571             : 
+    7572           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7573             : 
+    7574           0 :       return std::tuple(response->success, response->message);
+    7575             : 
+    7576             :     } else {
+    7577             : 
+    7578           0 :       std::stringstream ss;
+    7579           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'startTrajectoryTracking()' function!";
+    7580             : 
+    7581           0 :       return std::tuple(false, ss.str());
+    7582             :     }
+    7583             :   }
+    7584             : }
+    7585             : 
+    7586             : //}
+    7587             : 
+    7588             : /* stopTrajectoryTracking() //{ */
+    7589             : 
+    7590           0 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+    7591           0 :   if (!is_initialized_)
+    7592           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7593             : 
+    7594             :   {
+    7595           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7596             : 
+    7597           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7598           0 :     std_srvs::TriggerRequest            request;
+    7599             : 
+    7600             :     response =
+    7601           0 :         tracker_list_[active_tracker_idx_]->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7602             : 
+    7603           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7604             : 
+    7605           0 :       return std::tuple(response->success, response->message);
+    7606             : 
+    7607             :     } else {
+    7608             : 
+    7609           0 :       std::stringstream ss;
+    7610           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'stopTrajectoryTracking()' function!";
+    7611             : 
+    7612           0 :       return std::tuple(false, ss.str());
+    7613             :     }
+    7614             :   }
+    7615             : }
+    7616             : 
+    7617             : //}
+    7618             : 
+    7619             : /* resumeTrajectoryTracking() //{ */
+    7620             : 
+    7621           0 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+    7622           0 :   if (!is_initialized_)
+    7623           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7624             : 
+    7625             :   {
+    7626           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7627             : 
+    7628           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7629           0 :     std_srvs::TriggerRequest            request;
+    7630             : 
+    7631             :     response =
+    7632           0 :         tracker_list_[active_tracker_idx_]->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7633             : 
+    7634           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7635             : 
+    7636           0 :       return std::tuple(response->success, response->message);
+    7637             : 
+    7638             :     } else {
+    7639             : 
+    7640           0 :       std::stringstream ss;
+    7641           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'resumeTrajectoryTracking()' function!";
+    7642             : 
+    7643           0 :       return std::tuple(false, ss.str());
+    7644             :     }
+    7645             :   }
+    7646             : }
+    7647             : 
+    7648             : //}
+    7649             : 
+    7650             : /* gotoTrajectoryStart() //{ */
+    7651             : 
+    7652           0 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+    7653           0 :   if (!is_initialized_)
+    7654           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7655             : 
+    7656             :   {
+    7657           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7658             : 
+    7659           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7660           0 :     std_srvs::TriggerRequest            request;
+    7661             : 
+    7662           0 :     response = tracker_list_[active_tracker_idx_]->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7663             : 
+    7664           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7665             : 
+    7666           0 :       return std::tuple(response->success, response->message);
+    7667             : 
+    7668             :     } else {
+    7669             : 
+    7670           0 :       std::stringstream ss;
+    7671           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'gotoTrajectoryStart()' function!";
+    7672             : 
+    7673           0 :       return std::tuple(false, ss.str());
+    7674             :     }
+    7675             :   }
+    7676             : }
+    7677             : 
+    7678             : //}
+    7679             : 
+    7680             : // | ----------------- service client wrappers ---------------- |
+    7681             : 
+    7682             : /* arming() //{ */
+    7683             : 
+    7684           5 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+    7685          10 :   std::stringstream ss;
+    7686             : 
+    7687           5 :   if (input) {
+    7688             : 
+    7689           0 :     ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+    7690           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7691           0 :     return std::tuple(false, ss.str());
+    7692             :   }
+    7693             : 
+    7694           5 :   if (!input && !isOffboard()) {
+    7695             : 
+    7696           0 :     ss << "can not disarm, not in OFFBOARD mode";
+    7697           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7698           0 :     return std::tuple(false, ss.str());
+    7699             :   }
+    7700             : 
+    7701           5 :   if (!input && _rc_emergency_handoff_) {
+    7702             : 
+    7703           0 :     toggleOutput(false);
+    7704             : 
+    7705           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7706             :   }
+    7707             : 
+    7708           5 :   std_srvs::SetBool srv_out;
+    7709             : 
+    7710           5 :   srv_out.request.data = input ? 1 : 0;  // arm or disarm?
+    7711             : 
+    7712           5 :   ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+    7713             : 
+    7714           5 :   if (sch_arming_.call(srv_out)) {
+    7715             : 
+    7716           5 :     if (srv_out.response.success) {
+    7717             : 
+    7718           5 :       ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+    7719           5 :       ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7720             : 
+    7721           5 :       if (!input) {
+    7722             : 
+    7723           5 :         toggleOutput(false);
+    7724             : 
+    7725           5 :         ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+    7726           5 :         timer_failsafe_.stop();
+    7727           5 :         ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+    7728             : 
+    7729           5 :         ROS_DEBUG("[ControlManager]: stopping the eland timer");
+    7730           5 :         timer_eland_.stop();
+    7731           5 :         ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7732             : 
+    7733           5 :         shutdown();
+    7734             :       }
+    7735             : 
+    7736             :     } else {
+    7737           0 :       ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+    7738           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7739             :     }
+    7740             : 
+    7741             :   } else {
+    7742           0 :     ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+    7743           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7744             :   }
+    7745             : 
+    7746          10 :   return std::tuple(srv_out.response.success, ss.str());
+    7747             : }
+    7748             : 
+    7749             : //}
+    7750             : 
+    7751             : /* odometryCallbacksSrv() //{ */
+    7752             : 
+    7753           4 : void ControlManager::odometryCallbacksSrv(const bool input) {
+    7754           4 :   ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    7755             : 
+    7756           8 :   std_srvs::SetBool srv;
+    7757             : 
+    7758           4 :   srv.request.data = input;
+    7759             : 
+    7760           4 :   bool res = sch_set_odometry_callbacks_.call(srv);
+    7761             : 
+    7762           4 :   if (res) {
+    7763             : 
+    7764           4 :     if (!srv.response.success) {
+    7765           0 :       ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+    7766             :     }
+    7767             : 
+    7768             :   } else {
+    7769           0 :     ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+    7770             :   }
+    7771           4 : }
+    7772             : 
+    7773             : //}
+    7774             : 
+    7775             : /* elandSrv() //{ */
+    7776             : 
+    7777           3 : bool ControlManager::elandSrv(void) {
+    7778           3 :   ROS_INFO("[ControlManager]: calling for eland");
+    7779             : 
+    7780           6 :   std_srvs::Trigger srv;
+    7781             : 
+    7782           3 :   bool res = sch_eland_.call(srv);
+    7783             : 
+    7784           3 :   if (res) {
+    7785             : 
+    7786           3 :     if (!srv.response.success) {
+    7787           0 :       ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    7788             :     }
+    7789             : 
+    7790           3 :     return srv.response.success;
+    7791             : 
+    7792             :   } else {
+    7793             : 
+    7794           0 :     ROS_ERROR("[ControlManager]: service call for eland failed!");
+    7795             : 
+    7796           0 :     return false;
+    7797             :   }
+    7798             : }
+    7799             : 
+    7800             : //}
+    7801             : 
+    7802             : /* shutdown() //{ */
+    7803             : 
+    7804           8 : void ControlManager::shutdown() {
+    7805             :   // copy member variables
+    7806          16 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    7807             : 
+    7808           8 :   if (_automatic_pc_shutdown_enabled_) {
+    7809             : 
+    7810           0 :     ROS_INFO("[ControlManager]: calling service for PC shutdown");
+    7811             : 
+    7812           0 :     std_srvs::Trigger shutdown_out;
+    7813           0 :     sch_shutdown_.call(shutdown_out);
+    7814             :   }
+    7815           8 : }
+    7816             : 
+    7817             : //}
+    7818             : 
+    7819             : /* parachuteSrv() //{ */
+    7820             : 
+    7821           0 : bool ControlManager::parachuteSrv(void) {
+    7822           0 :   ROS_INFO("[ControlManager]: calling for parachute deployment");
+    7823             : 
+    7824           0 :   std_srvs::Trigger srv;
+    7825             : 
+    7826           0 :   bool res = sch_parachute_.call(srv);
+    7827             : 
+    7828           0 :   if (res) {
+    7829             : 
+    7830           0 :     if (!srv.response.success) {
+    7831           0 :       ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+    7832             :     }
+    7833             : 
+    7834           0 :     return srv.response.success;
+    7835             : 
+    7836             :   } else {
+    7837             : 
+    7838           0 :     ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+    7839             : 
+    7840           0 :     return false;
+    7841             :   }
+    7842             : }
+    7843             : 
+    7844             : //}
+    7845             : 
+    7846             : /* ungripSrv() //{ */
+    7847             : 
+    7848         103 : void ControlManager::ungripSrv(void) {
+    7849         206 :   std_srvs::Trigger srv;
+    7850             : 
+    7851         103 :   bool res = sch_ungrip_.call(srv);
+    7852             : 
+    7853         103 :   if (res) {
+    7854             : 
+    7855           0 :     if (!srv.response.success) {
+    7856           0 :       ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+    7857             :     }
+    7858             : 
+    7859             :   } else {
+    7860         103 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+    7861             :   }
+    7862         103 : }
+    7863             : 
+    7864             : //}
+    7865             : 
+    7866             : // | ------------------------ routines ------------------------ |
+    7867             : 
+    7868             : /* toggleOutput() //{ */
+    7869             : 
+    7870          15 : void ControlManager::toggleOutput(const bool& input) {
+    7871             : 
+    7872          15 :   if (input == output_enabled_) {
+    7873           3 :     ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+    7874           3 :     return;
+    7875             :   }
+    7876             : 
+    7877          12 :   ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+    7878             : 
+    7879          12 :   output_enabled_ = input;
+    7880             : 
+    7881             :   // if switching output off, switch to NullTracker
+    7882          12 :   if (!output_enabled_) {
+    7883             : 
+    7884           5 :     ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+    7885             : 
+    7886           5 :     switchTracker(_null_tracker_name_);
+    7887             : 
+    7888           5 :     ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+    7889             : 
+    7890           5 :     switchController(_eland_controller_name_);
+    7891             : 
+    7892             :     // | --------- deactivate all trackers and controllers -------- |
+    7893             : 
+    7894          35 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    7895             : 
+    7896          30 :       std::map<std::string, TrackerParams>::iterator it;
+    7897          30 :       it = trackers_.find(_tracker_names_[i]);
+    7898             : 
+    7899             :       try {
+    7900          30 :         ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+    7901          30 :         tracker_list_[i]->deactivate();
+    7902             :       }
+    7903           0 :       catch (std::runtime_error& ex) {
+    7904           0 :         ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+    7905             :       }
+    7906             :     }
+    7907             : 
+    7908          30 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    7909             : 
+    7910          25 :       std::map<std::string, ControllerParams>::iterator it;
+    7911          25 :       it = controllers_.find(_controller_names_[i]);
+    7912             : 
+    7913             :       try {
+    7914          25 :         ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+    7915          25 :         controller_list_[i]->deactivate();
+    7916             :       }
+    7917           0 :       catch (std::runtime_error& ex) {
+    7918           0 :         ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+    7919             :       }
+    7920             :     }
+    7921             : 
+    7922           5 :     timer_failsafe_.stop();
+    7923           5 :     timer_eland_.stop();
+    7924           5 :     timer_pirouette_.stop();
+    7925             : 
+    7926           5 :     offboard_mode_was_true_ = false;
+    7927             :   }
+    7928             : }
+    7929             : 
+    7930             : //}
+    7931             : 
+    7932             : /* switchTracker() //{ */
+    7933             : 
+    7934          24 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+    7935          72 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchTracker");
+    7936          72 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+    7937             : 
+    7938             :   // copy member variables
+    7939          48 :   auto last_tracker_cmd   = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7940          24 :   auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7941             : 
+    7942          48 :   std::stringstream ss;
+    7943             : 
+    7944          24 :   if (!got_uav_state_) {
+    7945             : 
+    7946           0 :     ss << "can not switch tracker, missing odometry!";
+    7947           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7948           0 :     return std::tuple(false, ss.str());
+    7949             :   }
+    7950             : 
+    7951          24 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    7952             : 
+    7953           0 :     ss << "can not switch tracker, missing odometry innovation!";
+    7954           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7955           0 :     return std::tuple(false, ss.str());
+    7956             :   }
+    7957             : 
+    7958          24 :   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+    7959             : 
+    7960             :   // check if the tracker exists
+    7961          24 :   if (!new_tracker_idx) {
+    7962           0 :     ss << "the tracker '" << tracker_name << "' does not exist!";
+    7963           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7964           0 :     return std::tuple(false, ss.str());
+    7965             :   }
+    7966             : 
+    7967             :   // check if the tracker is already active
+    7968          24 :   if (new_tracker_idx.value() == active_tracker_idx) {
+    7969           1 :     ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+    7970           1 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7971           1 :     return std::tuple(true, ss.str());
+    7972             :   }
+    7973             : 
+    7974             :   {
+    7975          23 :     std::scoped_lock lock(mutex_tracker_list_);
+    7976             : 
+    7977             :     try {
+    7978             : 
+    7979          23 :       ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_[new_tracker_idx.value()].c_str());
+    7980             : 
+    7981          23 :       auto [success, message] = tracker_list_[new_tracker_idx.value()]->activate(last_tracker_cmd);
+    7982             : 
+    7983          23 :       if (!success) {
+    7984             : 
+    7985           0 :         ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+    7986           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7987           0 :         return std::tuple(false, ss.str());
+    7988             : 
+    7989             :       } else {
+    7990             : 
+    7991          23 :         ss << "the tracker '" << tracker_name << "' was activated";
+    7992          23 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7993             : 
+    7994             :         {
+    7995          46 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7996             : 
+    7997             :           // update the time (used in failsafe)
+    7998          23 :           controller_tracker_switch_time_ = ros::Time::now();
+    7999             :         }
+    8000             : 
+    8001             :         // super important, switch the active tracker idx
+    8002             :         try {
+    8003             : 
+    8004          23 :           ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    8005          23 :           tracker_list_[active_tracker_idx_]->deactivate();
+    8006             : 
+    8007             :           // if switching from null tracker, re-activate the already active the controller
+    8008          23 :           if (active_tracker_idx_ == _null_tracker_idx_) {
+    8009             : 
+    8010           7 :             ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    8011             :             {
+    8012          14 :               std::scoped_lock lock(mutex_controller_list_);
+    8013             : 
+    8014           7 :               initializeControlOutput();
+    8015             : 
+    8016          14 :               auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8017             : 
+    8018           7 :               controller_list_[active_controller_idx_]->activate(last_control_output);
+    8019             : 
+    8020             :               {
+    8021          14 :                 std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8022             : 
+    8023             :                 // update the time (used in failsafe)
+    8024           7 :                 controller_tracker_switch_time_ = ros::Time::now();
+    8025             :               }
+    8026             :             }
+    8027             : 
+    8028             :             // if switching to null tracker, deactivate the active controller
+    8029          16 :           } else if (new_tracker_idx == _null_tracker_idx_) {
+    8030             : 
+    8031           5 :             ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    8032             :             {
+    8033          10 :               std::scoped_lock lock(mutex_controller_list_);
+    8034             : 
+    8035           5 :               controller_list_[active_controller_idx_]->deactivate();
+    8036             :             }
+    8037             : 
+    8038             :             {
+    8039           5 :               std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8040             : 
+    8041           5 :               last_tracker_cmd_ = {};
+    8042             :             }
+    8043             : 
+    8044           5 :             initializeControlOutput();
+    8045             :           }
+    8046             : 
+    8047          23 :           active_tracker_idx_ = new_tracker_idx.value();
+    8048             :         }
+    8049           0 :         catch (std::runtime_error& exrun) {
+    8050           0 :           ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    8051             :         }
+    8052             :       }
+    8053             :     }
+    8054           0 :     catch (std::runtime_error& exrun) {
+    8055           0 :       ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+    8056           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8057             :     }
+    8058             :   }
+    8059             : 
+    8060          23 :   return std::tuple(true, ss.str());
+    8061             : }
+    8062             : 
+    8063             : //}
+    8064             : 
+    8065             : /* switchController() //{ */
+    8066             : 
+    8067          23 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+    8068          69 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchController");
+    8069          69 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+    8070             : 
+    8071             :   // copy member variables
+    8072          46 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8073          46 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8074          23 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8075             : 
+    8076          46 :   std::stringstream ss;
+    8077             : 
+    8078          23 :   if (!got_uav_state_) {
+    8079             : 
+    8080           0 :     ss << "can not switch controller, missing odometry!";
+    8081           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8082           0 :     return std::tuple(false, ss.str());
+    8083             :   }
+    8084             : 
+    8085          23 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8086             : 
+    8087           0 :     ss << "can not switch controller, missing odometry innovation!";
+    8088           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8089           0 :     return std::tuple(false, ss.str());
+    8090             :   }
+    8091             : 
+    8092          23 :   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+    8093             : 
+    8094             :   // check if the controller exists
+    8095          23 :   if (!new_controller_idx) {
+    8096           0 :     ss << "the controller '" << controller_name << "' does not exist!";
+    8097           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8098           0 :     return std::tuple(false, ss.str());
+    8099             :   }
+    8100             : 
+    8101             :   // check if the controller is not active
+    8102          23 :   if (new_controller_idx.value() == active_controller_idx) {
+    8103             : 
+    8104           5 :     ss << "not switching, the controller '" << controller_name << "' is already active!";
+    8105           5 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8106           5 :     return std::tuple(true, ss.str());
+    8107             :   }
+    8108             : 
+    8109             :   {
+    8110          18 :     std::scoped_lock lock(mutex_controller_list_);
+    8111             : 
+    8112             :     try {
+    8113             : 
+    8114          18 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_[new_controller_idx.value()].c_str());
+    8115          18 :       if (!controller_list_[new_controller_idx.value()]->activate(last_control_output)) {
+    8116             : 
+    8117           0 :         ss << "the controller '" << controller_name << "' was not activated";
+    8118           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8119           0 :         return std::tuple(false, ss.str());
+    8120             : 
+    8121             :       } else {
+    8122             : 
+    8123          18 :         ss << "the controller '" << controller_name << "' was activated";
+    8124          18 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8125             : 
+    8126          18 :         ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_[new_controller_idx.value()].c_str(),
+    8127             :                  _tracker_names_[active_tracker_idx_].c_str());
+    8128             : 
+    8129             :         // reactivate the current tracker
+    8130             :         // TODO this is not the most elegant way to restart the tracker after a controller switch
+    8131             :         // but it serves the purpose
+    8132             :         {
+    8133          18 :           std::scoped_lock lock(mutex_tracker_list_);
+    8134             : 
+    8135          18 :           tracker_list_[active_tracker_idx_]->deactivate();
+    8136          18 :           tracker_list_[active_tracker_idx_]->activate({});
+    8137             :         }
+    8138             : 
+    8139             :         {
+    8140          36 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8141             : 
+    8142             :           // update the time (used in failsafe)
+    8143          18 :           controller_tracker_switch_time_ = ros::Time::now();
+    8144             :         }
+    8145             : 
+    8146             :         // super important, switch the active controller idx
+    8147             :         try {
+    8148             : 
+    8149          18 :           controller_list_[active_controller_idx_]->deactivate();
+    8150          18 :           active_controller_idx_ = new_controller_idx.value();
+    8151             :         }
+    8152           0 :         catch (std::runtime_error& exrun) {
+    8153           0 :           ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    8154             :         }
+    8155             :       }
+    8156             :     }
+    8157           0 :     catch (std::runtime_error& exrun) {
+    8158           0 :       ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+    8159           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8160             :     }
+    8161             :   }
+    8162             : 
+    8163          18 :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+    8164             : 
+    8165             :   {
+    8166          18 :     std::scoped_lock lock(mutex_constraints_);
+    8167             : 
+    8168          18 :     sanitized_constraints_ = current_constraints_;
+    8169          18 :     sanitized_constraints  = sanitized_constraints_;
+    8170             :   }
+    8171             : 
+    8172          18 :   setConstraintsToControllers(sanitized_constraints);
+    8173             : 
+    8174          18 :   return std::tuple(true, ss.str());
+    8175             : }
+    8176             : 
+    8177             : //}
+    8178             : 
+    8179             : /* updateTrackers() //{ */
+    8180             : 
+    8181        6807 : void ControlManager::updateTrackers(void) {
+    8182       13614 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateTrackers");
+    8183       13614 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+    8184             : 
+    8185             :   // copy member variables
+    8186        6807 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8187        6807 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8188        6807 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8189             : 
+    8190             :   // --------------------------------------------------------------
+    8191             :   // |                     Update the trackers                    |
+    8192             :   // --------------------------------------------------------------
+    8193             : 
+    8194           0 :   std::optional<mrs_msgs::TrackerCommand> tracker_command;
+    8195             : 
+    8196             :   // for each tracker
+    8197       47649 :   for (size_t i = 0; i < tracker_list_.size(); i++) {
+    8198             : 
+    8199       40842 :     if (i == active_tracker_idx) {
+    8200             : 
+    8201             :       try {
+    8202        6807 :         std::scoped_lock lock(mutex_tracker_list_);
+    8203             : 
+    8204             :         // active tracker => update and retrieve the command
+    8205        6807 :         tracker_command = tracker_list_[i]->update(uav_state, last_control_output);
+    8206             :       }
+    8207           0 :       catch (std::runtime_error& exrun) {
+    8208             : 
+    8209           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active tracker (%s)", _tracker_names_[active_tracker_idx].c_str());
+    8210           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8211           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active tracker");
+    8212             : 
+    8213           0 :         eland();
+    8214             :       }
+    8215             : 
+    8216             :     } else {
+    8217             : 
+    8218             :       try {
+    8219       34035 :         std::scoped_lock lock(mutex_tracker_list_);
+    8220             : 
+    8221             :         // nonactive tracker => just update without retrieving the command
+    8222       34035 :         tracker_list_[i]->update(uav_state, last_control_output);
+    8223             :       }
+    8224           0 :       catch (std::runtime_error& exrun) {
+    8225             : 
+    8226           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the tracker '%s'", _tracker_names_[i].c_str());
+    8227           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8228           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the tracker");
+    8229             : 
+    8230           0 :         eland();
+    8231             :       }
+    8232             :     }
+    8233             :   }
+    8234             : 
+    8235        6807 :   if (active_tracker_idx == _null_tracker_idx_) {
+    8236        1421 :     return;
+    8237             :   }
+    8238             : 
+    8239        5386 :   if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+    8240             : 
+    8241       10772 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8242             : 
+    8243        5386 :     last_tracker_cmd_ = tracker_command;
+    8244             : 
+    8245             :     // | --------- fill in the potentially missing header --------- |
+    8246             : 
+    8247        5386 :     if (last_tracker_cmd_->header.frame_id == "") {
+    8248           0 :       last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+    8249             :     }
+    8250             : 
+    8251        5386 :     if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+    8252           0 :       last_tracker_cmd_->header.stamp = ros::Time::now();
+    8253             :     }
+    8254             : 
+    8255             :   } else {
+    8256             : 
+    8257           0 :     if (active_tracker_idx != _null_tracker_idx_) {
+    8258             : 
+    8259           0 :       if (active_tracker_idx == _ehover_tracker_idx_) {
+    8260             : 
+    8261           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the ehover tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8262             : 
+    8263           0 :         failsafe();
+    8264             : 
+    8265             :       } else {
+    8266             : 
+    8267           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8268             : 
+    8269           0 :         if (_tracker_error_action_ == ELAND_STR) {
+    8270           0 :           eland();
+    8271           0 :         } else if (_tracker_error_action_ == EHOVER_STR) {
+    8272           0 :           ehover();
+    8273             :         } else {
+    8274           0 :           failsafe();
+    8275             :         }
+    8276             :       }
+    8277             : 
+    8278             :     } else {
+    8279             : 
+    8280           0 :       std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8281             : 
+    8282           0 :       last_tracker_cmd_ = tracker_command;
+    8283             :     }
+    8284             :   }
+    8285             : }
+    8286             : 
+    8287             : //}
+    8288             : 
+    8289             : /* updateControllers() //{ */
+    8290             : 
+    8291        8204 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+    8292       16408 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateControllers");
+    8293       16408 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+    8294             : 
+    8295             :   // copy member variables
+    8296        8204 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8297        8204 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8298             : 
+    8299             :   // | ----------------- update the controllers ----------------- |
+    8300             : 
+    8301             :   // the trackers are not running
+    8302        8204 :   if (!last_tracker_cmd) {
+    8303             : 
+    8304        1421 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: tracker command is empty, giving controller just the uav_state");
+    8305             : 
+    8306             :     // give the controllers current uav state
+    8307             :     {
+    8308        2842 :       std::scoped_lock lock(mutex_controller_list_);
+    8309             : 
+    8310             :       // nonactive controller => just update without retrieving the command
+    8311        8526 :       for (int i = 0; i < int(controller_list_.size()); i++) {
+    8312        7105 :         controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8313             :       }
+    8314             :     }
+    8315             : 
+    8316        1421 :     return;
+    8317             :   }
+    8318             : 
+    8319       13566 :   Controller::ControlOutput control_output;
+    8320             : 
+    8321             :   // for each controller
+    8322       40698 :   for (size_t i = 0; i < controller_list_.size(); i++) {
+    8323             : 
+    8324       33915 :     if (i == active_controller_idx) {
+    8325             : 
+    8326             :       try {
+    8327        6783 :         std::scoped_lock lock(mutex_controller_list_);
+    8328             : 
+    8329             :         // active controller => update and retrieve the command
+    8330        6783 :         control_output = controller_list_[active_controller_idx]->updateActive(uav_state, last_tracker_cmd.value());
+    8331             :       }
+    8332           0 :       catch (std::runtime_error& exrun) {
+    8333             : 
+    8334           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active controller (%s)", _controller_names_[active_controller_idx].c_str());
+    8335           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8336             : 
+    8337           0 :         if (eland_triggered_) {
+    8338             : 
+    8339           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering failsafe due to an exception in the active controller (eland is already active)");
+    8340           0 :           failsafe();
+    8341             : 
+    8342             :         } else {
+    8343             : 
+    8344           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active controller");
+    8345           0 :           eland();
+    8346             :         }
+    8347             :       }
+    8348             : 
+    8349             :     } else {
+    8350             : 
+    8351             :       try {
+    8352       54264 :         std::scoped_lock lock(mutex_controller_list_);
+    8353             : 
+    8354             :         // nonactive controller => just update without retrieving the command
+    8355       27132 :         controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8356             :       }
+    8357           0 :       catch (std::runtime_error& exrun) {
+    8358             : 
+    8359           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_[i].c_str());
+    8360           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8361           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland (somebody should notice this)");
+    8362             : 
+    8363           0 :         eland();
+    8364             :       }
+    8365             :     }
+    8366             :   }
+    8367             : 
+    8368             :   // normally, the active controller returns a valid command
+    8369        6783 :   if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+    8370             : 
+    8371        6783 :     mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+    8372             : 
+    8373             :     // but it can return an empty command, due to some critical internal error
+    8374             :     // which means we should trigger the failsafe landing
+    8375             :   } else {
+    8376             : 
+    8377             :     // only if the controller is still active, trigger escalating failsafe
+    8378             :     // if not active, we don't care, we should not ask the controller for
+    8379             :     // the result anyway -> this could mean a race condition occured
+    8380             :     // like it once happend during landing
+    8381           0 :     bool controller_status = false;
+    8382             : 
+    8383             :     {
+    8384           0 :       std::scoped_lock lock(mutex_controller_list_);
+    8385             : 
+    8386           0 :       controller_status = controller_list_[active_controller_idx]->getStatus().active;
+    8387             :     }
+    8388             : 
+    8389           0 :     if (controller_status) {
+    8390             : 
+    8391           0 :       if (active_controller_idx_ == _failsafe_controller_idx_) {
+    8392             : 
+    8393           0 :         ROS_ERROR("[ControlManager]: failsafe controller returned empty command, disabling control");
+    8394             : 
+    8395           0 :         toggleOutput(false);
+    8396             : 
+    8397           0 :       } else if (active_controller_idx_ == _eland_controller_idx_) {
+    8398             : 
+    8399           0 :         ROS_ERROR("[ControlManager]: triggering failsafe, the emergency controller returned empty or invalid command");
+    8400             : 
+    8401           0 :         failsafe();
+    8402             : 
+    8403             :       } else {
+    8404             : 
+    8405           0 :         ROS_ERROR("[ControlManager]: triggering eland, the controller returned empty or invalid command");
+    8406             : 
+    8407           0 :         eland();
+    8408             :       }
+    8409             :     }
+    8410             :   }
+    8411             : }
+    8412             : 
+    8413             : //}
+    8414             : 
+    8415             : /* publish() //{ */
+    8416             : 
+    8417        8204 : void ControlManager::publish(void) {
+    8418       16408 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publish");
+    8419       16408 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+    8420             : 
+    8421             :   // copy member variables
+    8422        8204 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8423        8204 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8424        8204 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8425        8204 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8426        8204 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8427             : 
+    8428        8204 :   publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+    8429             : 
+    8430             :   // --------------------------------------------------------------
+    8431             :   // |                 Publish the control command                |
+    8432             :   // --------------------------------------------------------------
+    8433             : 
+    8434        8204 :   mrs_msgs::HwApiAttitudeCmd attitude_target;
+    8435        8204 :   attitude_target.stamp = ros::Time::now();
+    8436             : 
+    8437        8204 :   mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+    8438        8204 :   attitude_rate_target.stamp = ros::Time::now();
+    8439             : 
+    8440        8204 :   if (!output_enabled_) {
+    8441             : 
+    8442         901 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+    8443             : 
+    8444        7303 :   } else if (active_tracker_idx == _null_tracker_idx_) {
+    8445             : 
+    8446         521 :     ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+    8447             : 
+    8448             :     Controller::HwApiOutputVariant output =
+    8449        1042 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8450             : 
+    8451             :     {
+    8452        1042 :       std::scoped_lock lock(mutex_last_control_output_);
+    8453             : 
+    8454         521 :       last_control_output_.control_output = output;
+    8455             :     }
+    8456             : 
+    8457         521 :     control_output_publisher_.publish(output);
+    8458             : 
+    8459        6782 :   } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+    8460             : 
+    8461           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+    8462             :                       _controller_names_[active_controller_idx].c_str());
+    8463             : 
+    8464             :     Controller::HwApiOutputVariant output =
+    8465           0 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8466             : 
+    8467           0 :     control_output_publisher_.publish(output);
+    8468             : 
+    8469        6782 :   } else if (last_control_output.control_output) {
+    8470             : 
+    8471        6782 :     if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+    8472        6782 :       control_output_publisher_.publish(last_control_output.control_output.value());
+    8473             :     } else {
+    8474           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+    8475           0 :       return;
+    8476             :     }
+    8477             : 
+    8478             :   } else {
+    8479           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+    8480             :   }
+    8481             : 
+    8482             :   // | ----------- publish the controller diagnostics ----------- |
+    8483             : 
+    8484        8204 :   ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+    8485             : 
+    8486             :   // | --------- publish the applied throttle and thrust -------- |
+    8487             : 
+    8488        8204 :   auto throttle = extractThrottle(last_control_output);
+    8489             : 
+    8490        8204 :   if (throttle) {
+    8491             : 
+    8492             :     {
+    8493        7301 :       std_msgs::Float64 msg;
+    8494        7301 :       msg.data = throttle.value();
+    8495        7301 :       ph_throttle_.publish(msg);
+    8496             :     }
+    8497             : 
+    8498        7301 :     double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+    8499             : 
+    8500             :     {
+    8501        7301 :       std_msgs::Float64 msg;
+    8502        7301 :       msg.data = thrust;
+    8503        7301 :       ph_thrust_.publish(msg);
+    8504             :     }
+    8505             :   }
+    8506             : 
+    8507             :   // | ----------------- publish tracker command ---------------- |
+    8508             : 
+    8509        8204 :   if (last_tracker_cmd) {
+    8510        6782 :     ph_tracker_cmd_.publish(last_tracker_cmd.value());
+    8511             :   }
+    8512             : 
+    8513             :   // | --------------- publish the odometry input --------------- |
+    8514             : 
+    8515        8204 :   if (last_control_output.control_output) {
+    8516             : 
+    8517       14602 :     mrs_msgs::EstimatorInput msg;
+    8518             : 
+    8519        7301 :     msg.header.frame_id = _uav_name_ + "/fcu";
+    8520        7301 :     msg.header.stamp    = ros::Time::now();
+    8521             : 
+    8522        7301 :     if (last_control_output.desired_unbiased_acceleration) {
+    8523        5347 :       msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()[0];
+    8524        5347 :       msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()[1];
+    8525        5347 :       msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()[2];
+    8526             :     }
+    8527             : 
+    8528        7301 :     if (last_control_output.desired_heading_rate) {
+    8529        5347 :       msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+    8530             :     }
+    8531             : 
+    8532        7301 :     if (last_control_output.desired_unbiased_acceleration) {
+    8533        5347 :       ph_mrs_odom_input_.publish(msg);
+    8534             :     }
+    8535             :   }
+    8536             : }
+    8537             : 
+    8538             : //}
+    8539             : 
+    8540             : /* deployParachute() //{ */
+    8541             : 
+    8542           0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+    8543             :   // if not enabled, return false
+    8544           0 :   if (!_parachute_enabled_) {
+    8545             : 
+    8546           0 :     std::stringstream ss;
+    8547           0 :     ss << "can not deploy parachute, it is disabled";
+    8548           0 :     return std::tuple(false, ss.str());
+    8549             :   }
+    8550             : 
+    8551             :   // we can not disarm if the drone is not in offboard mode
+    8552             :   // this is super important!
+    8553           0 :   if (!isOffboard()) {
+    8554             : 
+    8555           0 :     std::stringstream ss;
+    8556           0 :     ss << "can not deploy parachute, not in offboard mode";
+    8557           0 :     return std::tuple(false, ss.str());
+    8558             :   }
+    8559             : 
+    8560             :   // call the parachute service
+    8561           0 :   bool succ = parachuteSrv();
+    8562             : 
+    8563             :   // if the deployment was successful,
+    8564           0 :   if (succ) {
+    8565             : 
+    8566           0 :     arming(false);
+    8567             : 
+    8568           0 :     std::stringstream ss;
+    8569           0 :     ss << "parachute deployed";
+    8570             : 
+    8571           0 :     return std::tuple(true, ss.str());
+    8572             : 
+    8573             :   } else {
+    8574             : 
+    8575           0 :     std::stringstream ss;
+    8576           0 :     ss << "error during deployment of parachute";
+    8577             : 
+    8578           0 :     return std::tuple(false, ss.str());
+    8579             :   }
+    8580             : }
+    8581             : 
+    8582             : //}
+    8583             : 
+    8584             : /* velocityReferenceToReference() //{ */
+    8585             : 
+    8586           0 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+    8587           0 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8588           0 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8589           0 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    8590             : 
+    8591           0 :   mrs_msgs::ReferenceStamped reference_out;
+    8592             : 
+    8593           0 :   reference_out.header = vel_reference.header;
+    8594             : 
+    8595           0 :   if (vel_reference.reference.use_heading) {
+    8596           0 :     reference_out.reference.heading = vel_reference.reference.heading;
+    8597           0 :   } else if (vel_reference.reference.use_heading_rate) {
+    8598           0 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+    8599             :   } else {
+    8600           0 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    8601             :   }
+    8602             : 
+    8603           0 :   if (vel_reference.reference.use_altitude) {
+    8604           0 :     reference_out.reference.position.z = vel_reference.reference.altitude;
+    8605             :   } else {
+    8606             : 
+    8607           0 :     double stopping_time_z = 0;
+    8608             : 
+    8609           0 :     if (vel_reference.reference.velocity.x >= 0) {
+    8610           0 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+    8611             :     } else {
+    8612           0 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+    8613             :     }
+    8614             : 
+    8615           0 :     reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+    8616             :   }
+    8617             : 
+    8618             :   {
+    8619           0 :     double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8620           0 :     double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8621             : 
+    8622           0 :     reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+    8623           0 :     reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+    8624             :   }
+    8625             : 
+    8626           0 :   return reference_out;
+    8627             : }
+    8628             : 
+    8629             : //}
+    8630             : 
+    8631             : /* publishControlReferenceOdom() //{ */
+    8632             : 
+    8633        8204 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+    8634             :                                                  [[maybe_unused]] const Controller::ControlOutput&               control_output) {
+    8635        8204 :   if (!tracker_command || !control_output.control_output) {
+    8636        1422 :     return;
+    8637             :   }
+    8638             : 
+    8639       13564 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8640             : 
+    8641       13564 :   nav_msgs::Odometry msg;
+    8642             : 
+    8643        6782 :   msg.header = tracker_command->header;
+    8644             : 
+    8645        6782 :   if (tracker_command->use_position_horizontal) {
+    8646        6782 :     msg.pose.pose.position.x = tracker_command->position.x;
+    8647        6782 :     msg.pose.pose.position.y = tracker_command->position.y;
+    8648             :   } else {
+    8649           0 :     msg.pose.pose.position.x = uav_state.pose.position.x;
+    8650           0 :     msg.pose.pose.position.y = uav_state.pose.position.y;
+    8651             :   }
+    8652             : 
+    8653        6782 :   if (tracker_command->use_position_vertical) {
+    8654        6782 :     msg.pose.pose.position.z = tracker_command->position.z;
+    8655             :   } else {
+    8656           0 :     msg.pose.pose.position.z = uav_state.pose.position.z;
+    8657             :   }
+    8658             : 
+    8659             :   // transform the velocity in the reference to the child_frame
+    8660        6782 :   if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+    8661             : 
+    8662        6782 :     msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+    8663             : 
+    8664       13564 :     geometry_msgs::Vector3Stamped velocity;
+    8665        6782 :     velocity.header = tracker_command->header;
+    8666             : 
+    8667        6782 :     if (tracker_command->use_velocity_horizontal) {
+    8668        6782 :       velocity.vector.x = tracker_command->velocity.x;
+    8669        6782 :       velocity.vector.y = tracker_command->velocity.y;
+    8670             :     }
+    8671             : 
+    8672        6782 :     if (tracker_command->use_velocity_vertical) {
+    8673        6782 :       velocity.vector.z = tracker_command->velocity.z;
+    8674             :     }
+    8675             : 
+    8676       13564 :     auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+    8677             : 
+    8678        6782 :     if (res) {
+    8679        6782 :       msg.twist.twist.linear.x = res.value().vector.x;
+    8680        6782 :       msg.twist.twist.linear.y = res.value().vector.y;
+    8681        6782 :       msg.twist.twist.linear.z = res.value().vector.z;
+    8682             :     } else {
+    8683           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+    8684             :                          msg.child_frame_id.c_str());
+    8685             :     }
+    8686             :   }
+    8687             : 
+    8688             :   // fill in the orientation or heading
+    8689        6782 :   if (control_output.desired_orientation) {
+    8690        5347 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+    8691        1435 :   } else if (tracker_command->use_heading) {
+    8692        1435 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+    8693             :   }
+    8694             : 
+    8695             :   // fill in the attitude rate
+    8696        6782 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+    8697             : 
+    8698        5347 :     auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+    8699             : 
+    8700        5347 :     msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+    8701        5347 :     msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+    8702        5347 :     msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+    8703             :   }
+    8704             : 
+    8705        6782 :   ph_control_reference_odom_.publish(msg);
+    8706             : }
+    8707             : 
+    8708             : //}
+    8709             : 
+    8710             : /* initializeControlOutput() //{ */
+    8711             : 
+    8712          19 : void ControlManager::initializeControlOutput(void) {
+    8713          19 :   Controller::ControlOutput controller_output;
+    8714             : 
+    8715          19 :   controller_output.diagnostics.total_mass       = _uav_mass_;
+    8716          19 :   controller_output.diagnostics.mass_difference  = 0.0;
+    8717          19 :   controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+    8718          19 :   controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+    8719          19 :   controller_output.diagnostics.disturbance_wx_w = 0.0;
+    8720          19 :   controller_output.diagnostics.disturbance_wy_w = 0.0;
+    8721          19 :   controller_output.diagnostics.disturbance_bx_w = 0.0;
+    8722          19 :   controller_output.diagnostics.disturbance_by_w = 0.0;
+    8723          19 :   controller_output.diagnostics.controller       = "none";
+    8724             : 
+    8725          19 :   mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+    8726          19 : }
+    8727             : 
+    8728             : //}
+    8729             : 
+    8730             : }  // namespace control_manager
+    8731             : 
+    8732             : }  // namespace mrs_uav_managers
+    8733             : 
+    8734             : #include <pluginlib/class_list_macros.h>
+    8735           7 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::control_manager::ControlManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..b180ccc2c3 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html @@ -0,0 +1,2204 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..17f8fe7a30594b51172799adb15c7027e6ef9cdb GIT binary patch literal 25909 zcmV)kK%l>gP)Nd@8HH9(u z&NIX~0vEOCI?c&Wn%z^Nwx5o67qihy2tL^(?TcrLF#fTc_h06drA z0DDFW+%bYXRBlg>=yUxvr3N&1JrR^5;K;rYm>DS6CBWf5qppkC59s>rB+{>&F&KAz zb-jJk9TzONAqHK2wy$4SpH=g0TV%+~q2 zvUA48r39SR7%#&BwGG-^gZw>bVA&73BL-XM7{znalZx2mue~qm&of?pXJEFzZj5lG zMQa!U>M_dji5K-T>Xg01q@bnJp}pPtcKi3=ynph4p02dr!oR`an(sNF&`#I%i?jbO zB9s6FJnfkQBp72K01$lT0Oc_fK=iA$Yf21+J&|85uty7c)KMy|{t-$yYBR-IT#K62=HPVn)#bYY{9eBMOtO#yG~vO`C%M59ret z)qbh95FH7>V=?7`f?f~4(^9WfhOtD1DuNSH9r%dZ*s(=c`4~2#Dj&m2&H0##P>wNg z6tGT)g=*~eJWt4xKA4ZH_$d)1K5?4IC5>^6%ZpIA?>1csPtL4zJIMCY-zoW{B(QV* zzfRzLFp=)>fHAsK(?^~dU1-|=J#m}E$!?%p8$_bKSG08PmjYzuIdP(6DI~n?H!>i+ ztFj5yKEXjOwhvB6P8q}Yf7V>bGeTh=6&)j(>maV+s}Z^<><1&pcn<#u6Vx(B=1WNR z1*rV2b%j_VW$!mm=(BAgCTOJzvgVwwcf&Xvj);n`Q%72BZ-+cjz-Bn_vUk4ZfS1!b z`03nlIBmcHPG?8#*~>9dx=qBC!ym{_IGuy8it3*mqo*4J>OAIfjP*PF6FpxS_CmjZ z)g^-?MhRGm+aZ8FEDzWt$6=YA!S*7=KfxG8qeO{agUJVWL-_W{uERlR@*FtFF+lVe zy372NpUwctv$uz8bfB(FcQ0*#rMs6l!0P>wrZLfYof97SnY3=s;KyNy3TAUJ0yfcv zNvQ2J36PEq0{RccuFJR{eA;|8~FyN#F1Y}e&CQuI0fey3V4LOZQvgii>=V8t zfD^D=!Ff6SMwx1jHBzP8(+fR-y;KL|E^fD2ZHZF|$J6LR3TJ@}bVRftDu6*8?wN=( zf&{uz3+s>q7FM=tjNu${k(Vg#Iw||w7}bE27SQ7y3dnPl2drZ(ZyzeE)eT!I=eQ7i ztQ0WVi^IzL!6FTbfxZ4m*z+Dh(-`eDGZoka0d32H^P6?<*AN>aSqI2uHn4!%*f>Bj{VKz#C&ofG9YGH< z1%D6QwHA=Oy5j)#U28FBpgf@Dydr@5x$aAWK<=%=34!aH$H0NdE(ZW@~wm(l|0-PofBhzD}$2Wj&6L|^u~B)<%=`EWmu{qVQUL*&L- zF=x==BvXN+r6~d&j#O{C_>nDG4NC1;>K*p#aBZ^D8 zT;klf^U<3}aXQYjjf=QG$sWxUku|}EMC7nj#2hj3G;v8D>;de_z3+q=dKD&Nteffr zV88TGhRc8_y70U2U0eBBAm$84cM>t$#^^?P4%aTik+fy5&*wcHraxfd5+Z&IL(Img zy0srNM0`n7k{d9(IE^Ym(X|dFMj=8fVsHzrs%)C*2-9-}aK4JPSNi+23^0X@MP|m~ zqA}*)C_m{a(gp%1l2|~p6r32b9;Do+4;a(fE$l(-e@Hq9x~YISF1{B#MzsD%3=#_> z3JIiu?C%Waj1q3S^(@2h?}1_EQ8+ zHibP5+{D9~%v4JLQ!gL~dls~r02sc9>#)f13ZQ}E!cNk#Od^IKJ0Utr`dqcXB6PE zv13F>*P=%=9s!12y3pksEsP#w!f7}m6956*>Wk|Ph8TI@<9`feJeP3c9@S$UG04Ee z$jdZZrt3J^!azCsXQhY(Y$E~^0|+^b2qE{%7$L)xtE*$Ac#KmxoC*;d@k61*Y-Xf! zG7R4)C7&E_W~5oPQ(Zj4!kMXrNt{q_egE8Kp}-RyN{4&4{)>?6Hz+BUQ|%(Qv*y=X zj0CT=-DH50EM`($BnIz6!vVXs8JOMuZ^<;mfF~x~w4rO_LXxwu5TF>;(B8G=X3qe- zGp2E|lwDa(esMYd6%Hwk+1uGrrk&F}aSOXN>z+%z#GJrxLjmCzjPV~=S`lyp`v~fT z;Z&R_|BDQuWPIcpCnW6!z>^jbFSHUd=-<$oGxh-4EHfM^HvBzhPn4LGzPVqP`D|}V zOwZ-LH*^SjH*sTN04EvnMA*a!N+QgZrwC&LtXu z2c_C-aKs)(21Y>g8{PNPUc!?m<%NuM42|hp>li}4i-2enJI02(W?FWfy$J1$!AxB@ zx^-lX2L!O3(1~4?h@FDu-YH?c;JGdUYL8Nve6v&lmh|I^wI$q$z;%JTF@d|ihcm8C zRrVJ#o-<65=!*o%!rT4fmtd)u`5O8;!%ok51ZU&S#Mwx5!grl{QWDWnC!rrNWZu(f z%NQY|Ar7_+Su_2yKo=qv11x|`CRP&Q37^A993V4?zp86dL8<`_V>}TpAmGf2=|9?^ zbPQKdp}uQa^)lC_%_q(C1P+<(L9^;XYDhUbryyx}Z#TssY1jFxMvhSHtma-<|1+IR z+F2YJ1BibE2uHc=iywT3amoNE-?f2KGDe-!Lx9>cdi7_PRAW4osgC=iXQv&b)b?{Y zm;ezlj>Q8u5fhHsP;477x`Rt-3dU4iMnN4AE5+Q6w(CpK8_9y91`=Jl&0TwgKGp*g zug+svoPpxU*wA%Sl&r5r>Y8-lNOiIAtZVG@hFxcSVW!w@&ENxQ-6pCS6BR*WM7N0* zyJMT}))?3lHsOq}Nn5H8s-IXN{ae%CFNq|a> zg6WzFjxydEFfj4MS9Gn`xN3~CX#poSMkc<80~|?=b_zvkO+%wEEb%d$KI(ax>W&yg zXw5K;?%A&Nl%D+dmHdF#7rv%-j`A?*6;aGb|L9m3)z|!C!~6w7BSzRtfNdE48*n0E z9QZ8uPQ;9THIeBWa!Lf8Jx0yLaymedQIdG_4rD%?a|Bd2gi#QWy|qr^jY@I+`J2}D zkT-W-%hxrpBRf{%jDA|E@qxwjDCY2d_H%wqcH& zgD`^WDFK2RFJmNt`Z1=xu%vtq7{efgSuLF84Dxtt5xJt6(H#l-i7?e7Ahx9R?C}^& zRjSrcfDZx_{#?S(+y9;r@KI|20BI*8;6BD|Obnnj#yH9h zaL^bHVuogQ-sNXbbDV_Ym39y6yi1$0C*9Vwf?pLg{brs|`BbCHUe@jZ>n@L~ss1D)m;14q>(Hi1M{|4>~fmbp3C81(wJDvC%t&_vssw#BCFI{sTryk)M}sXIsBp zNfEdvq1fgE7(Zlc+Gl3QKgX%5&dj>HZRBHT1~FU(*Q6iu#Ly+vJ~Mb?(vqppOqiGt zTr;`TpfEG&Y^L7Wt2@_d)oj=mBJj(*e&ra6ri$O0Fcy`R8|A;RzCuZ*1d%~eq}3@5 z%+i=#vjz~X88zS-33p9r7WyF!qZ$B?BxY82ebxe9{rUs%SRCqgN|DY7S*gfUxxv7i&+-X z@qvKZgv)^BhC>9T7jpf8`Y~pJ+9{0H8k)O66 zfNQ|sxh7K>T|Hu0qjro#4&pk7D_x(YW7B=%5URm&-(YBJEK29;k&_lc=DE2-k`GRCi zr_xu=F-YaGi{$o}9KO1_9!T{$D*Jw_GivCNH^4djCx%<_A_oke12j+_lXTCQ7{%^F z77N5kf%|qle6n^8yGKLd0AX@W!x*J0ypj~6lCoD`#w!2=rLBNXRC6(8+gIIu%n;B0 zJJ(DODeYtQi;4wo!#IoCX~$=2Trz*#i#DV}glOMfN1ay&0Fw`R&t-4@?%rYLWi`R1TlkM4%;{w9p50>inmNMP6cF%|+6 zcfjpKoH==nTItE9Vg}a8jJoCl18?hqZX)eMaYg+kEF@ow&vwZK>5u^ED-&@#`#7$U@;Ns1bkUI;+o9=c0pTo1TfNKC>d5jhJgE0!3(2@rw7l=W$Jj=ug7zYt^ z8lH)q3t5P3rpZ9+#U*H5qHtT%5a{B&W!o@!CS)-xyH4p>)r*a?*>mwI_O6x7wShgJ zFYLiM1H?zYxq$J?Z*yh9R$^3Hr**X#y@kV*FRse0Q-x}d`^aqZ~k+|{TOS^l?W(I(57Qm8i{pC$_343MSM32eH*`=qL>*k2z z_Uk3=>~XALzF*&|YSzl5)dz4+X|b_>;tq4155PH7r%OtG_VprzSG@75x>)uO)V>rw z8LIUbDjFT@wwL?aMmDX*9^K@bV*v+vj5mM7X8fT-QBV}ULa|~k@in_^L#-yX z5vIN4NPjp?xGK-Js>w>KITEl_f#;wxsR z-vV$m8a^DqxSYQ5y-~pa#f8j(@0h}-%THXA&$Sm{ezK+2l&uy}RVwPlfpuJdiXtao zem;pS|Io|Naw$aH`8ou}I15wkTDYpb^H*gb3#>fJ~QfFmAFw!a@2rjMw`fsX$4I%#6M-txWID=<10w+i7<}0Q~lp?`1 z4EdAthO3OcjRgep;Z=-!z*&v)rUHy1{$)UOu=_Q5&bQ{i!vhR4>qjAJBLSDX(kC{Z4)-{sI7mbmV$RZyz!2ju* zVc5ZqQFYeTcTM&}x~ZmZIF7N_wfigaFgVphiN$bsK7{IHE@;ACv;Tj1m2khSwe}bd zm~e2*E?}(2N1DNCJY_{MH^(UQoBd57=9tnG7jHyTU_%tMKbrVOCVWzv`tp z*J!F=8{vyHM80BX21WQbRiAH<@a47SxCmcZQS$SruqDDb#=5~X{;TTk1)pBX>BMub zlpaA4;o>KN@OdR(%yeDb7#=z8s~m#QZ(#WC9?wYI0d(;D3RwwHq%ZmyUJ5FX&7#fx z2rJT~W{;GGkSVFdo=%i+Gp)pcvW_8d@a%tb;@BmRP$=>-%9GJgj9Y<;jviu=Q{k3< zFqc;+FBqfZp}lo9*~*VrKH`~iUvl<9l1K2{ML;M96~NJ5s{yZCK!~ygcvWJ&Y610K zBUlB)9e5q^9b?4eECUj;S5nQp_xa+1ETFQn3ZU-^^ZbtFF5EUF_Q*H<31a}fGc$0d znqnpn6c%vy7{zlT?Yau+G0GlxILX)CLEK;gWMNYLX)>A$CAYMTiDN@&*qdAr!v)61 zBhEpoWZW6NKy{7u!we*Dmhq_LQYZt?Nr%gTAz#7uT*f#jl~St-dTZFP%Qy`fk&=p6 z#OHxZ%5_vM;vI<60kX*?6tJ`<%|ug(fc##lBW54L7-C8W&CGPI9pkJ7)Kjg&sGpex zI*{SxzMvNMePwCE7;BHfx$f70WD#QD< zPk4)MmK_R?7&XJ$ur#hVxAKf@=z8eGzryaoq^65PdXI7Aq>*KNR!e{%U9Nyq_rbqJ z(Z}#Rqavw(TtVys@7qgoQWo25e0z)xvBaz)CPpSkCg+{R(AZ*Be!2iCZX<^*1*Mn? z#N6pf3E-x7gfYI1m~3|VHRIL|5;eB88YgNm6*KY_Q-K(y1Q=pAH~*X049RTbiCHzo z@G9Vy#E?}Z6Z6cfNoeNfmBb9&XCzXNw8QJ6mpaYE`Tzt#wH>Z*huGoX$~_Ol&Dgl{ z$`UT^CcCG|QYU5#3DPI!E6wZq0P{LL1iMd9iQ&O?y^6i2`@-~(vJYa0fUFWOJOV239>z*h$zu#FLw zb3rCh_0U<82@rU3_oYy;qByDn@2>dt8v&IGpti8y!nNAHL@ zFKGczv|Q2Lha@{~OcO-(tEBTduoI+;~se4Xlt>jmuo&=76Pz zfXwT1t*`c7LC(JVgk2$S3KQBEUSa_ht=wjY14n-mFq6vuuBi>&h_O&&#CLL3i4iTv zOxdS87PosWIM#y^?Vqz2FoHLAeJhw4D})9xaSb^G0O zx##R!FulsXeGoN8sKQu@P3;pip@@!!>dUS}_xrdh+{^XcQU{Fjiq8kfW0V^)`vuax z5s@j3SOv($9-2%-SJ%t{hP>sBjX6fnFg_4aPxX!0x;L;#*)Z;dl*`z)V#K2(V=y)YHuLcx>v+Isj0rJlX3Ks`w*7#}(}1zAqzX1g+z{Js3ZZEM zZ5zuXM%kD+~x67@0z&F_99uN4wng1Mq*MAED!MD-%OgBy8 zl~~mE)luU@H*WqhH<>-qBkMo!Zzv>wBtZCWm;j1V3;kjxS zl=uy~7zK<%*PUa;Z39#}k*!Kc>oa5|8fH4G9U*hxTI$<9I;J7(ZJmu0KQbXJ}GDLmRE5gv$bk zn2$=T!liz`EvX7fAuEyQJf!Tmx&5Kz79e2G0JTE#@$svtZ>faH;9}OT1*3R+!hrwp z+KO(RBTFW_3NhMhjjyeQ2&(NAlb6w62)-9eMJu$$D=I(>nmns!8~$8fzI%AYeK9PB z;ni%9cyCypo_>#_8#&5In`xg30UE^_BBcmq=D)KPLC!}M1{Fzi(zC=2ktg{Sn_iJ_ z!DfyvD@L(qWtcyok4eI(7*7G8tnPZfs8m>KS9_XzNiF+z=!mP9bj2Gx2M7Xz$oIS- zz?9kK86N)#sd?e$sHTTgCSUm=m{j4X3B)FZP=z?l=a7&a|8NaiDWyk(-nq}bw+N$` z1s&kE4){Ly1o3gA;1k`u)J%%TDFp%>8O~}7Dx6JTg`!m0FltV(J4RNUFkKH08N8CC zCegofI2ORsP8VUWGd=VbbNxsr05%rLe!l^#On^h?`n^$~zJ|tBIvGYcr9@67#{GQ_ zHX%A2-jc;_JG%xy7;uQWZ4{0i#Ytvum)|Ho>XvXjvH$=e07*naRIWn}l1nra;y@Zr z1@u;BFdPP)XK>5v^jIS*q37{@4wbi24VG&z6)f)~;8g-N7pPIMh3jzLbJ9xWHJK?w zn|r#>%i&O}Z+Fp;l4izRNYsyV!#sMYl1ly_J_aESH~|(zzKpN279H%!XCHzZ(gE%q zOYVGIsc6VLz(-^R*z)2s|E-`Y3>a@ERQs1VYbFWj!QcSZ7|Hqd7RB?H7y>YY?(^Tf z8-yYvD=*s}P@9~l70uCb`ewRfi_b)d`eJb_et8lLQ&mr|aj$N1-R82gH(w&V?W1;p ztkoEgfH5XU9N^V*=6d+Tu6$c#N+>)6Bscdn_#}OW*VXwS4>%7)Zl z#>_lBRXsCx-Q223W!-F5q;Ts=NN*XS8l$M2=cuD0Mg)i1K3i4uj!ZK~&EneHyL9t= zbvUQO!>Q%im$DPebF8hL5O(hhx6FjKmVeun9=T+p*CnmjE)uHMW#U^fKHwNZVSptA zRAZ#?h(@KqhDTJF{Z7-jr@M>-7gA^w&`)9LS~EuNKJ(61cuoR-$wa(I!iT?b$5-Y)rUOu>zQDI3h2u(R>&7peHZKt%Hu<6 zb2S=B1)IxgASrAv8^+23dkyDn&=&t%eFTqh*XxA>bX_0g=-RV~?FvL5+a>(*lSd$k z=?t(wi99~DGXTP6R1cC)S-Dmp=Aq7*IG zHN(gh3wT8ItYKzCD8Ll<>afXAuk?opCnnW<%_%2TAYwD#h{KRR-E@8~fGTgsvN#~Ll9Y|+B zZ(CVNPuVEd$zln_?^=B!e1x-un;YY}0s59N*bw+lmTrg*^$;S$v24tpsXJ!`F;mFx z5m9z0%0?M(Fn3(nPKA}BuEg&kRR$n{v-kw|0a2eYvZ%Ow_?@ei5)+Bhj?v9vvNtYB zZ-ix$N2bnuacJE6Y{><9#1m4m{a2_h;z!dy1NOAZNA)uT@`&3^xp;Bk-A6j_tnqBs zictqx#rQRMA0eL0{T&{+Y>fK!78gqVNLHBdfEdGUhT6@REiJib_N~wf#IgxT!+EAM z+4}R$bPjj8ZAS=(-^XQ3534)F1rmfghMrAJ_SL87Hs3xbw^>eYZps^mGPgyZvvY3T5j_`*;@p9*SL8~RyH<};^~}J7=tt}fU((;pd()~( zfNvTbScbE}edJmXc|8b1jVS(Kft$~tMcw!$iV9Z)z zFdmv;NT$Z$h~M_|9a^e;9y#E+lhu=o++oszk4|lz*&DYcJw6EUE(Srj&deP=`+)u+ z>?u*L;>|T>4Fy+zd5q}%F(dc4*TQt-ZloF}ae z=dbh4>V^KTFR=D4^XZO8rU26_VS)inFKx!#lmT*XbYK)y{1qR~*X9Z~v z;YrFeg~Kl(m2AA8lt->Z>L9fWt`vT5Oc3FxaE6Dj28>m}zga2vXK40rA6|O$<%%R6 ztb#FSAuQJ_c=n8$$R`loA1dRVIsGQxX`&xzOFA&Q<_^w#W(t*!FNGStx4{gFx1{-P z0hJZKP++@<53uPXh995^$Psf$Wa<)&R@#kre_0b|_I0eOXUS^`7?*hP#GG-J<- zmkI{76Z7=4IgUL6V{di=n*3$u!&G|WNXg#TigDRyqzY&sG}AGbLRaSa{RGYe^T2$l11UpuXL_8d<<*w z@hB>aBLSafWo9G>Sfm3ms&8oXnOXC-yY?BH3v2S(>_n46PbBk?%M*2xFS1ffyf|XY*x%K~=5@u#-JLf!>bsar0Z^iii+PDfM-0Ts2nWR|gNAmH+KW^d49ISI zT&Y~NVH9s4I>w07V%Ct946aiZ7}J{ZVI)#4`y&@z(L3|M6x{!Jr1Yq#j^U^Ha8P9hMl~lt(vtMdUsBD_LsWBWjehpxe7_X7j~}`-T;t+;7MwE_ zP;{jE%b+4l!LD zjtZ{MGWz1shpK{YJ*sXQle*LUc*zHpE!$%(C|zkhe>3%4>@*h9sb)*h#xar?R&R4C zJdqx5u~K~kYPP&U(4!=vzIiIUyZx{y`JuW~Bg&v)w)q#3d@699j06ZX@QWBLd4`P- z{?w?>s0o!r@RQLypsH(aO_u{6-9QYNILf5j7TBXACSg=O(QCw5#@qTv&WJIdkZtd) ztNOPa$;@2C6SS1`XbS8SK2uG9%K_CG^9*c8Hbg*}^SZU3%1Sdk;Z}@*H3H+C#|Sd8 zLt(9%se-_Ey8oK3s8cK-Naq_0@(5!5kG?cO=M*w(rC%Ns{pS5zz(@!B0Xkek%n`*6 z;yV&iow-LXLK@s|Y~)p9x@&0AQ(Q<@CULvuxL3BCGmI+}Sik@ZXFgMmrjFka{>a|Z z^@C6oQtg%}{x4u$GGNM{|Lq-n(`h(6*S_f4W><>jasj06?w9Af zT71p__vnn3f>EG*)muM;;mKPFnhfBLt%B2qYjGp&5_YV#l; zPdsS@61Blhhv0PH?g4NqUAc^ry|=cH(arTa8lQ2VLfmA>QYc1fHS3<0Y1U7Mh)F%sQ#+U}Vz&xp-DO@9L$>eRDoWZJ;4pF)cGzRWY;f1rLdCv5ev%Kq_ z9odlv16Gp!GK}1T#u|kTE~GtW*#+O;HBQA9$EfT2!BMllGT>)c%(zYg9^ovWO6|$Qca@u!-S$pHAf9DqV+XG zp~DD?#tI)uiZRPqcfoH0{P-05Lp&Pc-IQR8#)zbEJnltd;;XqdVuYvkF&N|6BnvwM z`hSuvZ0e{av3o6JMCELgVDv<$MJG}$oW}p=gRWNLypRK8$^}cxBU*E~91t-b03QRW zo0*3z)gUItw*>C)8&~zwFK!!VSH&@aVo{pX7?G!92``f^ls%N03`Rvo$@k@7ljSP! z5l}y-Kbe=HHH3I#lsiP6n93t`dIy`gGt0;IN45$@nJ{#%ZY9&BHIF@vz7)WEl!GlA z{=w~d4xX$j1Dwz~foG4#WNu?mg+*-`vy}pd|L3AU2jBSck%#mKf3`#xXd^23t8>Mz z_*cX~;y{J)xJ}QamT$cA0qg@raa60>Q!1}K1FP4sZ!)&N`qkbzTDuUk)Kg%buL!Zw!e! z%pabNiV;yH=aI>22a53*{U9|+^edwuR$G|F|1iX=AS9Y z=l_^2sV*J#fc{S5shY))5&WYN+O#$&bS@EZPq8*f_2kXf_(-wa#SqmC9r&^kG28sf zZ@=;l;mK!giSG-p#XUPY=1g1q-344DgFDQBjAGv}-DW6d0tyFuF|uldDltjW`M(kIajhw1y1%YSIEXS0k&XV^QVKPt_yF* z9~rje6?j@-`fs5mmo;kweog|3@9>2Ky z9p^a$N10bCmi3_%-ra4s^xrsh2J+u(2%KdtN#l7p?n@gHu+|y$6*rB%U)OdT2)lt=YQ^hYS^=dS?CVwcqJ^{lI;Tddk{$c(3k~%9}}DZ~md}iB%@P z26z3ev;bHy9i(QDx}g!uZ!uc=I8@(fT!blx{y4WrK8`iHt|)ysdnzZWOsh7l((?R- zB$i-2iBM42&vBa-b_F$i(Bkl`Ya#aqjTsPw3jJslGXX5Dt^TgF?lVAMF_zl z3pB6_VEcoy9%(z|g{6bb@;Ij+`hsyv7p?KGv}%%9?K&89fD9SSn_Km34v08^V0{UZ1#rLKL5{kqC|;ymrx5O~bK!El<$1@ftPmZe zD~S|iqhsjtG8n_ z6eYk)y*)$M)$8!<*CireE@P4r>)OmksQk2{M*#t zDdtayvy=qb_qbC}9($O=!0UX$wqRVC=T8Vib2ebLd`}2rU46jn8dDOsRK%TQ7_cs9 zC*>14m9ArrL7{7q?vA?Fv1h!`PPzUc0B~5*yQgZk8hk10cIfUpC`wb_UGeu{vb!F# z7kYfZvjXklD>hB>UTAlor^p7o<2>C9b(fR{Q#}aK()C}`j|2TETPvJC&_8^gek>Uw z2kXa>3ABWZ>!#N)fBLZqsBZGTxl?jML-y#$?s(jvejL)s-RJ-%`NzC7Y~7h2jFqUA zpb4#~VZ6FHW;H6(L4eAx$EOWiH>5BJoccBrW7uZ^A1lu$n7Yozj4Y*twvWH0AJ5}F z*IN_s;&52y5w&NJxQl}qh>4IHUd~;OcOUU0r*!#p@I_7;(9-qayvP}{7z(BW^6h?~ zsZj7FA9s`rnqGv>jwv9B4Q+Qv9Hm}(ObIrWqp0hc^5p3H9aGBf=u6}gj+{Rh;FlQ^ zx48d;!Pp*xRh8Vn@-Y|CqA_-z-hr)_VA5kuIp+nO2KLzBc7Zk$Td9HYjuwfS1;C8= z4S-rc@<|$b#K`D0fm4 zeN3DbiH%~PNal%&v-f$#piE#+9$8|Z@W%dgh$+{9GKV#Ogg-nPXe%^Q?f&p_o8}=E zsxs}`H-($Vb=x^mIX8DkC1H)O(^?cF1RVEbEKw3Tr0yT5n!Pmr#0i}1O_?i%7-?_Kc*(}wX~G3@TVrYUe} znrmpfExxb`WsiDhMl<}FZJ79w^F?jqqop#Z7UPvGug3#wHtejJWxfGh$2fj!9>Eaz zvbYHTu6Qo231l(Wl@y+O;{h^E;4eXh!$=ae`)!p!mhJ{@l;Rlm#2jFy%t+w?f$wnw zYSxTOJ9d>qW!32adAW{FPE{?}t+g%JO6l^r%}8a#!g943NN-98>U(fJm(R8nJ6nmk z2_v%Km}bVPvag9v@o+ENJoCd%W_(ClD*7CsD>CA)d$mJ}kBAjJn|Q#{mECc<>mtK- zjk~E}WwHP`6TsmQj>8Slr;n) z;2aW{!f`i_w+mxA(8c0a2+#k*+QAc(AN$~`$o6jx7$27|5tF#iN|I41#ClvURkW1p zt~S5f60E53#}ac?2RtnI(qN^}NWMUt&Ch>RRW}wPLv;(rI59aZaEKO_-?ob6@5^J? z6eA~Hm!<8bDvtXQyj)x}c)9cqytXl=_cxe2?38LT2U10gu_j;`Vli9^2!*Ik1H)CL zY>*zmFL8U3S$YQZ@c|4U`s}FtG6ItP{lq$oQ4MIQf)F;Eg()iOZ?P8Awc=GWg|GRo zsME;29Qdj#N$mgIZgahW6`DL*Ea&{|*LBe=MIZ*a&1 z(lFYMQvqzi_{We)v+Gjax7aV0-4G#}cGxSHB~-xe21 z9Oe47xJZD%j^nu`XqS!WiEDYtROhtk!>vL&I&rG^g#GISkPfEsX&YF2JEZ$6S% zgvYVXyj-iMz1bc$CTk7%RBUfR?WR zqTmh+-mQmnI0^nz!HumQ<4rcpC`CI7=MC0p#8#?i-6r>wlqA}NOdVJ)_zv-gRI^8 zM3-Gm3=w_-0je$_6l2u2;|l4 zk_yVts-$90S`9Fn6Vfk0K;6tJ#;9vYE2;Gvim+vr|CCgv=ff?j{>0&*C6$(STInqb zA{9$&HS^I8@~5QuhAk<{j`}I7YR_L#XIOQmv3!Y=3c6=tq)Xz$pzxT{(A9MLWB zzQ2CsW&B_D&b;Q}v)h$lwOswyJ`&^b<@yA#<8mwZw_MAB-?m)Ggu2Mf6<`1SxtQU> ziC;DePmWK}F9o!8{U;@SjF=yL$gN8_aQJ?8aV-8NKcFYlA(#8tm=YD^G0E9gDrNa4 z>@g>uZs^$*aYt*q(@ZSldH=%8rksyxG!Ljx3xGuNwt9w(E|>TZ#&tcqrVBHoH9oQw zoU4E9YN&Qq+2~;7rzbpd2JeOV>P_~8s!Z%E3nAKV-&03TGRNr1Ud zTMq=(Q_ad5{BrRaVLuoU$XNso*D20hnF|^MROCw4b+~{&_C&B8&gosVF8i+BJ+Bgq0cZ56 zTmg_g2S(l+QWCSXhkF>14BEJ^s}E!}Jd#Cr%D-;oO41W%D{#2&p&ByH1&04`Y+Ut* z)F!w;8y9|l{A^sE0+{FUQjSn(+)~c^{+Fv%cN@81h{~> z-nJBC&%Og5u(WA;KttE!&INn;?Of&d?bXQO)cf)%n%L| zv%kD)9%+YzUGukTD!Z;w{v~4Or3`pL&%qPg>G~W|U0`d)=-FEe{`^JHmRpR8!w=kN z4Fu1{s2BlI&u~0v4tW)D1Nql;9U~lK86zZmelZ{pPMraufsePqK^l;Y0Sy=7koEju4}^F zZ`wYpfU9sP|Jy#wfM2(L4B0|75krel%GWadw|%hdb#K4zT|bQeFW7DlwYJI1P-(@ore>+04DV_XhW@_w<%aur$_cD%=CxuD!qs}aXa&u6X~fyK>r;!ZAsy#8dArJx)r4wtl)TY`&IRST`` z{-Mite4V%}H-`$qD^&v$V6Fz{0|E6^v*H84+~mj=vf#vLS)ZX4;U)x34Gms=JYZdX z4Ao5zD*q<&rbW!04nLzm*(FXdzkzW`^v?-#GvPAOJ~3K~z7o_yGIxlHP)Y+wK0(#dRI_ zkL|-d*Y%e?od_I-V`3HHeDKbu<=GT)I`a84Fqk67KXjCm3w_}c;iFVINwHq8FR;zj z%Mf^F$y{bsE60ecZ515d$(#vdwm$t2PjrC&TjtD{R1ZB*K{-db&GS$H58$i02`O9c<&UiQ}>?=3l?Gg2Pm_r6_7LmFsp3}^nUNU zrmOn5u4`lLUDw&8i~``Aso-}nSLXzqyv6s=)Z4$xBWv7!U*GSBi?TsGIzkZvt(7AGp>mW~@ zJ0xr$&- z;yCw%um`W{gL!wY`XB7>hcF_^lilZj;3oUMNBd)Tq!9ji+1G5UGI(w&m^I!GM>9x$ zGH9;xWP!OOb&&L8mahPAUG$x~qUE(9z^v~?(24-H@@QbVsC@QCv!pRbpb72>RjXf zynV>4x&b4z{$2rS8l#M=593jMWM7Us2GBG{4If{yX^Lai5|d$MkssnL7;V~%M>7)t zU~%IFB)q2y@KKGC+$MILuHbgB>43%Y@qi1lwCE~f5^0w%c;k!`F8Uu(NQ@d#-*vWg z(Py`<7`gKv@2AjV+*0CU6YCxV2rdia4f^((0Is5i zv;%4snqkfSU)gg^e31eF=$5@4?I8&B+Wb_T~-t``%@21d2?eBLg z?r*33517J0cIx&VKcewHP~W2W!8+FrC6t+nXiPkg zH7^j0z@D0x3dB6ca~=j%BsN&bNYg^3uItU?br%&=y0nD1macF^B)VW4K3C@YN-_q_ zV~lS$iIX0FLl|9I9t(g^XRoHQXAeP`SD`bA;~WD2O(Vv7s2vOVU|e!6Ab0dL1L}!k zf@Yd8o9H0GhjW2-P1CNkVFic>SC-yZDOn%cc{sZxLr81 zA9AQTC^;p@G=xZ&bF_~l)f)52xMqND9qnvqC_y{mLHd?lG{!@PH;4y;d-Rc6Cq?YLWtnrSWv{eh3W7KyI!s3d6Y11zI zn(;{m*mOXwj90#j^<`6iV^yh=JCWhu$F@Kja!z0Gh!EylI*6AACjq|e7T}PQ4nS;! z?5yAmCuT~+{u5p~8TWt)^TIn{3BK{Y4^Z(r0}N-bdWz`qGZ$YLoK*du8(xA2C~p{5 z0Q{1dpa;ttQ^5Pi2}Z+1HFy4b^&X6PF3W&GXz0{_MXmVs@?N;4le?ZbX5VJAPHtFb z_=4^A_770E81(E>FD^BE4lM{WXnnEO@fkhdq{GV(gSaD?LNy<#!z-^HfaN-el_CJ@ z*%L({G!D>=5ssBX+k|lj^j5mQi7&Qv5kscN{=kZaZKi-N7-yc>MvT2WJY!Jvt2^SR zbW$o@gYj|NV_|*h<0Tdtk4ny7X#6AIO}VE9$gr$qW;o5G2E1=d>c}Y!GZ=l%@0mj8 zd7egwr)_K$#=Gx~9`*5WIq9#B2kZ$Gh1RrXNV>Jz)R>plO;Z!zH+oRpe8rLM31{IXPwvxDaPf9@>s zYOl9?fVi&7Km68pxs5kn!Md(Io-Eo5nQf--x_4a@h4x$5<(AIJqg?FVy5^1_|I>BN zVdMe3#@J;)s=FR9kLpG@q<;Aor#jz|c!>^=#mDEtP1Km4t9RW=8|Z9(3O@V&{JWEX z{Z@f-xK|-iy%9X7m*?du zhc94HVc}HR6HOP9J$K?j`!e=SWeKWDPoTZT9wb3u${s<2)QhVUvlp|ItZ~hSl+WYt zGzoj;3O-fo@uE?b9=N)`QF=l&R0Dhb!l}lvhp^GFke)!b5O77NP`nyJQ&`6aF)9GH zLh%JTRlpb?b&O-ZGfW6ZV_;gkE^wc2ATVwF19Xmz(Oae|ig!M6jQC$}vvkKUbd4@J zzQq;q!>LALyk}CuugNAm{J`^!$1-EUD3JW^=++#dQTK#jECBvV-80OC^kmX6R}dw= zJ{#U_0uQ#poT(L+p)p}3qYEWbaL%SJJ2eD7?5om z-~)YMF@@zB#FcCcEUHXlT8N)Jg(#2x^QX{{u-^4|B-%hfcP!rC9&HlM!ho`W$kI*j zjr`PR#Fb31XmZst9_Ezh4cBB+9u@4KHW09ApU4P#Z)Q`?G35C7Nn@n*7|wt1&Ja@w z_}4^-{dA8EsBS;qQ(@0H=$^m3t_LjbBcj8;VOkbf2wtXNI1+)N>stJNQT?_Kd$`>lF&>)cpy?XrU;}#6fmXeQUpQ zbIsjmUH`YcE9YTHlIhz?Q)$PGna=BScu84Q?mvZUNXz zvBbyt?n<%DEPkhcL@yj538Y0*GU_YTK1KNe6EE7lR$8uTDXS|=mjrpy;ru26> z&R-DykPOY41hD9A3g_KFgBZ>9h~c?4mox?0Caci3FS_3$iUUKFdVn0QWDs z2%uiT*!qYm9CF(*>drF{rE~8kCrv3J|InHR>3%KL59wZoj7)}UJY*H_?*jUd{9C~^i-0wYs~Ir1K4LVcnN=w2rdh+iD|d1uDn+B18}2k&CL?0L!|Mv1 zt|8S2l+0|3yaJJ1Bci3HgFSV_pn%iTw7=JnyT}xL$Qad$+F3f6k3Rn>y@G+p_=I5Thd9O& zUB~D8I#yxvMQy6?nYI$arHg<(bf|FyR4_gaxUt?FmY4^OT5Q8BDc~J&@TPV>rOz1{>G4Fe}X%i!Uwr zjxjj}doco0Zyuvttnmq!;L*wJKoVl$GHdLbWlz*SjTouwVmD&(;1g9)Dz>2p)J0yD zFg61o7n+WzCchh2HfCnZR~jTobd#HMzJpJGg0_$xiLMDI2Hn?T5& zdkJ3!kAnjQ52j53?K$7xwNy)YkANFyJG`1+F!s0?J;P5$*1+K&P0!9QuR;)tBr$?- z<3ShKDhqBtM$lTvnC{`kw_wQ_i&vGxUG9>fK4h6q?SQ#HkqtW*p<>KyO0{^X(9mMO zPMu_rFf&G32?T~~M%f}gQ9TxK&LAIU=~43Won_0yp)5F(&up-C*siJfhJG>JjniRM zNXCHCqxpE$d@ji>toU4-JxHraS@X^XMwJ7O_CIQy((%zXg{$h+&?Y>ETty68nR?+QhSivv6=-?bh*_a$kpG3aJY zc0-oqz03L2?@jwHmygDIf-=0tDde!>oznxV8SYHD2_9EjVrcZk>Pyb%W7Kwywcc7t z)HV<^T|hfe@wASEz!@hO>k6T&IkV(~>>u|q$P|*5!-s5o#z!Fozejn9n_|w)w(!L^ z6yOBZb-gbIZryC!pUg~VS@YiOgVbIf?U$%M`p7a`LcL{ghZL$Wlp>m8lGmQ&Gl$fe zax&29*AV(&5hX)ze6Aj)eq}Fo!oFxK6$CuIo|Ke`WzF8NWU=zklWlqBw$y-b} zC$cjcHU=#8&0@TT*EIydTdBk%0Gx}j{xwlO%R*+#8&->%ZmM6C(3$?2{X;l=#k6mD z95gug#vzXTc(9z1#Q^tfn%5+4nm9jXi8z-S&tx@*o4a<$AQx4U&u7_J(06~mawrL)`;z07wd>NwnQL}uDs@^>mz>cPm)-j2Vq$U(sgeuy?UANt zEv*{o+gszgH)s6n{<)bw3oDEd)3x*)qeG9X)G+9&sI6fUltY5QSBFQ(OhWJ~9lrmE zHxS#2J>6p#W*p_5irQ~|Lco+Tw@!8O^1YxFgTb@5aVKP zyqKNA&Dn*?3j9nl^Y)v2&CDWZ?@SR@(Cjv(v@sXiLsU{C-tS|8k28E@k@cir!&K=t-oy6*5(Xe$B%=l*<^ocsIR1?pP> zSm=)j+`nurQJ17F^E+E!zR;<&%*I{9=d6^~U$Swps_Ta~hL=s}M)s^J)q8!W$Bvy5 zH~zNS_6Up@cE7b&Via|*Cj}-&y8jYABP@?pxyS`O+7^_gw8((Rl?HGrQ@fHIw zij`tX*J)+nZt$+yNX(`oj$rtp8}Z?{E+@MF{pP}H3f20zS}9@@2LwF8h(i`TFgFOp zzoYDJ$Uho|5iurvmM>MpZj6%>&r&CLQDUr)AZv(Dw4K>coER zF{MzuoH<^k>^m*qt@7atz9#l^jN_|epQ|ncS}~T%X~S6JcDhl5?n0CE3ei!o8Ww~-LDLw)O|}VSD=rs?*=}CK}TFY89C3W@PwfGjW>EgrUJc_vi3&k zJuHHP+WV*XerhktG=uV|_G%b^YERFepV}iBf!Z5WVGWBQtM*n`66~k;f=n|g4^Vsl zZdkakOYNu4YC`eTW+U36PC)vF z&8|STnmr&%bIZ}Z{cH1W_UZn;81LcDBKKa=4OZZr+bW!v7LBdQ7#ih(H^hA1GMA^j z^Uslb3rhHQ+p88S>y{b2E)dZ~j4Cz06{BMlmv&A8%av?cj3n*6e~h$tfQ>yFMy7H= z!x$BFJ-7ga+QxmvFxx##)YWx&KZo<${sG#!aIe`x8q1)JrsV`^al7OKLe&5Lll!+d zH$DVfj@lLSLLhlviEqol9%K^>RZ`rZG{cG)Isc-D3UBWHQ{twbDAp_ z*y?#R1=M4_X-)v``?YK}QvtU#SLI?Um!NJ2m69onkWU}uzxVTuR#SYT3?RuO&6ebw*EL>+12BH%DhwwipW8fo6<*0^*iZE#6?Y{CfYR;kN75QM^(y5% zJaGrB6!Ao5FMG>wus}rHPs7qmY;%#M)tbr>G|FQ{_7nw^xvAhG|KeUaQgpNH3^Ch< znd<1l0_?X%_y%NUKm*2wTp@L1Tr|=1HVKCV6mb{#!r)xp{y1PSq}N>iRKIjD3^KWR z_9(~rf8Gm6K46v~n|Xup6~=JCdv>@n+^7J+r3;kwHy=r{&oxc>9h9AP@0=;O*Du+I zaTqep<>*W27`L%A&MWVa#?owD3aOlP!NwKJk^1|&=nwPnrf9m>N{_k;F1y`0->zS7 z{u;}Ev@chlH60*BJ>|uu}O_zbc_8hUL#KPHA`-tLq6N{@s)wjKSgR_0DX4 zor5n?+cCzdE8>aslHUC~x9Ez0W8B5_Hj^jGZg}n+%LmUGWd_I`N4wb5T0VFVBXnC$ z_?ayYcDKA7LKnRs02vRa}^ZONb=y>*Gwvwz((g;!B}aA=SuJ(&Q#TzZl}hObgB zQ2lc0iO0QnN>623MfMcg#{-TE7=UAx_hP+G@kOV&f#w9Zogwnao?(wdRV=)-|(A=@|4IyR-I?lT@yHnLA10(|pcZ zx&vwsa!S!T8Q^rJF!4P;#?{~ICu&$g;1!OB@d0Vo>ZUwDep@EVsOxyFbhtyD`nLYx?>sK(GuxAoI(^-xz++f z0B)ZVPQbV|`be|owMMo7hkc~rmV&xT5Acy@7-KAI3}2hkh9t~)sxV+`&^Iu#4$=fT zc?C6v!a(N<{;ppH;d|%HD|FrI8J=@Ksu`~LXlen+dxn3G^J3#2=i5gLc(CJ~mx~OL zoE93`;}mJX`yu@5^G~=R!f$_#`yrDUMk)-;wbSm&F{-;3#k&MJqjf+54-TK_7OI)* zyUN7}0RQ}n{rBcoNmR~ej!xZjk*sWd}WcQ51wFN3JhjA!mx7CI)?NxaNK%m* z@1ScYxF>zn6LDU6zfdI5`-v%P;Jjkym7}{{*)_Mh)QTypIFyaG<~0xQ8edxsb9#afIp0nFn+Bp~YYqg9-wm z8DrWM1VAN5kcc>;1jKN_Z%IVV8M%4&DAfx)BCXZ$c&9(SO!adK*PBMFQ9TLox?!nSY&6b! zg(o@Ib6P0iL(A}0QAP(i*vcpmaj>b|fiE_YT1=Qk+j5QTGkoF!|DO*r#3T2hbV>e> z;D$fV^LKe??ynEqA#Y@a>-#BsNSZ3EgjX_(ZjGzL^{|goO-!*ij<=6*cS}`ZKZl7a z`)8IUC8Ms&YkQm11y;OBLI-a|eb<^=1pG;pj5%B)YfG0Ok{SO2m276fkF0BD15Uuo zv*UsjkkpdsaRSPvdDX;p`%>~~9DUc(33!2*_f!>uu0<#J*8ZL^Mc*~3ieCg&W2_8v zGs#z*MXKt0Yhx|QIx)^EWjq2XiTQZ*`t*JUP~8k+*JHN0$CoI8irhY9JcVhD8o-Zt zrbf{LF*ujqK&EeiAI|^)1fNMnK~#50-05S^1nAaiy0YRJ@)h(;!NsqIDt6&E%*VWi zbh<$HIqsKf&!m>_5sCtMN2d@81(s{%J1XdK2Ag~Qs9d zW$ap6woH$)Tp&BEc%i_EN>0ed!$yoT;=kD{K>rg++|GhSe}ld=?FL4$HA~ms{&1$a zfs&6*nACjFmcvPTvNu&~;JDZonl6eNa@yLme2K{e?7Va#j_jVN)YgYBZcizXQlB92 z<7u@>+3~ZsnGPeFnY3ow=UPaCq>)mrgmdlW3gf7Rq7)_JO3E$X7=BjG`p0|2c8avP zbFV+o9FYVK1HF zBmeO(q$&QzQB~eNYFNZe2hT;Aa244xjy9ARDRxBr! z?pK=e0&s-jo?vAxw{#0rSXe9euHzJr$EXyF;ygEWy~N%{vJ5kG;6t!V_RuO9^p_gh zGh}gn*Iww&>Fyc_Xx}5T1*vU;KtLKFivYQ24SB$ws~n)$HV`AtN<)waq2Q?Ls>=m1 z!Vz|1WGH1evD{Ia$7yU-NW)nN1t*Q5*?F)G=kZY&>}?Symq=E4^FIChLsNiEBm zT_GoXGx!*cX-fE6S$&CP9oZ&~I$~^mEKcEim>*ZG!^vFp3hm#w7C##dJg-KxWH6&~6#N_?CT27FC%RSx4 + + + + + + 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:1701371245.8 %
Date:2023-11-30 22:31:42Functions:6312351.2 %
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.cpp +
51.2%51.2%
+ 		51.2 %22 / 4341.7 %5 / 12
<unnamed>51.2 %22 / 4341.7 %5 / 12
control_manager.cpp +
45.8%45.8%
+ 		45.8 %1679 / 366952.3 %58 / 111
<unnamed>45.8 %1679 / 366952.3 %58 / 111
+
+
+ + + + +
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..e5565f0285 --- /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:1701371245.8 %
Date:2023-11-30 22:31:42Functions:6312351.2 %
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 +
45.8%45.8%
+ 		45.8 %1679 / 366952.3 %58 / 111
<unnamed>45.8 %1679 / 366952.3 %58 / 111
output_publisher.cpp +
51.2%51.2%
+ 		51.2 %22 / 4341.7 %5 / 12
<unnamed>51.2 %22 / 4341.7 %5 / 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..1de561375e --- /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:1701371245.8 %
Date:2023-11-30 22:31:42Functions:6312351.2 %
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 +
45.8%45.8%
+ 		45.8 %1679 / 366952.3 %58 / 111
<unnamed>45.8 %1679 / 366952.3 %58 / 111
output_publisher.cpp +
51.2%51.2%
+ 		51.2 %22 / 4341.7 %5 / 12
<unnamed>51.2 %22 / 4341.7 %5 / 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..705624d9b8 --- /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:1701371245.8 %
Date:2023-11-30 22:31:42Functions:6312351.2 %
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.cpp +
51.2%51.2%
+ 		51.2 %22 / 4341.7 %5 / 12
control_manager.cpp +
45.8%45.8%
+ 		45.8 %1679 / 366952.3 %58 / 111
+
+
+ + + + +
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..8aef82f655 --- /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:1701371245.8 %
Date:2023-11-30 22:31:42Functions:6312351.2 %
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 +
45.8%45.8%
+ 		45.8 %1679 / 366952.3 %58 / 111
output_publisher.cpp +
51.2%51.2%
+ 		51.2 %22 / 4341.7 %5 / 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..3a93f90f8f --- /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:1701371245.8 %
Date:2023-11-30 22:31:42Functions:6312351.2 %
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 +
45.8%45.8%
+ 		45.8 %1679 / 366952.3 %58 / 111
output_publisher.cpp +
51.2%51.2%
+ 		51.2 %22 / 4341.7 %5 / 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..3177cc5e76 --- /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:224351.2 %
Date:2023-11-30 22:31:42Functions:51241.7 %
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&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)7
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()7
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)1435
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)5868
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&)7303
+
+
+ + + +
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..4fff39dae2 --- /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:224351.2 %
Date:2023-11-30 22:31:42Functions:51241.7 %
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&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)1435
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)5868
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)0
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&)7303
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)7
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()7
+
+
+ + + +
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..5f62ec4189 --- /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:224351.2 %
Date:2023-11-30 22:31:42Functions:51241.7 %
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           7 : OutputPublisher::OutputPublisher() {
+      10           7 : }
+      11             : 
+      12           7 : OutputPublisher::OutputPublisher(ros::NodeHandle& nh) {
+      13             : 
+      14           7 :   ph_hw_api_actuator_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>(nh, "hw_api_actuator_cmd_out", 1);
+      15           7 :   ph_hw_api_control_group_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>(nh, "hw_api_control_group_cmd_out", 1);
+      16           7 :   ph_hw_api_attitude_rate_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>(nh, "hw_api_attitude_rate_cmd_out", 1);
+      17           7 :   ph_hw_api_attitude_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>(nh, "hw_api_attitude_cmd_out", 1);
+      18           7 :   ph_hw_api_acceleration_hdg_rate_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd>(nh, "hw_api_acceleration_hdg_rate_cmd_out", 1);
+      19           7 :   ph_hw_api_acceleration_hdg_cmd_      = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>(nh, "hw_api_acceleration_hdg_cmd_out", 1);
+      20           7 :   ph_hw_api_velocity_hdg_rate_cmd_     = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>(nh, "hw_api_velocity_hdg_rate_cmd_out", 1);
+      21           7 :   ph_hw_api_velocity_hdg_cmd_          = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>(nh, "hw_api_velocity_hdg_cmd_out", 1);
+      22           7 :   ph_hw_api_position_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>(nh, "hw_api_position_cmd_out", 1);
+      23           7 : }
+      24             : 
+      25        7303 : void OutputPublisher::publish(const Controller::HwApiOutputVariant& control_output) {
+      26             : 
+      27        7303 :   std::visit(OutputPublisher::PublisherVisitor(this), control_output);
+      28        7303 : }
+      29             : 
+      30             : // | ------------------------- private ------------------------ |
+      31             : 
+      32           0 : void OutputPublisher::publish(const mrs_msgs::HwApiActuatorCmd& msg) {
+      33           0 :   ph_hw_api_actuator_cmd_.publish(msg);
+      34           0 : }
+      35             : 
+      36           0 : void OutputPublisher::publish(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      37           0 :   ph_hw_api_control_group_cmd_.publish(msg);
+      38           0 : }
+      39             : 
+      40        5868 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      41        5868 :   ph_hw_api_attitude_rate_cmd_.publish(msg);
+      42        5868 : }
+      43             : 
+      44        1435 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      45        1435 :   ph_hw_api_attitude_cmd_.publish(msg);
+      46        1435 : }
+      47             : 
+      48           0 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      49           0 :   ph_hw_api_acceleration_hdg_rate_cmd_.publish(msg);
+      50           0 : }
+      51             : 
+      52           0 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      53           0 :   ph_hw_api_acceleration_hdg_cmd_.publish(msg);
+      54           0 : }
+      55             : 
+      56           0 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      57           0 :   ph_hw_api_velocity_hdg_rate_cmd_.publish(msg);
+      58           0 : }
+      59             : 
+      60           0 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      61           0 :   ph_hw_api_velocity_hdg_cmd_.publish(msg);
+      62           0 : }
+      63             : 
+      64           0 : void OutputPublisher::publish(const mrs_msgs::HwApiPositionCmd& msg) {
+      65           0 :   ph_hw_api_position_cmd_.publish(msg);
+      66           0 : }
+      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..2aec979314a484bbbde5169de7cbd25971fea47a GIT binary patch literal 346 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1=!VDx=Zs3yuQW60^A+G=b|6c_J%iqVwzWUF= zunH&+rp|e9UJ7J$7I;J!GcfQS0b$0e+I-SL!55w`jv*eMXTxp_HY*6U@GW3{@gRrM zs^MRBXVRIL-OlG&b zZF)ab&hA;KPb{-kOx}KtJISv%>T=cni~XvJSznds&Sp#ywwKj{*J6;OXk;vd$@?2>=f#j+6iZ 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..cb936467ec --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33052862.5 %
Date:2023-11-30 22:31:42Functions:182475.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()0
mrs_uav_managers::estimation_manager::EstimationManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::estimation_manager::EstimationManager::switchToHealthyEstimator()0
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)7
mrs_uav_managers::estimation_manager::EstimationManager::onInit()7
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()7
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)13
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)24
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const24
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const48
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const195
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const932
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)970
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)9730
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)9730
mrs_uav_managers::estimation_manager::StateMachine::isInTheAir() const9730
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const10699
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const20404
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const82416
+
+
+ + + +
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..bded3db731 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33052862.5 %
Date:2023-11-30 22:31:42Functions:182475.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)24
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()0
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)9730
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&)9730
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)7
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)970
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> >&)13
mrs_uav_managers::estimation_manager::EstimationManager::onInit()7
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()7
mrs_uav_managers::estimation_manager::StateMachine::isInTheAir() const9730
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const24
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const20404
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const48
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const10699
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const932
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const82416
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const195
+
+
+ + + +
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..350a9a20ee --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html @@ -0,0 +1,1313 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33052862.5 %
Date:2023-11-30 22:31:42Functions:182475.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*//{ includes */
+       2             : #include <ros/ros.h>
+       3             : #include <nodelet/nodelet.h>
+       4             : 
+       5             : #include <pluginlib/class_loader.h>
+       6             : #include <nodelet/loader.h>
+       7             : 
+       8             : #include <geometry_msgs/QuaternionStamped.h>
+       9             : #include <geometry_msgs/Vector3Stamped.h>
+      10             : 
+      11             : #include <nav_msgs/Odometry.h>
+      12             : 
+      13             : #include <std_srvs/Trigger.h>
+      14             : #include <std_srvs/SetBool.h>
+      15             : 
+      16             : #include <mrs_msgs/UavState.h>
+      17             : #include <mrs_msgs/Float64Stamped.h>
+      18             : #include <mrs_msgs/String.h>
+      19             : #include <mrs_msgs/EstimationDiagnostics.h>
+      20             : #include <mrs_msgs/HwApiCapabilities.h>
+      21             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      22             : 
+      23             : #include <mrs_lib/param_loader.h>
+      24             : #include <mrs_lib/publisher_handler.h>
+      25             : #include <mrs_lib/service_client_handler.h>
+      26             : #include <mrs_lib/transformer.h>
+      27             : #include <mrs_lib/subscribe_handler.h>
+      28             : #include <mrs_lib/gps_conversions.h>
+      29             : #include <mrs_lib/scope_timer.h>
+      30             : 
+      31             : 
+      32             : #include <mrs_uav_managers/state_estimator.h>
+      33             : #include <mrs_uav_managers/agl_estimator.h>
+      34             : #include <mrs_uav_managers/estimation_manager/support.h>
+      35             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      36             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      37             : /*//}*/
+      38             : 
+      39             : namespace mrs_uav_managers
+      40             : {
+      41             : 
+      42             : namespace estimation_manager
+      43             : {
+      44             : 
+      45             : // --------------------------------------------------------------
+      46             : // |                           classes                          |
+      47             : // --------------------------------------------------------------
+      48             : 
+      49             : /*//{ class StateMachine */
+      50             : class StateMachine {
+      51             : 
+      52             :   /*//{ states */
+      53             : public:
+      54             :   typedef enum
+      55             :   {
+      56             : 
+      57             :     UNINITIALIZED_STATE,
+      58             :     INITIALIZED_STATE,
+      59             :     READY_FOR_TAKEOFF_STATE,
+      60             :     TAKING_OFF_STATE,
+      61             :     FLYING_STATE,
+      62             :     HOVER_STATE,
+      63             :     LANDING_STATE,
+      64             :     LANDED_STATE,
+      65             :     ESTIMATOR_SWITCHING_STATE,
+      66             :     DUMMY_STATE,
+      67             :     EMERGENCY_STATE,
+      68             :     FAILSAFE_STATE,
+      69             :     ERROR_STATE
+      70             : 
+      71             :   } SMState_t;
+      72             : 
+      73             :   /*//}*/
+      74             : 
+      75             : public:
+      76          98 :   StateMachine(const std::string& nodelet_name) : nodelet_name_(nodelet_name) {
+      77           7 :   }
+      78             : 
+      79       82416 :   bool isInState(const SMState_t& state) const {
+      80       82416 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) == state;
+      81             :   }
+      82             : 
+      83       20404 :   bool isInitialized() const {
+      84       20404 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) != UNINITIALIZED_STATE;
+      85             :   }
+      86             : 
+      87       10699 :   bool isInPublishableState() const {
+      88       10699 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      89        5152 :     return current_state == READY_FOR_TAKEOFF_STATE || current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE ||
+      90       15852 :            current_state == LANDING_STATE || current_state == DUMMY_STATE || current_state == FAILSAFE_STATE;
+      91             :   }
+      92             : 
+      93        9730 :   bool isInTheAir() const {
+      94        9730 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      95        9730 :     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         932 :   std::string getCurrentStateString() const {
+     103         932 :     return mrs_lib::get_mutexed(mtx_state_, sm_state_names_[current_state_]);
+     104             :   }
+     105             : 
+     106          48 :   std::string getStateAsString(const SMState_t& state) const {
+     107          48 :     return sm_state_names_[state];
+     108             :   }
+     109             : 
+     110             :   /*//{ changeState() */
+     111          24 :   bool changeState(const SMState_t& target_state) {
+     112             : 
+     113          24 :     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          24 :     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           7 :       case INITIALIZED_STATE: {
+     129           7 :         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           7 :         break;
+     135             :       }
+     136             : 
+     137           7 :       case READY_FOR_TAKEOFF_STATE: {
+     138           7 :         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           7 :         break;
+     145             :       }
+     146             : 
+     147           5 :       case TAKING_OFF_STATE: {
+     148           5 :         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           5 :         break;
+     154             :       }
+     155             : 
+     156           5 :       case FLYING_STATE: {
+     157           5 :         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           5 :         break;
+     164             :       }
+     165             : 
+     166           0 :       case HOVER_STATE: {
+     167           0 :         if (current_state_ != FLYING_STATE) {
+     168           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(HOVER_STATE).c_str(),
+     169             :                              getStateAsString(FLYING_STATE).c_str());
+     170           0 :           return false;
+     171             :         }
+     172           0 :         break;
+     173             :       }
+     174             : 
+     175           0 :       case ESTIMATOR_SWITCHING_STATE: {
+     176           0 :         if (current_state_ != FLYING_STATE && current_state_ != HOVER_STATE) {
+     177           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s", getPrintName().c_str(),
+     178             :                              getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str(), getStateAsString(FLYING_STATE).c_str(),
+     179             :                              getStateAsString(HOVER_STATE).c_str());
+     180           0 :           return false;
+     181             :         }
+     182           0 :         pre_switch_state_ = current_state_;
+     183           0 :         break;
+     184             :       }
+     185             : 
+     186           0 :       case LANDING_STATE: {
+     187           0 :         if (current_state_ != FLYING_STATE && current_state_ != HOVER_STATE) {
+     188           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s", getPrintName().c_str(), getStateAsString(LANDING_STATE).c_str(),
+     189             :                              getStateAsString(FLYING_STATE).c_str(), getStateAsString(HOVER_STATE).c_str());
+     190           0 :           return false;
+     191             :         }
+     192           0 :         break;
+     193             :       }
+     194             : 
+     195           0 :       case LANDED_STATE: {
+     196           0 :         if (current_state_ != LANDING_STATE) {
+     197           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(LANDED_STATE).c_str(),
+     198             :                              getStateAsString(LANDING_STATE).c_str());
+     199           0 :           return false;
+     200             :         }
+     201           0 :         break;
+     202             :       }
+     203             : 
+     204           0 :       case DUMMY_STATE: {
+     205           0 :         if (current_state_ != INITIALIZED_STATE) {
+     206           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(DUMMY_STATE).c_str(),
+     207             :                              getStateAsString(INITIALIZED_STATE).c_str());
+     208           0 :           return false;
+     209             :         }
+     210           0 :         break;
+     211             :       }
+     212           0 :       case EMERGENCY_STATE: {
+     213           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(EMERGENCY_STATE).c_str());
+     214           0 :         break;
+     215             :       }
+     216             : 
+     217           0 :       case ERROR_STATE: {
+     218           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(ERROR_STATE).c_str());
+     219           0 :         break;
+     220             :       }
+     221             : 
+     222           0 :       case FAILSAFE_STATE: {
+     223           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(FAILSAFE_STATE).c_str());
+     224           0 :         break;
+     225             :       }
+     226             : 
+     227           0 :       default: {
+     228           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: rejected transition to unknown state id %d", getPrintName().c_str(), target_state);
+     229           0 :         return false;
+     230             :         break;
+     231             :       }
+     232             :     }
+     233             : 
+     234          24 :     std::scoped_lock lock(mtx_state_);
+     235             :     {
+     236          24 :       previous_state_ = current_state_;
+     237          24 :       current_state_  = target_state;
+     238             :     }
+     239             : 
+     240          24 :     ROS_INFO("[%s]: successfully changed states %s -> %s", getPrintName().c_str(), getStateAsString(previous_state_).c_str(),
+     241             :              getStateAsString(current_state_).c_str());
+     242             : 
+     243          24 :     return true;
+     244             :   }
+     245             :   /*//}*/
+     246             : 
+     247             :   /*//{ changeToPreSwitchState() */
+     248           0 :   void changeToPreSwitchState() {
+     249           0 :     changeState(pre_switch_state_);
+     250           0 :   }
+     251             :   /*//}*/
+     252             : 
+     253             : private:
+     254             :   const std::string name_ = "StateMachine";
+     255             :   const std::string nodelet_name_;
+     256             : 
+     257             :   SMState_t current_state_    = UNINITIALIZED_STATE;
+     258             :   SMState_t previous_state_   = UNINITIALIZED_STATE;
+     259             :   SMState_t pre_switch_state_ = UNINITIALIZED_STATE;
+     260             : 
+     261             :   mutable std::mutex mtx_state_;
+     262             : 
+     263             :   std::string getName() const {
+     264             :     return name_;
+     265             :   }
+     266             : 
+     267          24 :   std::string getPrintName() const {
+     268          48 :     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           7 :   EstimationManager() {
+     381           7 :   }
+     382             : 
+     383             :   void onInit();
+     384             : 
+     385             :   std::string getName() const;
+     386             : };
+     387             : /*//}*/
+     388             : 
+     389             : /*//{ onInit() */
+     390           7 : void EstimationManager::onInit() {
+     391             : 
+     392           7 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     393             : 
+     394           7 :   ros::Time::waitForValid();
+     395             : 
+     396           7 :   ROS_INFO("[%s]: initializing", getName().c_str());
+     397             : 
+     398           7 :   sm_ = std::make_shared<StateMachine>(nodelet_name_);
+     399             : 
+     400           7 :   ch_ = std::make_shared<CommonHandlers_t>();
+     401             : 
+     402           7 :   ch_->nodelet_name = nodelet_name_;
+     403           7 :   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           7 :   timer_initialization_ = nh_.createTimer(ros::Rate(1.0), &EstimationManager::timerInitialization, this, true, true);
+     407           7 : }
+     408             : /*//}*/
+     409             : 
+     410             : // --------------------------------------------------------------
+     411             : // |                          callbacks                         |
+     412             : // --------------------------------------------------------------
+     413             : 
+     414             : // | --------------------- timer callbacks -------------------- |
+     415             : 
+     416             : /*//{ timerPublish() */
+     417        9730 : void EstimationManager::timerPublish([[maybe_unused]] const ros::TimerEvent& event) {
+     418             : 
+     419        9730 :   if (!sm_->isInitialized()) {
+     420         861 :     return;
+     421             :   }
+     422             : 
+     423       19460 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublish", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     424             : 
+     425        9730 :   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        9730 :   if (!sm_->isInPublishableState()) {
+     431         861 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     432         861 :     return;
+     433             :   }
+     434             : 
+     435        8869 :   mrs_msgs::UavState uav_state;
+     436        8869 :   auto               ret = active_estimator_->getUavState();
+     437        8869 :   if (ret) {
+     438        8869 :     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        8869 :   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        8869 :   if (active_estimator_->isMitigatingJump()) {
+     451           0 :     estimator_switch_count_++;
+     452             :   }
+     453        8869 :   uav_state.estimator_iteration = estimator_switch_count_;
+     454             : 
+     455        8869 :   scope_timer.checkpoint("get uav state");
+     456             :   // TODO state health checks
+     457             : 
+     458        8869 :   ph_uav_state_.publish(uav_state);
+     459             : 
+     460        8869 :   scope_timer.checkpoint("pub uav state");
+     461       17738 :   nav_msgs::Odometry odom_main = Support::uavStateToOdom(uav_state);
+     462             : 
+     463        8869 :   scope_timer.checkpoint("uav state to odom");
+     464       17738 :   const std::vector<double> pose_covariance = active_estimator_->getPoseCovariance();
+     465      328153 :   for (size_t i = 0; i < pose_covariance.size(); i++) {
+     466      319284 :     odom_main.pose.covariance[i] = pose_covariance[i];
+     467             :   }
+     468             : 
+     469       17738 :   const std::vector<double> twist_covariance = active_estimator_->getTwistCovariance();
+     470      328153 :   for (size_t i = 0; i < twist_covariance.size(); i++) {
+     471      319284 :     odom_main.twist.covariance[i] = twist_covariance[i];
+     472             :   }
+     473             : 
+     474        8869 :   scope_timer.checkpoint("get covariance");
+     475        8869 :   ph_odom_main_.publish(odom_main);
+     476             : 
+     477       17738 :   nav_msgs::Odometry innovation = active_estimator_->getInnovation();
+     478        8869 :   ph_innovation_.publish(innovation);
+     479             : 
+     480             : 
+     481       17738 :   mrs_msgs::Float64Stamped agl_height;
+     482             : 
+     483        8869 :   if (is_using_agl_estimator_) {
+     484        4753 :     agl_height = est_alt_agl_->getUavAglHeight();
+     485        4753 :     ph_altitude_agl_.publish(agl_height);
+     486             :   }
+     487             : }
+     488             : /*//}*/
+     489             : 
+     490             : /*//{ timerPublishDiagnostics() */
+     491         970 : void EstimationManager::timerPublishDiagnostics([[maybe_unused]] const ros::TimerEvent& event) {
+     492             : 
+     493         970 :   if (!sm_->isInitialized()) {
+     494          81 :     return;
+     495             :   }
+     496             : 
+     497        1940 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublishDiagnostics", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     498             : 
+     499         970 :   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         970 :   if (!sm_->isInPublishableState()) {
+     505          81 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     506          81 :     return;
+     507             :   }
+     508             : 
+     509         889 :   mrs_msgs::UavState uav_state;
+     510         889 :   auto               ret = active_estimator_->getUavState();
+     511         889 :   if (ret) {
+     512         889 :     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         889 :   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        1778 :   mrs_msgs::Float64Stamped agl_height;
+     524             : 
+     525         889 :   if (is_using_agl_estimator_) {
+     526         477 :     agl_height = est_alt_agl_->getUavAglHeight();
+     527         477 :     ph_altitude_agl_.publish(agl_height);
+     528             :   }
+     529             : 
+     530        1778 :   mrs_msgs::Float64Stamped max_flight_z_msg;
+     531         889 :   max_flight_z_msg.header.stamp = ros::Time::now();
+     532         889 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     533         889 :     max_flight_z_msg.header.frame_id = active_estimator_->getFrameId();
+     534         889 :     max_flight_z_msg.value           = active_estimator_->getMaxFlightZ();
+     535             :   }
+     536         889 :   max_flight_z_ = max_flight_z_msg.value;
+     537         889 :   ph_max_flight_z_.publish(max_flight_z_msg);
+     538             : 
+     539             :   // publish diagnostics
+     540        1778 :   mrs_msgs::EstimationDiagnostics diagnostics;
+     541             : 
+     542         889 :   diagnostics.header.stamp   = uav_state.header.stamp;
+     543         889 :   diagnostics.child_frame_id = uav_state.child_frame_id;
+     544             : 
+     545         889 :   diagnostics.pose         = uav_state.pose;
+     546         889 :   diagnostics.velocity     = uav_state.velocity;
+     547         889 :   diagnostics.acceleration = uav_state.acceleration;
+     548             : 
+     549         889 :   diagnostics.sm_state                 = sm_->getCurrentStateString();
+     550         889 :   diagnostics.max_flight_z             = max_flight_z_msg.value;
+     551         889 :   diagnostics.estimator_iteration      = estimator_switch_count_;
+     552         889 :   diagnostics.estimation_rate          = ch_->desired_uav_state_rate;
+     553         889 :   diagnostics.running_state_estimators = estimator_names_;
+     554             : 
+     555             :   {
+     556        1778 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     557         889 :     diagnostics.switchable_state_estimators = switchable_estimator_names_;
+     558             :   }
+     559             : 
+     560             : 
+     561         889 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     562         889 :     diagnostics.header.frame_id         = active_estimator_->getFrameId();
+     563         889 :     diagnostics.current_state_estimator = active_estimator_->getName();
+     564             :   } else {
+     565           0 :     diagnostics.header.frame_id         = "";
+     566           0 :     diagnostics.current_state_estimator = "";
+     567             :   }
+     568             : 
+     569         889 :   diagnostics.estimator_horizontal = uav_state.estimator_horizontal;
+     570         889 :   diagnostics.estimator_vertical   = uav_state.estimator_vertical;
+     571         889 :   diagnostics.estimator_heading    = uav_state.estimator_heading;
+     572             : 
+     573         889 :   if (is_using_agl_estimator_) {
+     574         477 :     diagnostics.estimator_agl_height = est_alt_agl_name_;
+     575         477 :     diagnostics.agl_height           = agl_height.value;
+     576             :   } else {
+     577         412 :     diagnostics.estimator_agl_height = "NOT_USED";
+     578         412 :     diagnostics.agl_height           = std::nanf("");
+     579             :   }
+     580             : 
+     581         889 :   diagnostics.platform_config = _platform_config_;
+     582         889 :   diagnostics.custom_config   = _custom_config_;
+     583             : 
+     584         889 :   ph_diagnostics_.publish(diagnostics);
+     585             : 
+     586         889 :   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        9730 : void EstimationManager::timerCheckHealth([[maybe_unused]] const ros::TimerEvent& event) {
+     594             : 
+     595        9730 :   if (!sm_->isInitialized()) {
+     596           0 :     return;
+     597             :   }
+     598             : 
+     599       29190 :   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       19460 :   std::vector<std::string> switchable_estimator_names;
+     603       19460 :   for (auto estimator : estimator_list_) {
+     604             : 
+     605        9730 :     if (estimator->isReady()) {
+     606             :       try {
+     607          21 :         ROS_INFO_THROTTLE(1.0, "[%s]: starting the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     608          21 :         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        9730 :     if (estimator->isRunning() && estimator->getName() != "dummy" && estimator->getName() != "ground_truth") {
+     617        9613 :       switchable_estimator_names.push_back(estimator->getName());
+     618             :     }
+     619             :   }
+     620             : 
+     621             :   {
+     622       19460 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     623        9730 :     switchable_estimator_names_ = switchable_estimator_names;
+     624             :   }
+     625             : 
+     626        9730 :   if (is_using_agl_estimator_ && est_alt_agl_->isReady()) {
+     627           4 :     est_alt_agl_->start();
+     628             :   }
+     629             : 
+     630             :   /*//}*/
+     631             : 
+     632       14209 :   if (!callbacks_disabled_by_service_ &&
+     633       14209 :       (sm_->isInState(StateMachine::FLYING_STATE) || sm_->isInState(StateMachine::HOVER_STATE) || sm_->isInState(StateMachine::READY_FOR_TAKEOFF_STATE))) {
+     634        3608 :     callbacks_enabled_ = true;
+     635             :   } else {
+     636        6122 :     callbacks_enabled_ = false;
+     637             :   }
+     638             : 
+     639             :   // TODO fuj if, zmenit na switch
+     640             :   // activate initial estimator
+     641        9730 :   if (sm_->isInState(StateMachine::INITIALIZED_STATE) && initial_estimator_->isRunning()) {
+     642        1488 :     std::scoped_lock lock(mutex_active_estimator_);
+     643         744 :     ROS_INFO_THROTTLE(1.0, "[%s]: activating the initial estimator %s", getName().c_str(), initial_estimator_->getName().c_str());
+     644         744 :     active_estimator_ = initial_estimator_;
+     645         744 :     if (active_estimator_->getName() == "dummy") {
+     646           0 :       sm_->changeState(StateMachine::DUMMY_STATE);
+     647             :     } else {
+     648         744 :       if (!is_using_agl_estimator_ || est_alt_agl_->isRunning()) {
+     649           7 :         sm_->changeState(StateMachine::READY_FOR_TAKEOFF_STATE);
+     650             :       } else {
+     651         737 :         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        9730 :   if (sm_->isInTheAir() && active_estimator_->isError()) {
+     659           0 :     sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+     660             :   }
+     661             : 
+     662        9730 :   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        9730 :   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        9730 :   if (sm_->isInState(StateMachine::READY_FOR_TAKEOFF_STATE)) {
+     680        5051 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == "LandoffTracker") {
+     681           5 :       sm_->changeState(StateMachine::TAKING_OFF_STATE);
+     682             :     }
+     683             :   }
+     684             : 
+     685        9730 :   if (sm_->isInState(StateMachine::TAKING_OFF_STATE)) {
+     686        3685 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker != "LandoffTracker") {
+     687           5 :       sm_->changeState(StateMachine::FLYING_STATE);
+     688             :     }
+     689             :   }
+     690             : 
+     691        9730 :   if (sm_->isInState(StateMachine::FLYING_STATE)) {
+     692         150 :     if ((ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     693          95 :       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           7 : void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEvent& event) {
+     702             : 
+     703          14 :   mrs_lib::ParamLoader param_loader(nh_, getName());
+     704             : 
+     705           7 :   param_loader.loadParam("custom_config", _custom_config_);
+     706           7 :   param_loader.loadParam("platform_config", _platform_config_);
+     707           7 :   param_loader.loadParam("world_config", _world_config_);
+     708             : 
+     709           7 :   if (_custom_config_ != "") {
+     710           7 :     param_loader.addYamlFile(_custom_config_);
+     711             :   }
+     712             : 
+     713           7 :   if (_platform_config_ != "") {
+     714           7 :     param_loader.addYamlFile(_platform_config_);
+     715             :   }
+     716             : 
+     717           7 :   if (_world_config_ != "") {
+     718           7 :     param_loader.addYamlFile(_world_config_);
+     719             :   }
+     720             : 
+     721           7 :   param_loader.addYamlFileFromParam("private_config");
+     722           7 :   param_loader.addYamlFileFromParam("public_config");
+     723           7 :   param_loader.addYamlFileFromParam("estimators_config");
+     724           7 :   param_loader.addYamlFileFromParam("active_estimators_config");
+     725             : 
+     726           7 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     727             : 
+     728             :   /*//{ load world_origin parameters */
+     729             : 
+     730          14 :   std::string world_origin_units;
+     731           7 :   bool        is_origin_param_ok = true;
+     732           7 :   double      world_origin_x     = 0;
+     733           7 :   double      world_origin_y     = 0;
+     734             : 
+     735           7 :   param_loader.loadParam("world_origin/units", world_origin_units);
+     736             : 
+     737           7 :   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           7 :   } else if (Support::toLowercase(world_origin_units) == "latlon") {
+     743             :     double lat, lon;
+     744           7 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getName().c_str());
+     745           7 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     746           7 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     747           7 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     748           7 :     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           7 :   ch_->world_origin.x = world_origin_x;
+     756           7 :   ch_->world_origin.y = world_origin_y;
+     757             : 
+     758           7 :   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           7 :   param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/");
+     765             : 
+     766             :   /*//{ load parameters into common handlers */
+     767           7 :   param_loader.loadParam("frame_id/fcu", ch_->frames.fcu);
+     768           7 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + ch_->frames.fcu;
+     769             : 
+     770           7 :   param_loader.loadParam("frame_id/fcu_untilted", ch_->frames.fcu_untilted);
+     771           7 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + ch_->frames.fcu_untilted;
+     772             : 
+     773           7 :   param_loader.loadParam("frame_id/rtk_antenna", ch_->frames.rtk_antenna);
+     774           7 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     775             : 
+     776           7 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getName());
+     777           7 :   ch_->transformer->retryLookupNewest(true);
+     778             : 
+     779           7 :   param_loader.loadParam("rate/diagnostics", ch_->desired_diagnostics_rate);
+     780             : 
+     781             :   /*//}*/
+     782             : 
+     783             :   /*//{ load parameters */
+     784             : 
+     785             :   /*//{ publish debug topics parameters */
+     786           7 :   param_loader.loadParam("debug_topics/input", ch_->debug_topics.input);
+     787           7 :   param_loader.loadParam("debug_topics/output", ch_->debug_topics.output);
+     788           7 :   param_loader.loadParam("debug_topics/state", ch_->debug_topics.state);
+     789           7 :   param_loader.loadParam("debug_topics/covariance", ch_->debug_topics.covariance);
+     790           7 :   param_loader.loadParam("debug_topics/innovation", ch_->debug_topics.innovation);
+     791           7 :   param_loader.loadParam("debug_topics/diagnostics", ch_->debug_topics.diag);
+     792           7 :   param_loader.loadParam("debug_topics/correction", ch_->debug_topics.correction);
+     793           7 :   param_loader.loadParam("debug_topics/correction_delay", ch_->debug_topics.corr_delay);
+     794             :   /*//}*/
+     795             : 
+     796             :   /*//}*/
+     797             : 
+     798          14 :   mrs_lib::SubscribeHandlerOptions shopts;
+     799           7 :   shopts.nh                 = nh_;
+     800           7 :   shopts.node_name          = getName();
+     801           7 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     802           7 :   shopts.threadsafe         = true;
+     803           7 :   shopts.autostart          = true;
+     804           7 :   shopts.queue_size         = 10;
+     805           7 :   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          21 :       mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     811          14 :   while (!sh_hw_api_capabilities_.hasMsg()) {
+     812           7 :     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           7 :     ros::Duration(1.0).sleep();
+     815             :   }
+     816             : 
+     817          14 :   mrs_msgs::HwApiCapabilitiesConstPtr hw_api_capabilities = sh_hw_api_capabilities_.getMsg();
+     818             :   /*//}*/
+     819             : 
+     820             :   /*//{ wait for desired uav_state rate */
+     821           7 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     822          14 :   while (!sh_control_manager_diag_.hasMsg()) {
+     823           7 :     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           7 :     ros::Duration(1.0).sleep();
+     826             :   }
+     827             : 
+     828          14 :   mrs_msgs::ControlManagerDiagnosticsConstPtr control_manager_diag_msg = sh_control_manager_diag_.getMsg();
+     829           7 :   ch_->desired_uav_state_rate                                          = control_manager_diag_msg->desired_uav_state_rate;
+     830           7 :   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           7 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     836             : 
+     837             :   /*//}*/
+     838             : 
+     839             :   /*//{ load state estimator plugins */
+     840           7 :   param_loader.loadParam("state_estimators", estimator_names_);
+     841             : 
+     842           7 :   state_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>>("mrs_uav_managers", "mrs_uav_managers::StateEstimator");
+     843             : 
+     844          14 :   for (size_t i = 0; i < estimator_names_.size(); i++) {
+     845             : 
+     846          14 :     const std::string estimator_name = estimator_names_[i];
+     847             : 
+     848             :     // load the estimator parameters
+     849          14 :     std::string address;
+     850           7 :     param_loader.loadParam(estimator_name + "/address", address);
+     851             : 
+     852             :     try {
+     853           7 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     854           7 :       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           7 :   param_loader.loadParam("agl_height_estimator", est_alt_agl_name_);
+     870           7 :   is_using_agl_estimator_ = est_alt_agl_name_ != "";
+     871           7 :   ROS_WARN_COND(!is_using_agl_estimator_, "[%s]: not using AGL estimator for min height safe checking", getName().c_str());
+     872             : 
+     873             : 
+     874           7 :   if (is_using_agl_estimator_) {
+     875             : 
+     876           4 :     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           8 :     std::string address;
+     880           4 :     param_loader.loadParam(est_alt_agl_name_ + "/address", address);
+     881             : 
+     882             :     try {
+     883           4 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     884           4 :       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           7 :   ROS_INFO("[%s]: estimators were loaded", getName().c_str());
+     900             :   /*//}*/
+     901             : 
+     902             :   /*//{ check whether initial estimator was loaded */
+     903           7 :   param_loader.loadParam("initial_state_estimator", initial_estimator_name_);
+     904           7 :   bool initial_estimator_found = false;
+     905           7 :   for (auto estimator : estimator_list_) {
+     906           7 :     if (estimator->getName() == initial_estimator_name_) {
+     907           7 :       initial_estimator_      = estimator;
+     908           7 :       initial_estimator_found = true;
+     909           7 :       break;
+     910             :     }
+     911             :   }
+     912             : 
+     913           7 :   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          14 :   for (auto estimator : estimator_list_) {
+     921             : 
+     922             :     // create private handlers
+     923          14 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     924             : 
+     925           7 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     926           7 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, estimator->getName()), "EstimationManager/" + estimator->getName());
+     927             : 
+     928           7 :     if (_custom_config_ != "") {
+     929           7 :       ph->param_loader->addYamlFile(_custom_config_);
+     930             :     }
+     931             : 
+     932           7 :     if (_platform_config_ != "") {
+     933           7 :       ph->param_loader->addYamlFile(_platform_config_);
+     934             :     }
+     935             : 
+     936           7 :     if (_world_config_ != "") {
+     937           7 :       ph->param_loader->addYamlFile(_world_config_);
+     938             :     }
+     939             : 
+     940             :     try {
+     941           7 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     942           7 :       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           7 :     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           7 :   if (is_using_agl_estimator_) {
+     957             : 
+     958           8 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     959             : 
+     960           4 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     961           4 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, est_alt_agl_->getName()), "EstimationManager/" + est_alt_agl_->getName());
+     962             : 
+     963           4 :     if (_custom_config_ != "") {
+     964           4 :       ph->param_loader->addYamlFile(_custom_config_);
+     965             :     }
+     966             : 
+     967           4 :     if (_platform_config_ != "") {
+     968           4 :       ph->param_loader->addYamlFile(_platform_config_);
+     969             :     }
+     970             : 
+     971           4 :     if (_world_config_ != "") {
+     972           4 :       ph->param_loader->addYamlFile(_world_config_);
+     973             :     }
+     974             : 
+     975             :     try {
+     976           4 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), est_alt_agl_->getName().c_str());
+     977           4 :       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           4 :     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           7 :   ROS_INFO("[%s]: estimators were initialized", getName().c_str());
+     991             : 
+     992             :   /*//}*/
+     993             : 
+     994             :   /*//{ initialize publishers */
+     995           7 :   ph_uav_state_    = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh_, "uav_state_out", 10);
+     996           7 :   ph_odom_main_    = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "odom_main_out", 10);
+     997           7 :   ph_innovation_   = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "innovation_out", 10);
+     998           7 :   ph_diagnostics_  = mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics>(nh_, "diagnostics_out", 10);
+     999           7 :   ph_max_flight_z_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "max_flight_z_agl_out", 10);
+    1000           7 :   ph_altitude_agl_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "height_agl_out", 10);
+    1001             : 
+    1002             :   /*//}*/
+    1003             : 
+    1004             :   /*//{ initialize timers */
+    1005           7 :   timer_publish_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerPublish, this);
+    1006             : 
+    1007           7 :   timer_publish_diagnostics_ = nh_.createTimer(ros::Rate(ch_->desired_diagnostics_rate), &EstimationManager::timerPublishDiagnostics, this);
+    1008             : 
+    1009           7 :   timer_check_health_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerCheckHealth, this);
+    1010             :   /*//}*/
+    1011             : 
+    1012             :   /*//{ initialize service clients */
+    1013             : 
+    1014           7 :   srvch_failsafe_.initialize(nh_, "failsafe_out");
+    1015             : 
+    1016             :   /*//}*/
+    1017             : 
+    1018             :   /*//{ initialize service servers */
+    1019           7 :   srvs_change_estimator_ = nh_.advertiseService("change_estimator_in", &EstimationManager::callbackChangeEstimator, this);
+    1020           7 :   srvs_toggle_callbacks_ = nh_.advertiseService("toggle_service_callbacks_in", &EstimationManager::callbackToggleServiceCallbacks, this);
+    1021             :   /*//}*/
+    1022             : 
+    1023             :   /*//{ initialize scope timer */
+    1024           7 :   param_loader.loadParam("scope_timer/enabled", ch_->scope_timer.enabled);
+    1025          14 :   std::string       filepath;
+    1026          14 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/scope_timer.txt";
+    1027           7 :   ch_->scope_timer.logger                = std::make_shared<mrs_lib::ScopeTimerLogger>(time_logger_filepath, ch_->scope_timer.enabled);
+    1028             :   /*//}*/
+    1029             : 
+    1030           7 :   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           7 :   sm_->changeState(StateMachine::INITIALIZED_STATE);
+    1036             : 
+    1037           7 :   ROS_INFO("[%s]: initialized", getName().c_str());
+    1038           7 : }
+    1039             : /*//}*/
+    1040             : 
+    1041             : // | -------------------- service callbacks ------------------- |
+    1042             : 
+    1043             : /*//{ callbackChangeEstimator() */
+    1044           0 : bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    1045             : 
+    1046           0 :   if (!sm_->isInitialized()) {
+    1047           0 :     return false;
+    1048             :   }
+    1049             : 
+    1050           0 :   if (!callbacks_enabled_) {
+    1051           0 :     res.success = false;
+    1052           0 :     res.message = ("Service callbacks are disabled");
+    1053           0 :     ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
+    1054           0 :     return true;
+    1055             :   }
+    1056             : 
+    1057           0 :   if (req.value == "dummy" || req.value == "ground_truth") {
+    1058           0 :     res.success = false;
+    1059           0 :     std::stringstream ss;
+    1060           0 :     ss << "Switching to " << req.value << " estimator is not allowed.";
+    1061           0 :     res.message = ss.str();
+    1062           0 :     ROS_WARN("[%s]: Switching to %s estimator is not allowed.", getName().c_str(), req.value.c_str());
+    1063           0 :     return true;
+    1064             :   }
+    1065             : 
+    1066             :   // we do not want the switch to be disturbed by a service call
+    1067           0 :   callbacks_enabled_ = false;
+    1068             : 
+    1069           0 :   bool                                                target_estimator_found = false;
+    1070           0 :   boost::shared_ptr<mrs_uav_managers::StateEstimator> target_estimator;
+    1071           0 :   for (auto estimator : estimator_list_) {
+    1072           0 :     if (estimator->getName() == req.value) {
+    1073           0 :       target_estimator       = estimator;
+    1074           0 :       target_estimator_found = true;
+    1075           0 :       break;
+    1076             :     }
+    1077             :   }
+    1078             : 
+    1079           0 :   if (target_estimator_found) {
+    1080             : 
+    1081           0 :     if (target_estimator->isRunning()) {
+    1082           0 :       sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+    1083           0 :       switchToEstimator(target_estimator);
+    1084           0 :       sm_->changeToPreSwitchState();
+    1085             :     } else {
+    1086           0 :       ROS_WARN("[%s]: Switch to not running estimator %s requested", getName().c_str(), req.value.c_str());
+    1087           0 :       res.success = false;
+    1088           0 :       res.message = ("Requested estimator is not running");
+    1089           0 :       return true;
+    1090             :     }
+    1091             : 
+    1092             :   } else {
+    1093           0 :     ROS_WARN("[%s]: Switch to invalid estimator %s requested", getName().c_str(), req.value.c_str());
+    1094           0 :     res.success = false;
+    1095           0 :     res.message = ("Not a valid estimator type");
+    1096           0 :     return true;
+    1097             :   }
+    1098             : 
+    1099           0 :   res.success = true;
+    1100           0 :   res.message = "Estimator switch successful";
+    1101             : 
+    1102             :   // allow service calllbacks after switch again
+    1103           0 :   callbacks_enabled_ = true;
+    1104             : 
+    1105           0 :   return true;
+    1106             : }
+    1107             : /*//}*/
+    1108             : 
+    1109             : /* //{ callbackToggleServiceCallbacks() */
+    1110          13 : bool EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1111             : 
+    1112          13 :   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          13 :   callbacks_disabled_by_service_ = !req.data;
+    1118             : 
+    1119          13 :   res.success = true;
+    1120          13 :   res.message = (callbacks_disabled_by_service_ ? "Service callbacks disabled" : "Service callbacks enabled");
+    1121             : 
+    1122          13 :   if (callbacks_disabled_by_service_) {
+    1123             : 
+    1124           6 :     ROS_INFO("[%s]: Service callbacks disabled.", getName().c_str());
+    1125             : 
+    1126             :   } else {
+    1127             : 
+    1128           7 :     ROS_INFO("[%s]: Service callbacks enabled", getName().c_str());
+    1129             :   }
+    1130             : 
+    1131          13 :   return true;
+    1132             : }
+    1133             : 
+    1134             : //}
+    1135             : 
+    1136             : 
+    1137             : // --------------------------------------------------------------
+    1138             : // |                        other methods                       |
+    1139             : // --------------------------------------------------------------
+    1140             : //
+    1141             : /*//{ switchToHealthyEstimator() */
+    1142           0 : bool EstimationManager::switchToHealthyEstimator() {
+    1143             : 
+    1144             :   // available estimators should be specified in decreasing priority order in config file
+    1145           0 :   for (auto estimator : estimator_list_) {
+    1146           0 :     if (estimator->isRunning()) {
+    1147           0 :       switchToEstimator(estimator);
+    1148           0 :       return true;
+    1149             :     }
+    1150             :   }
+    1151           0 :   return false;  // no other estimator is running
+    1152             : }
+    1153             : /*//}*/
+    1154             : 
+    1155             : /*//{ switchToEstimator() */
+    1156           0 : void EstimationManager::switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator) {
+    1157             : 
+    1158           0 :   std::scoped_lock lock(mutex_active_estimator_);
+    1159           0 :   ROS_INFO("[%s]: switching estimator from %s to %s", getName().c_str(), active_estimator_->getName().c_str(), target_estimator->getName().c_str());
+    1160           0 :   active_estimator_ = target_estimator;
+    1161           0 :   estimator_switch_count_++;
+    1162           0 : }
+    1163             : /*//}*/
+    1164             : 
+    1165             : /*//{ callFailsafeService() */
+    1166           0 : bool EstimationManager::callFailsafeService() {
+    1167           0 :   std_srvs::Trigger srv_out;
+    1168           0 :   return srvch_failsafe_.call(srv_out);
+    1169             : }
+    1170             : /*//}*/
+    1171             : 
+    1172             : /*//{ getName() */
+    1173         195 : std::string EstimationManager::getName() const {
+    1174         195 :   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           7 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::estimation_manager::EstimationManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..2f21979eca --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html @@ -0,0 +1,328 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..86871c9f194a40ca6ab4d87f667d31d9ea1c8b5d GIT binary patch literal 4305 zcmV;?5H9bDP)dWGf&=@ptAr1!tdHt_`rh$jB+%#Ppg-?T%KmxzLPxZS4PyN%OO zXb05(&&>>I05&y@@!ce15nz#{rmiKXIKXoh5R9jAaTpcq%z%wuE6n6_Gy&SXmU&_T zdDeNOI3p*DQAEXbZ3eV)ZKkoJYa>Rn>+2h@LPkE#auoU301E8-b~7E1p-=z1xQCg+_(-6Ur1<*&%i;m8G@2WxF(a*pM%Vh%==xmvE)E(OS;>(B z#LNtV7W}ExH31|r$NQdK%M4$_HDznA7z*}wyMe%GpX9YgK(^H6eCFko<{g=|EO~t> zqz^g`SJD^nS#lah>*=BEDd?@l%$%|D$YoyPaqpRZvqQ&@@%endT-(Aw>-(Secv;?OQ{i0;(3!OKEwk>^73EXoE%s8+4>5A zD-oNB(IlRWeZ~C_O;?uWA;&bof?-Q0T4z0qe7tnF0rSRL+v^ZY#8?vK0cbHa$BgJm zFxMuh(kgIS=90kUlpP+f7Wf+wH*RU{t?(CXMvWhZp+LTMxn^F!SKzX^Wr4@15WQY2 z@cmQ6Q1Id0uhPsj&4uDT7@3Ka=ITQT<||ifX2Te-6Szlc0yM=lO#%;GvOVwu53lKi zO9k$lVJIYK`e?=r2H61%Xu{Z0h1#I`hzi|jzjw^6MH;EVnXg0r0*ka1~wGbG;ZYB|r|wEz;-Sbz%Ia!DVIInj#J2Ino|lRu=$Xl!3+AfR;4C z$qJa*IYVqjXez)oV)QuGOw$mOP37UxIoTwqUUAZH-v5*QV~0e}=p|WVpE`i>npNlI z>s}F~(XL>>h5`*~=;eUMx(j=?$>k*<7PhNtG}i%LSl&|x@qob|FGJX8Dg=pc9neEf zV9coQs?c@AgcSQ{wJ!s1vO;^<&a;0tzMMwVf_*B0PE-l- zny%x(&;TdU@l6Eux*Wl~YXSGj*8*6lEt%{1WCg6XW)UO8T8+^nhU2ee)>l@J+}O27 z0PgyRzBe^UG3#1JK)XKAH|1I>yc!7C=XeIzQ#uC`KNWL@(~jf~I5B`@xoG=>b|Euf z4LP9aVw;g>ngF14ylwaNWg%PY8Il+p@lq0!t*0>3#svT=RW zn3-}w6M1#ajCp5gn(?3=1r!H)W7olp)Z!ortIsQF6`D2sJvK16Gd!N*k!L&Y#A!>e z3WR+&ugft6t@)O!uWAm6|a$zs2NYl8ADMz(U_j4=*ERhAWi z)~=73B7_g)6hfdq3>c@lz4VSLfUYz4;rqepDDDF*G?qs5;A%W{noT1;51ozx<>-AN zaQ}_RSZ@Zek{=@h3QQ%Nj~c^;7)$554BGB?h$We+f1DpMHT@mBb|-uk6$a1gl3ch^U2B!k5%3O;OXb>x z@IG)AA6u}dEm_f5S&v{vfSGl#SJ@)KV1*p1)krjUU?!^V_$OEB*{=QR9~N+daEv04wg;NWU*3oieKQ$d*fnEr zNY_0)Zq5uNPLUbv4Krq-Wco9MDLY;mj6Dr^rc9$Q@~-8=G-KCo;+ZprG;xg#MVuCc zQDk;SdC#mz5|G_`%mI0~&4RQwBmv5ieN@%~@E6$^Xgm|ybMgIG#b$qz{a<9?Zasn( z{fNl^|2cD=5f3XNdG(QPA%hDG7(+^qH0PR^6b3xL%g5{$nF`8;DDbQw9<*}T)so|( zT}w`=ahZZ;sBw*jeX{HQT|}tUh}5hzBlFTGp70s9Gp1_>kf-r+*bQC)@qi1{wYUTT zyf<-e4sw|jISea;H_HOFd!IsbYio5<2gO;eDR51B_TH>1RPfX1#>}GmA(rD9HV4>u zc0D9Vg&E{XKW2v(Gtck?q33+fa3927ijA4geg|ApET(zqZ zk7=ZXdC?HirVby6AC1Z}$UjlXNweiHJBL*PyccSPK-ZX^#l6n(t(Rl!S;uHA7G0=J%RzP=>7cSK6B zGtHpc=X<>O1?U4rMP&~7CO1&xM63@DNj0SY~I)U#M z=Tj`VQVLGTTPVX)l_c+8V@b+mCa(kpe~d1B4p$O9Tt*mS1mlX$oMl8hKSi|#u0ka( z_qdzk1`T&v8(9Qk7ceIpbq5KUu^AdW7tSHRoR>Hy;`y5P3q8ef{jD5*egwjo2z4Ve+U)8+Cm&kms{krrtvTL!wH!KxK}AG!}+dr3m0hg79N9>m?3cYtgYdz1S3c+ ztT3(MuDumRxMf`%<=ElQ0($}iU&{yI0^t*`L(XMQp(3~u2qIji>yz!`$rZD`;_X== zRQ>0Mi!brcAh~@pEq0DCU?R3arBnLW~uG`_=6Wj{xtkxPDd-fNa6qyWGkKM%ITP%Se8i5-(_^q+{$%`FK(f>9H3qf0QC*i76+Lqwh3hfRfL1x8!KG}e zU^%YGui}83%hiv!t&1Ex?2$5U&MOz2W`*e%H(;2VL5t0B6$vCPBM=QPv#$Au$9P^6 zlYVjxuPm1HUDNU=q5O%eW@+J?DO^h=h%|B-&vBhsX;hh6m$g)u6FrSFIY+a_=mGnj8w!i6S6X{lq=FXkTugL_A%Rb^B@Hp+N%7;1koZ71 zVA1a8rZt+Ze3`EQ!TK6R=|CQR# zRd?eYxD2O!tr+9L4g>$JhZzfp}*bghxNZSI4ZkRL&<4rJkvg@*hN4%d= zF8TtxHm0vgkzLx-$kx}))<-<*mg?B0Sim}nglInW@A$KEHex*Wo9-8 z<3-H$DCJ1=wdSJ?>+3KS^0D%8b21%V3^r%rA^+%Jx*T{n9~v-r4F};h9{1mPv*>ka zsKN>wdwDY@B=j2^f8OkWw5mmL@|QF&MPNJx~jh&vh{&>-;dsm2kr>?Aeh&K+!SAg(^m(JeL9uO_Cn zm3#dmmQ-;%DHGu^2vZzwb4^|$&ywW$-v0qW?f`pgO-FZ1OfOpn^a z1{&3x7=^|R@M;A8u}R-jXg%`4;OjX92WoHw)nLr_L%IH3e1{eeG0+SAoVX zrO9)@;3SFvHYcVI;^8J&F7OiTalxIaUsTsj8`X@lk!e^yGx6DjmDLG3&<0qXs@ zCM!??U&?g=0k9;T&n_UbBNZ;Ecvk6}-gqH6Kjqqqs09VkhEb%^cuUL^4QzOE#zq~f z%}ZfGfQ#Cj2Q0ga4tpu}r`X6%BuF-|6zx?_aa$bWuY4qacIJvC7P^L-PB)8iTw7@j zrJ>xazc5997<=8b33kAp?A(z!2gRfshkynVPD8^6Du5i0ZvpK1Md) z!=_{wfB0}Ci{`=<$45m?Q(Tu|I2go5N9cL$S(BZ3M_o0pH~95bN4_DetM?RsF}l8R zR2^xqo4F=X@g%Fs?ChV3#OR|O@9XLxUXE)wnD)2Cd!pe~y*sme#1bGb@n7Kq0KB7V zTc>=uf-R_URL9v9awrzWfPD%^g-DIxrb2MZX&ewgOoh4zfeKX(mQ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:618571.8 %
Date:2023-11-30 22:31:42Functions: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)108
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)108
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const108
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const168
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const216
mrs_uav_managers::Estimator::isStarted() const419
mrs_uav_managers::Estimator::getMaxFlightZ() const905
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)11072
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)11077
mrs_uav_managers::Estimator::isReady() const15281
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const23437
mrs_uav_managers::Estimator::isMitigatingJump() const25363
mrs_uav_managers::Estimator::getName[abi:cxx11]() const30916
mrs_uav_managers::Estimator::publishDiagnostics() const36428
mrs_uav_managers::Estimator::isError() const44491
mrs_uav_managers::Estimator::isRunning() const57919
mrs_uav_managers::Estimator::isInitialized() const131051
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const369827
mrs_uav_managers::Estimator::getCurrentSmState() const392159
+
+
+ + + +
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..1e831322c5 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:618571.8 %
Date:2023-11-30 22:31:42Functions: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)108
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)11077
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)11072
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&)108
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const23437
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const168
mrs_uav_managers::Estimator::getMaxFlightZ() const905
mrs_uav_managers::Estimator::isInitialized() const131051
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const216
mrs_uav_managers::Estimator::isMitigatingJump() const25363
mrs_uav_managers::Estimator::getCurrentSmState() const392159
mrs_uav_managers::Estimator::publishDiagnostics() const36428
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const108
mrs_uav_managers::Estimator::getName[abi:cxx11]() const30916
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isError() const44491
mrs_uav_managers::Estimator::isReady() const15281
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const369827
mrs_uav_managers::Estimator::isRunning() const57919
mrs_uav_managers::Estimator::isStarted() const419
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..5bd9b7734a --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html @@ -0,0 +1,290 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:618571.8 %
Date:2023-11-30 22:31:42Functions: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         108 : bool Estimator::changeState(SMStates_t new_state) {
+       9             : 
+      10         108 :   if (new_state == getCurrentSmState()) {
+      11           0 :     return true;
+      12             :   }
+      13             : 
+      14         108 :   previous_sm_state_ = getCurrentSmState();
+      15         108 :   setCurrentSmState(new_state);
+      16             : 
+      17         108 :   ROS_INFO("[%s]: Switching sm state %s -> %s", getPrintName().c_str(), getSmStateString(previous_sm_state_).c_str(), getCurrentSmStateString().c_str());
+      18         108 :   return true;
+      19             : }
+      20             : /*//}*/
+      21             : 
+      22             : /*//{ isInState() */
+      23      369827 : bool Estimator::isInState(const SMStates_t& state_in) const {
+      24      369827 :   return state_in == getCurrentSmState();
+      25             : }
+      26             : /*//}*/
+      27             : 
+      28             : /*//{ isInitialized() */
+      29      131051 : bool Estimator::isInitialized() const {
+      30      131051 :   return !isInState(UNINITIALIZED_STATE);
+      31             : }
+      32             : /*//}*/
+      33             : 
+      34             : /*//{ isReady() */
+      35       15281 : bool Estimator::isReady() const {
+      36       15281 :   return isInState(READY_STATE);
+      37             : }
+      38             : /*//}*/
+      39             : 
+      40             : /*//{ isStarted() */
+      41         419 : bool Estimator::isStarted() const {
+      42         419 :   return isInState(STARTED_STATE);
+      43             : }
+      44             : /*//}*/
+      45             : 
+      46             : /*//{ isRunning() */
+      47       57919 : bool Estimator::isRunning() const {
+      48       57919 :   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       44491 : bool Estimator::isError() const {
+      60       44491 :   return isInState(ERROR_STATE);
+      61             : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ getCurrentSmState() */
+      65      392159 : SMStates_t Estimator::getCurrentSmState() const {
+      66      392159 :   return current_sm_state_;
+      67             : }
+      68             : /*//}*/
+      69             : 
+      70             : /*//{ setCurrentSmState() */
+      71         108 : void Estimator::setCurrentSmState(const SMStates_t& new_state) {
+      72         108 :   std::scoped_lock lock(mutex_current_state_);
+      73         108 :   current_sm_state_ = new_state;
+      74         108 : }
+      75             : /*//}*/
+      76             : 
+      77             : /*//{ getSmStateString() */
+      78         216 : std::string Estimator::getSmStateString(const SMStates_t& state) const {
+      79         216 :   return sm::state_names[state];
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ getCurrentSmStateName() */
+      84         108 : std::string Estimator::getCurrentSmStateString(void) const {
+      85         216 :   return getSmStateString(getCurrentSmState());
+      86             : }
+      87             : /*//}*/
+      88             : 
+      89             : /*//{ isMitigatingJump() */
+      90       25363 : bool Estimator::isMitigatingJump(void) const {
+      91       25363 :   return is_mitigating_jump_;
+      92             : }
+      93             : /*//}*/
+      94             : 
+      95             : /*//{ getName() */
+      96       30916 : std::string Estimator::getName(void) const {
+      97       30916 :   return name_;
+      98             : }
+      99             : /*//}*/
+     100             : 
+     101             : /*//{ getPrintName() */
+     102         168 : std::string Estimator::getPrintName(void) const {
+     103         336 :   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       23437 : std::string Estimator::getFrameId(void) const {
+     115       23437 :   return ns_frame_id_;
+     116             : }
+     117             : /*//}*/
+     118             : 
+     119             : /*//{ getMaxFlightZ() */
+     120         905 : double Estimator::getMaxFlightZ(void) const {
+     121         905 :   return max_flight_z_;
+     122             : }
+     123             : /*//}*/
+     124             : 
+     125             : /*//{ publishDiagnostics() */
+     126       36428 : void Estimator::publishDiagnostics() const {
+     127             : 
+     128       36428 :   if (!ch_->debug_topics.diag) {
+     129       36425 :     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       11072 : tf2::Vector3 Estimator::getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr& input_msg, const double hdg) {
+     153             : 
+     154       22102 :   geometry_msgs::Vector3Stamped acc_stamped;
+     155       11052 :   acc_stamped.vector = input_msg->control_acceleration;
+     156       11022 :   acc_stamped.header = input_msg->header;
+     157       22000 :   return getAccGlobal(acc_stamped, hdg);
+     158             : }
+     159             : 
+     160       11077 : tf2::Vector3 Estimator::getAccGlobal(const geometry_msgs::Vector3Stamped& acc_stamped, const double hdg) {
+     161             : 
+     162             :   // untilt the desired acceleration vector
+     163       22080 :   geometry_msgs::Vector3Stamped des_acc;
+     164       11008 :   geometry_msgs::Vector3        des_acc_untilted;
+     165       11019 :   des_acc.vector.x        = acc_stamped.vector.x;
+     166       11019 :   des_acc.vector.y        = acc_stamped.vector.y;
+     167       11019 :   des_acc.vector.z        = acc_stamped.vector.z;
+     168       11019 :   des_acc.header.frame_id = ch_->frames.ns_fcu;
+     169       11042 :   des_acc.header.stamp    = acc_stamped.header.stamp;
+     170       22118 :   auto response_acc       = ch_->transformer->transformSingle(des_acc, ch_->frames.ns_fcu_untilted);
+     171       11087 :   if (response_acc) {
+     172       11087 :     des_acc_untilted.x = response_acc.value().vector.x;
+     173       11087 :     des_acc_untilted.y = response_acc.value().vector.y;
+     174       11087 :     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       11087 :   const tf2::Vector3 des_acc_global = Support::rotateVecByHdg(des_acc_untilted, hdg);
+     182             : 
+     183       22031 :   return des_acc_global;
+     184             : }
+     185             : /*//}*/
+     186             : 
+     187             : /*//{ getHeadingRate() */
+     188           0 : std::optional<double> Estimator::getHeadingRate(const nav_msgs::OdometryConstPtr& odom_msg) {
+     189             : 
+     190             :   double hdg_rate;
+     191             :   try {
+     192           0 :     hdg_rate = mrs_lib::AttitudeConverter(odom_msg->pose.pose.orientation).getHeadingRate(odom_msg->twist.twist.angular);
+     193             :   }
+     194           0 :   catch (...) {
+     195           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading rate", getPrintName().c_str());
+     196           0 :     return {};
+     197             :   }
+     198           0 :   return hdg_rate;
+     199             : }
+     200             : /*//}*/
+     201             : 
+     202             : 
+     203             : /*//}*/
+     204             : 
+     205             : }  // namespace mrs_uav_managers
+     206             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html new file mode 100644 index 0000000000..109fe8a5e8 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html @@ -0,0 +1,72 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::publishAglHeight() const5153
mrs_uav_managers::AglEstimator::publishCovariance() const5153
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const4
+
+
+ + + +
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..d2574426c7 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2023-11-30 22:31:42Functions: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        5153 : void AglEstimator::publishAglHeight() const {
+       8        5153 :   ph_agl_height_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_));
+       9        5153 : }
+      10             : /*//}*/
+      11             : 
+      12             : /*//{ publishCovariance() */
+      13        5153 : void AglEstimator::publishCovariance() const {
+      14             : 
+      15        5153 :   if (!ch_->debug_topics.covariance) {
+      16        5153 :     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           4 : bool AglEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+      25             : 
+      26           4 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      27           4 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      28             : 
+      29           4 :   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           4 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+      35           4 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+      36           4 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+      37           4 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+      38           4 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+      39           4 :   ph_->param_loader->loadParam("requires/position", requires_position);
+      40           4 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+      41           4 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+      42           4 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+      43             : 
+      44           4 :   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           4 :   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           4 :   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           4 :   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           4 :   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           4 :   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           4 :   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           4 :   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           4 :   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           4 :   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           4 :   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..622e7783ad --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2023-11-30 22:31:42Functions: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..551944cf0a --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2023-11-30 22:31:42Functions: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..aa5bd0cb4e --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2023-11-30 22:31:42Functions: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..7de94d7cea --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2023-11-30 22:31:42Functions: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..7690a5574f --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2023-11-30 22:31:42Functions: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..a86c2c7dfa --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2023-11-30 22:31:42Functions: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..17d0daea71 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2023-11-30 22:31:42Functions: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&) const7
mrs_uav_managers::StateEstimator::getInnovation() const8869
mrs_uav_managers::StateEstimator::getPoseCovariance() const8869
mrs_uav_managers::StateEstimator::getTwistCovariance() const8869
mrs_uav_managers::StateEstimator::publishOdom() const9618
mrs_uav_managers::StateEstimator::publishUavState() const9618
mrs_uav_managers::StateEstimator::publishCovariance() const9618
mrs_uav_managers::StateEstimator::publishInnovation() const9618
mrs_uav_managers::StateEstimator::getUavState()9757
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const11005
+
+
+ + + +
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..2d6b2c43dc --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2023-11-30 22:31:42Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
<unnamed>62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..e101a2e728 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:39161363.8 %
Date:2023-11-30 22:31:42Functions:374778.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
<unnamed>62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail.html b/mrs_uav_managers/src/estimation_manager/index-detail.html new file mode 100644 index 0000000000..088ab6f808 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:39161363.8 %
Date:2023-11-30 22:31:42Functions:374778.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
<unnamed>62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-sort-f.html b/mrs_uav_managers/src/estimation_manager/index-sort-f.html new file mode 100644 index 0000000000..d290c611d2 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:39161363.8 %
Date:2023-11-30 22:31:42Functions:374778.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-sort-l.html new file mode 100644 index 0000000000..01c02a8703 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:39161363.8 %
Date:2023-11-30 22:31:42Functions:374778.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index.html b/mrs_uav_managers/src/estimation_manager/index.html new file mode 100644 index 0000000000..b5eafe8045 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:39161363.8 %
Date:2023-11-30 22:31:42Functions:374778.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..80da1bbdad --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:20225379.8 %
Date:2023-11-30 22:31:42Functions: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()7
mrs_uav_managers::gain_manager::GainManager::onInit()7
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)7
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)114
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&)154
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)1182
+
+
+ + + +
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..cc01c10f41 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.func.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:20225379.8 %
Date:2023-11-30 22:31:42Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()7
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&)154
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&)114
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)1182
mrs_uav_managers::gain_manager::GainManager::onInit()7
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)7
+
+
+ + + +
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..198a09da6b --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.html @@ -0,0 +1,720 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:20225379.8 %
Date:2023-11-30 22:31:42Functions: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           7 : void GainManager::onInit() {
+     122             : 
+     123          14 :   ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     124             : 
+     125           7 :   ros::Time::waitForValid();
+     126             : 
+     127           7 :   ROS_INFO("[GainManager]: initializing");
+     128             : 
+     129             :   // | ------------------------- params ------------------------- |
+     130             : 
+     131          14 :   mrs_lib::ParamLoader param_loader(nh_, "GainManager");
+     132             : 
+     133          14 :   std::string custom_config_path;
+     134          14 :   std::string platform_config_path;
+     135             : 
+     136           7 :   param_loader.loadParam("custom_config", custom_config_path);
+     137           7 :   param_loader.loadParam("platform_config", platform_config_path);
+     138             : 
+     139           7 :   if (custom_config_path != "") {
+     140           7 :     param_loader.addYamlFile(custom_config_path);
+     141             :   }
+     142             : 
+     143           7 :   if (platform_config_path != "") {
+     144           7 :     param_loader.addYamlFile(platform_config_path);
+     145             :   }
+     146             : 
+     147           7 :   param_loader.addYamlFileFromParam("private_config");
+     148           7 :   param_loader.addYamlFileFromParam("public_config");
+     149           7 :   param_loader.addYamlFileFromParam("public_gains");
+     150             : 
+     151          14 :   const std::string yaml_prefix = "mrs_uav_managers/gain_manager/";
+     152             : 
+     153             :   // params passed from the launch file are not prefixed
+     154           7 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     155             : 
+     156           7 :   param_loader.loadParam(yaml_prefix + "gains", _gain_names_);
+     157             : 
+     158           7 :   param_loader.loadParam(yaml_prefix + "estimator_types", _current_state_estimators_);
+     159             : 
+     160           7 :   param_loader.loadParam(yaml_prefix + "rate", _gain_management_rate_);
+     161           7 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     162             : 
+     163             :   // | ------------------- scope timer logger ------------------- |
+     164             : 
+     165           7 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     166          21 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     167           7 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     168             : 
+     169           7 :   std::vector<std::string>::iterator it;
+     170             : 
+     171             :   // loading gain_names
+     172          21 :   for (it = _gain_names_.begin(); it != _gain_names_.end(); ++it) {
+     173             : 
+     174          14 :     ROS_INFO_STREAM("[GainManager]: loading gains '" << *it << "'");
+     175             : 
+     176          14 :     Gains_t new_gains;
+     177             : 
+     178          14 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kp", new_gains.kpxy);
+     179          14 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kv", new_gains.kvxy);
+     180          14 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/ka", new_gains.kaxy);
+     181          14 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib", new_gains.kibxy);
+     182          14 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw", new_gains.kiwxy);
+     183          14 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib_lim", new_gains.kibxy_lim);
+     184          14 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw_lim", new_gains.kiwxy_lim);
+     185             : 
+     186          14 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kp", new_gains.kpz);
+     187          14 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kv", new_gains.kvz);
+     188          14 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ka", new_gains.kaz);
+     189             : 
+     190          14 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_roll_pitch", new_gains.kqrp);
+     191          14 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_yaw", new_gains.kqy);
+     192             : 
+     193          14 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km", new_gains.km);
+     194          14 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km_lim", new_gains.km_lim);
+     195             : 
+     196          14 :     _gains_.insert(std::pair<std::string, Gains_t>(*it, new_gains));
+     197             :   }
+     198             : 
+     199             :   // loading the allowed gains lists
+     200          56 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     201             : 
+     202          49 :     std::vector<std::string> temp_vector;
+     203          49 :     param_loader.loadParam(yaml_prefix + "allowed_gains/" + *it, temp_vector);
+     204             : 
+     205          49 :     std::vector<std::string>::iterator it2;
+     206         147 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     207          98 :       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          49 :     _map_type_allowed_gains_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     214             :   }
+     215             : 
+     216             :   // loading the default gains
+     217          56 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     218             : 
+     219          49 :     std::string temp_str;
+     220          49 :     param_loader.loadParam(yaml_prefix + "default_gains/" + *it, temp_str);
+     221             : 
+     222          49 :     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          49 :     _map_type_default_gains_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     228             :   }
+     229             : 
+     230           7 :   ROS_INFO("[GainManager]: done loading dynamical params");
+     231             : 
+     232           7 :   current_gains_       = "";
+     233           7 :   last_estimator_name_ = "";
+     234             : 
+     235             :   // | ------------------------ services ------------------------ |
+     236             : 
+     237           7 :   service_server_set_gains_ = nh_.advertiseService("set_gains_in", &GainManager::callbackSetGains, this);
+     238             : 
+     239           7 :   service_client_set_gains_ = nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_gains_out");
+     240             : 
+     241             :   // | ----------------------- subscribers ---------------------- |
+     242             : 
+     243          14 :   mrs_lib::SubscribeHandlerOptions shopts;
+     244           7 :   shopts.nh                 = nh_;
+     245           7 :   shopts.node_name          = "GainManager";
+     246           7 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     247           7 :   shopts.threadsafe         = true;
+     248           7 :   shopts.autostart          = true;
+     249           7 :   shopts.queue_size         = 10;
+     250           7 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     251             : 
+     252           7 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     253           7 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     254             : 
+     255             :   // | ----------------------- publishers ----------------------- |
+     256             : 
+     257           7 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     258             : 
+     259             :   // | ------------------------- timers ------------------------- |
+     260             : 
+     261           7 :   timer_gain_management_ = nh_.createTimer(ros::Rate(_gain_management_rate_), &GainManager::timerGainManagement, this);
+     262           7 :   timer_diagnostics_     = nh_.createTimer(ros::Rate(_diagnostics_rate_), &GainManager::timerDiagnostics, this);
+     263             : 
+     264             :   // | ------------------------ profiler ------------------------ |
+     265             : 
+     266           7 :   profiler_ = mrs_lib::Profiler(nh_, "GainManager", _profiler_enabled_);
+     267             : 
+     268             :   // | ----------------------- finish init ---------------------- |
+     269             : 
+     270           7 :   if (!param_loader.loadedSuccessfully()) {
+     271           0 :     ROS_ERROR("[GainManager]: could not load all parameters!");
+     272           0 :     ros::shutdown();
+     273             :   }
+     274             : 
+     275           7 :   is_initialized_ = true;
+     276             : 
+     277           7 :   ROS_INFO("[GainManager]: initialized");
+     278             : 
+     279           7 :   ROS_DEBUG("[GainManager]: debug output is enabled");
+     280           7 : }
+     281             : 
+     282             : //}
+     283             : 
+     284             : // --------------------------------------------------------------
+     285             : // |                           methods                          |
+     286             : // --------------------------------------------------------------
+     287             : 
+     288             : /* setGains() //{ */
+     289             : 
+     290           7 : bool GainManager::setGains(std::string gains_name) {
+     291             : 
+     292           7 :   std::map<std::string, Gains_t>::iterator it;
+     293           7 :   it = _gains_.find(gains_name);
+     294             : 
+     295           7 :   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          14 :   dynamic_reconfigure::Config          conf;
+     301          14 :   dynamic_reconfigure::DoubleParameter param;
+     302             : 
+     303           7 :   param.name  = "kpxy";
+     304           7 :   param.value = it->second.kpxy;
+     305           7 :   conf.doubles.push_back(param);
+     306             : 
+     307           7 :   param.name  = "kvxy";
+     308           7 :   param.value = it->second.kvxy;
+     309           7 :   conf.doubles.push_back(param);
+     310             : 
+     311           7 :   param.name  = "kaxy";
+     312           7 :   param.value = it->second.kaxy;
+     313           7 :   conf.doubles.push_back(param);
+     314             : 
+     315           7 :   param.name  = "kq_roll_pitch";
+     316           7 :   param.value = it->second.kqrp;
+     317           7 :   conf.doubles.push_back(param);
+     318             : 
+     319           7 :   param.name  = "kibxy";
+     320           7 :   param.value = it->second.kibxy;
+     321           7 :   conf.doubles.push_back(param);
+     322             : 
+     323           7 :   param.name  = "kiwxy";
+     324           7 :   param.value = it->second.kiwxy;
+     325           7 :   conf.doubles.push_back(param);
+     326             : 
+     327           7 :   param.name  = "kibxy_lim";
+     328           7 :   param.value = it->second.kibxy_lim;
+     329           7 :   conf.doubles.push_back(param);
+     330             : 
+     331           7 :   param.name  = "kiwxy_lim";
+     332           7 :   param.value = it->second.kiwxy_lim;
+     333           7 :   conf.doubles.push_back(param);
+     334             : 
+     335           7 :   param.name  = "kpz";
+     336           7 :   param.value = it->second.kpz;
+     337           7 :   conf.doubles.push_back(param);
+     338             : 
+     339           7 :   param.name  = "kvz";
+     340           7 :   param.value = it->second.kvz;
+     341           7 :   conf.doubles.push_back(param);
+     342             : 
+     343           7 :   param.name  = "kaz";
+     344           7 :   param.value = it->second.kaz;
+     345           7 :   conf.doubles.push_back(param);
+     346             : 
+     347           7 :   param.name  = "kq_yaw";
+     348           7 :   param.value = it->second.kqy;
+     349           7 :   conf.doubles.push_back(param);
+     350             : 
+     351           7 :   param.name  = "km";
+     352           7 :   param.value = it->second.km;
+     353           7 :   conf.doubles.push_back(param);
+     354             : 
+     355           7 :   param.name  = "km_lim";
+     356           7 :   param.value = it->second.km_lim;
+     357           7 :   conf.doubles.push_back(param);
+     358             : 
+     359          14 :   dynamic_reconfigure::ReconfigureRequest  srv_req;
+     360          14 :   dynamic_reconfigure::ReconfigureResponse srv_resp;
+     361             : 
+     362           7 :   srv_req.config = conf;
+     363             : 
+     364          14 :   dynamic_reconfigure::Reconfigure reconf;
+     365           7 :   reconf.request = srv_req;
+     366             : 
+     367           7 :   ROS_INFO_THROTTLE(1.0, "[GainManager]: setting up gains for '%s'", gains_name.c_str());
+     368             : 
+     369           7 :   bool res = service_client_set_gains_.call(reconf);
+     370             : 
+     371           7 :   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           7 :     mrs_lib::set_mutexed(mutex_current_gains_, gains_name, current_gains_);
+     379             : 
+     380           7 :     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        1182 : void GainManager::timerGainManagement(const ros::TimerEvent& event) {
+     469             : 
+     470        1182 :   if (!is_initialized_) {
+     471         296 :     return;
+     472             :   }
+     473             : 
+     474        2364 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("gainManagementTimer", _gain_management_rate_, 0.01, event);
+     475        2364 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::gainManagementTimer", scope_timer_logger_, scope_timer_enabled_);
+     476             : 
+     477        1182 :   if (!sh_estimation_diag_.hasMsg()) {
+     478         296 :     return;
+     479             :   }
+     480             : 
+     481         886 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     482             : 
+     483         886 :   if (!sh_control_manager_diag_.hasMsg()) {
+     484           0 :     return;
+     485             :   }
+     486             : 
+     487        1772 :   auto control_manager_diagnostics = *sh_estimation_diag_.getMsg();
+     488             : 
+     489        1772 :   auto current_gains       = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     490         886 :   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         886 :   if (estimation_diagnostics.current_state_estimator != last_estimator_name) {
+     494             : 
+     495           7 :     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           7 :     std::map<std::string, std::string>::iterator it;
+     499           7 :     it = _map_type_default_gains_.find(estimation_diagnostics.current_state_estimator);
+     500             : 
+     501           7 :     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           7 :       if (stringInVector(current_gains, _map_type_allowed_gains_.at(estimation_diagnostics.current_state_estimator))) {
+     510             : 
+     511           0 :         last_estimator_name = estimation_diagnostics.current_state_estimator;
+     512             : 
+     513             :         // else, try to set the default gains
+     514             :       } else {
+     515             : 
+     516           7 :         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           7 :         if (setGains(it->second)) {
+     520             : 
+     521           7 :           last_estimator_name = estimation_diagnostics.current_state_estimator;
+     522             : 
+     523           7 :           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         886 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     534             : }
+     535             : 
+     536             : //}
+     537             : 
+     538             : /* dignosticsTimer() //{ */
+     539             : 
+     540         114 : void GainManager::timerDiagnostics(const ros::TimerEvent& event) {
+     541             : 
+     542         114 :   if (!is_initialized_) {
+     543          28 :     return;
+     544             :   }
+     545             : 
+     546         228 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     547         228 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     548             : 
+     549         114 :   if (!sh_estimation_diag_.hasMsg()) {
+     550          28 :     ROS_WARN_THROTTLE(10.0, "[GainManager]: can not do constraint management, missing estimator diagnostics!");
+     551          28 :     return;
+     552             :   }
+     553             : 
+     554          86 :   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         172 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     560             : 
+     561         172 :   auto current_gains = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     562             : 
+     563         172 :   mrs_msgs::GainManagerDiagnostics diagnostics;
+     564             : 
+     565          86 :   diagnostics.stamp        = ros::Time::now();
+     566          86 :   diagnostics.current_name = current_gains;
+     567          86 :   diagnostics.loaded       = _gain_names_;
+     568             : 
+     569             :   // get the available gains
+     570             :   {
+     571          86 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     572          86 :     it = _map_type_allowed_gains_.find(estimation_diagnostics.current_state_estimator);
+     573             : 
+     574          86 :     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          86 :       diagnostics.available = it->second;
+     579             :     }
+     580             :   }
+     581             : 
+     582             :   // get the current gain values
+     583             :   {
+     584          86 :     std::map<std::string, Gains_t>::iterator it;
+     585          86 :     it = _gains_.find(current_gains);
+     586             : 
+     587          86 :     diagnostics.current_values.kpxy = it->second.kpxy;
+     588          86 :     diagnostics.current_values.kvxy = it->second.kvxy;
+     589          86 :     diagnostics.current_values.kaxy = it->second.kaxy;
+     590             : 
+     591          86 :     diagnostics.current_values.kqrp = it->second.kqrp;
+     592             : 
+     593          86 :     diagnostics.current_values.kibxy     = it->second.kibxy;
+     594          86 :     diagnostics.current_values.kibxy_lim = it->second.kibxy_lim;
+     595             : 
+     596          86 :     diagnostics.current_values.kiwxy     = it->second.kiwxy;
+     597          86 :     diagnostics.current_values.kiwxy_lim = it->second.kiwxy_lim;
+     598             : 
+     599          86 :     diagnostics.current_values.kpz = it->second.kpz;
+     600          86 :     diagnostics.current_values.kvz = it->second.kvz;
+     601          86 :     diagnostics.current_values.kaz = it->second.kaz;
+     602             : 
+     603          86 :     diagnostics.current_values.kqy = it->second.kqy;
+     604             : 
+     605          86 :     diagnostics.current_values.km     = it->second.km;
+     606          86 :     diagnostics.current_values.km_lim = it->second.km_lim;
+     607             :   }
+     608             : 
+     609          86 :   ph_diagnostics_.publish(diagnostics);
+     610             : }
+     611             : 
+     612             : //}
+     613             : 
+     614             : // --------------------------------------------------------------
+     615             : // |                          routines                          |
+     616             : // --------------------------------------------------------------
+     617             : 
+     618             : /* stringInVector() //{ */
+     619             : 
+     620         154 : bool GainManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     621             : 
+     622         154 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     623           7 :     return false;
+     624             :   } else {
+     625         147 :     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           7 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::gain_manager::GainManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..99dde9c011 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html @@ -0,0 +1,179 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/gain_manager.cpp.gcov.png b/mrs_uav_managers/src/gain_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6ceeb4cc94ea7fff3233934c9b9015cf2640a8eb GIT binary patch literal 2109 zcmV-D2*US?P)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp8KD5nZ!^B+UfK=dl8xQFx63Mk7pEcF#5; z0<4&a#pC4X5sD{M91V*147;uaFX(zb4P$Myija4*w1M)%EY{6d%Yl7W0dD4rL(!Y3 z1zZ_|d(Py+KqhkZLx3OYJi~TysIfB$5&@ zR8!$Gr8vjnlBuUAZ?2yC$N?O8i_gJS(xZ@-CYKbKMB#x*o-}19tnvsccFTxROrqJA zGBTw{M*f(6!tf#$npc<@PY8p&3FD&|5%n6@(wrmAEuRUcLbKIRmIKHp!lq~Wwd?Bk zxx^^$yV^rjcd{+?zjB}>rgyD(tm#pTwn_46rKSn@=x zGljW(UvbYx0P2oCOWbAJ!0&SBq~*9n!emV;KobBbDdw#1(-eU>Ta{P425>zXaYgc; zDvv-xxtk&22!vX24FgUn9Gww6J=|>^HPn+%>H3URKs&mBV$Y?~j*^Lir&|nBv|P1- zqdXczt71z208onqJ+ucXqsNfhTmlrbwU@gMbgv}4R-R&;M>X5E_Zauf1I2R+AA8mi z!kMuru&L5FN2Rvv!{j3r?)ulk)RS(8^?d?$GOxI(7>%PmFv%djPKx#1@_4rwXvh0; zKEIxaf9w6~I1=Ea(oWz}0|cP_&N-P3M~gbAxY}_mJYMCv=HoTFc#f+(=fWZuW*t}{ z)5~5ESoa>Lmp!DtM_u7PS?JNhoS9Ag)55#SE*U`U3^gFPYx&L8L_OFhQvdAQqPX`` zls^zF+)?eso>bx`n#^<~MGuUBFx5i>wWCPxQ{-JYfsSHhPs$TTB5D9KTe>(a*JQvc z606jJ+^*$M?Zd7s?c8%b%I;`Z)Tp7tX~XClVWfgoqHPr!3*1ix(lwT|jPqD358GN? z^6A>n*w6RFz5%9T)^z;_@}}#a`8o*D%zKy$$4&u=CBO(p?NW4PJ7Qs`6dhe(a@bUD zTLdPbD~jU)kV&zVN6EVHy&N<4jchSB7gyoRyHKEBmsw3B*oCP~KfU>s!<58GLPS+pjkpYZ6veN7S zlgD~xLB=D+BhlIvR0Ypc0$*o~G=3!@DhzdjN64)s%0ut5EC@T)L}}mJFv=_CrO;YvfX9 zuOf~ptR3Os{4w*06pz|S{Nx_MBMMlhJtVb{P_q;t?ws@KERk%X4pPim^uOJK>L|tI zAb3N8ZHmvWa@$>cJ_uG%o;rizkb*Dg0 zSTqR}7U>M1V$!_l9*+JIfqfKzhY8KB>8h=(Q|==j|21KlJVsl;qG7F-$0`1z;mYMv zJ`b;sLcsY?8-=ZOMtT(f=Cwj`q4M$*NZVGCuU0`$Ai>FDT%-?OfvPU>UHLqR;&nA~ zaiNWFZ@{u0&np3BHl$1fKPvsOdSHQXUgZOMWOm(G9Kr}I*Btq#Oy-szxl8H`wyD}8 z?cEHN^HbGIy0->t4Uys&zMH{+F_U>~+fd6mK7ifTXvKNlfK=`e^XjZgh7<`FtnNBY zh?YN;1rVmVn69mht+QNNVt{CW5gRmk`i4w>5zuiqpWw!Y9NKy0f zzOIvbfgB1<%cQ)K)ubKwG;?Xe`G+5!sxy#^+b|!eab$;O-^)&M58g!t!y>bDX&k$e zJ_s)yo&LzKR%Ev;*9tOOIG%?qpckJc{^j8x$-@&D-FFvcReVeDpVi|??RD6DFb{Bm z;7c#RLmn=(`tTGhNqDFC + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:931169055.1 %
Date:2023-11-30 22:31:42Functions:537372.6 %
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 +
64.4%64.4%
+ 		64.4 %38 / 5950.0 %10 / 20
<unnamed>64.4 %38 / 5950.0 %10 / 20
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
<unnamed>65.2 %148 / 22775.0 %6 / 8
uav_manager.cpp +
47.2%47.2%
+ 		47.2 %543 / 115181.6 %31 / 38
<unnamed>47.2 %543 / 115181.6 %31 / 38
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
<unnamed>79.8 %202 / 25385.7 %6 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-detail-sort-l.html b/mrs_uav_managers/src/index-detail-sort-l.html new file mode 100644 index 0000000000..d4ce119a4a --- /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:931169055.1 %
Date:2023-11-30 22:31:42Functions:537372.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
uav_manager.cpp +
47.2%47.2%
+ 		47.2 %543 / 115181.6 %31 / 38
<unnamed>47.2 %543 / 115181.6 %31 / 38
null_tracker.cpp +
64.4%64.4%
+ 		64.4 %38 / 5950.0 %10 / 20
<unnamed>64.4 %38 / 5950.0 %10 / 20
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
<unnamed>65.2 %148 / 22775.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
<unnamed>79.8 %202 / 25385.7 %6 / 7
+
+
+ + + + +
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..148ae41ab4 --- /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:931169055.1 %
Date:2023-11-30 22:31:42Functions:537372.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
<unnamed>65.2 %148 / 22775.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
<unnamed>79.8 %202 / 25385.7 %6 / 7
null_tracker.cpp +
64.4%64.4%
+ 		64.4 %38 / 5950.0 %10 / 20
<unnamed>64.4 %38 / 5950.0 %10 / 20
uav_manager.cpp +
47.2%47.2%
+ 		47.2 %543 / 115181.6 %31 / 38
<unnamed>47.2 %543 / 115181.6 %31 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-sort-f.html b/mrs_uav_managers/src/index-sort-f.html new file mode 100644 index 0000000000..3c1b139dc2 --- /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:931169055.1 %
Date:2023-11-30 22:31:42Functions:537372.6 %
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 +
64.4%64.4%
+ 		64.4 %38 / 5950.0 %10 / 20
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
uav_manager.cpp +
47.2%47.2%
+ 		47.2 %543 / 115181.6 %31 / 38
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-sort-l.html b/mrs_uav_managers/src/index-sort-l.html new file mode 100644 index 0000000000..716a6c4621 --- /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:931169055.1 %
Date:2023-11-30 22:31:42Functions:537372.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
uav_manager.cpp +
47.2%47.2%
+ 		47.2 %543 / 115181.6 %31 / 38
null_tracker.cpp +
64.4%64.4%
+ 		64.4 %38 / 5950.0 %10 / 20
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
+
+
+ + + + +
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..969f34f3d8 --- /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:931169055.1 %
Date:2023-11-30 22:31:42Functions:537372.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
null_tracker.cpp +
64.4%64.4%
+ 		64.4 %38 / 5950.0 %10 / 20
uav_manager.cpp +
47.2%47.2%
+ 		47.2 %543 / 115181.6 %31 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html b/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..c645090916 --- /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:385964.4 %
Date:2023-11-30 22:31:42Functions:102050.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::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> 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::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
(anonymous namespace)::ProxyExec0::ProxyExec0()7
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>)7
mrs_uav_managers::NullTracker::~NullTracker()7
mrs_uav_managers::NullTracker::~NullTracker().27
mrs_uav_managers::NullTracker::deactivate()21
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)21
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)24
mrs_uav_managers::NullTracker::getStatus()346
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)6807
+
+
+ + + +
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..2b97bfa5e8 --- /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:385964.4 %
Date:2023-11-30 22:31:42Functions:102050.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::NullTracker::deactivate()21
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>)7
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&)24
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
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&)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::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)6807
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)21
mrs_uav_managers::NullTracker::getStatus()346
mrs_uav_managers::NullTracker::~NullTracker()7
mrs_uav_managers::NullTracker::~NullTracker().27
+
+
+ + + +
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..4c9a411526 --- /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:385964.4 %
Date:2023-11-30 22:31:42Functions:102050.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          14 :   ~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           7 : 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          14 :   ros::NodeHandle nh_(parent_nh, "null_tracker");
+      59             : 
+      60           7 :   ros::Time::waitForValid();
+      61             : 
+      62           7 :   is_initialized = true;
+      63             : 
+      64           7 :   this->common_handlers = common_handlers;
+      65             : 
+      66           7 :   ROS_INFO("[NullTracker]: initialized");
+      67             : 
+      68          14 :   return true;
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* //{ activate() */
+      74             : 
+      75          21 : std::tuple<bool, std::string> NullTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+      76             : 
+      77          21 :   std::stringstream ss;
+      78          21 :   ss << "activated";
+      79             : 
+      80          21 :   ROS_INFO_STREAM("[NullTracker]: " << ss.str());
+      81          21 :   is_active = true;
+      82             : 
+      83          42 :   return std::tuple(true, ss.str());
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* //{ deactivate() */
+      89             : 
+      90          21 : void NullTracker::deactivate(void) {
+      91             : 
+      92          21 :   ROS_INFO("[NullTracker]: deactivated");
+      93          21 :   is_active = false;
+      94          21 : }
+      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        6807 : 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        6807 :   return {};
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* //{ getStatus() */
+     125             : 
+     126         346 : const mrs_msgs::TrackerStatus NullTracker::getStatus() {
+     127             : 
+     128         346 :   mrs_msgs::TrackerStatus tracker_status;
+     129             : 
+     130         346 :   tracker_status.active            = is_active;
+     131         346 :   tracker_status.callbacks_enabled = callbacks_enabled;
+     132             : 
+     133         346 :   return tracker_status;
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* //{ enableCallbacks() */
+     139             : 
+     140           1 : const std_srvs::SetBoolResponse::ConstPtr NullTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     141             : 
+     142           2 :   std_srvs::SetBoolResponse res;
+     143             : 
+     144           1 :   std::stringstream ss;
+     145             : 
+     146           1 :   if (cmd->data != callbacks_enabled) {
+     147             : 
+     148           1 :     callbacks_enabled = cmd->data;
+     149             : 
+     150           1 :     ss << "callbacks " << (callbacks_enabled ? "enabled" : "disabled");
+     151             : 
+     152           1 :     ROS_DEBUG_STREAM("[NullTracker]: " << ss.str());
+     153             : 
+     154             :   } else {
+     155             : 
+     156           0 :     ss << "callbacks were already " << (callbacks_enabled ? "enabled" : "disabled");
+     157             :   }
+     158             : 
+     159           1 :   res.message = ss.str();
+     160           1 :   res.success = true;
+     161             : 
+     162           3 :   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           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr NullTracker::setTrajectoryReference([
+     187             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     188           0 :   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          24 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr NullTracker::setConstraints([
+     238             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     239             : 
+     240          24 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : }  // namespace mrs_uav_managers
+     246             : 
+     247             : #include <pluginlib/class_list_macros.h>
+     248           7 : 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..c182b92810fc7b98104ce4869887f19f647d19d2 GIT binary patch literal 831 zcmV-F1Hk-=P)tii$Ia)-$Uy3nzo^-QDCd1==$uyFp=*tmMg(Vp9ZU!L0SxV}b zJ%m^T1`UXpo7Q|(W`GYe16iR_vD?ns5EZ~R%z8pCT)Y3b2=pFQEhg3IHAJh)0#9%h z@g#5nz(+nJ5y_$F{Kx`oF7QAyfadb?>CSweyVTYw zp4V9)OX6}ytXBrh;;n1Y8hCABn3;Nmt``n2$3yTR z-i&>XH#f}@xJ&_SHCjvQYxHrN2v-|dZS!+ToyNj~9k4L&1Rz`9NLQC!V!5Rt*J-Yt zSN6Y#Eu82HatGR}6x|M3Z8`*{#k0_dtk{e@#z{{ss;e-2vxZ9$&Z`vP@>I~s-Wj%! zFYb)cIa8@^`)okgb-_d}+X=<5#=s)N1Np0dgU2xBh=lph%yyUbn{D$vLg|=CE!vON zoC?yFY^fndK(@FV&dBSoYPeLPP;eu_rhsbRR>Qr%=hGfgWYGa29TjG4XUv1P34jE4 z6+L1Bgr#x%O);-;gvYA#z-o=f(Uq^!#a|eICRB5U#^5A1XVz)F261b{j<|fdvSMoW zF=)e_t+<73+V6bhvMk%j_2!iEoW~Y^`XK&tN%=V*1&I95qfD9nc^-v^T>L4|bxezo z0Aut}Gy9V%d7MfJb^3@899^6~jD`=(pKH8^6Cfxse{O^O^bfo`g=f>T@%R7$002ov JPDHLkV1f?#gGK-V literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/transform_manager/index-detail-sort-f.html b/mrs_uav_managers/src/transform_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..9b8ef5433e --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/index-detail-sort-f.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:27543663.1 %
Date:2023-11-30 22:31:42Functions: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
transform_manager.cpp +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 13
<unnamed>63.1 %275 / 43676.9 %10 / 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..0bdddc982a --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions: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
transform_manager.cpp +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 13
<unnamed>63.1 %275 / 43676.9 %10 / 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..4024cf22c6 --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions: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
transform_manager.cpp +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 13
<unnamed>63.1 %275 / 43676.9 %10 / 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..a36e1483c6 --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions: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
transform_manager.cpp +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 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..05b1a3f85a --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions: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
transform_manager.cpp +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 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..e32abf9a6e --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions: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
transform_manager.cpp +
63.1%63.1%
+ 		63.1 %275 / 43676.9 %10 / 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..e970131a50 --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions:101376.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)0
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const0
mrs_uav_managers::transform_manager::TransformManager::getName[abi:cxx11]() const0
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::transform_manager::TransformManager::onInit()7
mrs_uav_managers::transform_manager::TransformManager::TransformManager()7
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const99
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)5229
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)5766
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)8866
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)9442
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)27254
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)27254
+
+
+ + + +
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..b93fc8a598 --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions:101376.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)5766
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)0
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)8866
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)5229
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)9442
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)27254
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)27254
mrs_uav_managers::transform_manager::TransformManager::onInit()7
mrs_uav_managers::transform_manager::TransformManager::TransformManager()7
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const99
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const0
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..cf16150e67 --- /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:27543663.1 %
Date:2023-11-30 22:31:42Functions:101376.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* //{ includes */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <nodelet/nodelet.h>
+       5             : 
+       6             : #include <mrs_lib/param_loader.h>
+       7             : #include <mrs_lib/subscribe_handler.h>
+       8             : #include <mrs_lib/publisher_handler.h>
+       9             : #include <mrs_lib/attitude_converter.h>
+      10             : #include <mrs_lib/transformer.h>
+      11             : #include <mrs_lib/transform_broadcaster.h>
+      12             : #include <mrs_lib/gps_conversions.h>
+      13             : 
+      14             : #include <mrs_msgs/UavState.h>
+      15             : #include <mrs_msgs/Float64Stamped.h>
+      16             : #include <mrs_msgs/HwApiAltitude.h>
+      17             : #include <mrs_msgs/RtkGps.h>
+      18             : 
+      19             : #include <tf2_ros/transform_broadcaster.h>
+      20             : #include <tf2_ros/static_transform_broadcaster.h>
+      21             : #include <geometry_msgs/TransformStamped.h>
+      22             : 
+      23             : #include <sensor_msgs/NavSatFix.h>
+      24             : 
+      25             : #include <nav_msgs/Odometry.h>
+      26             : 
+      27             : #include <memory>
+      28             : #include <string>
+      29             : 
+      30             : #include <mrs_uav_managers/estimation_manager/support.h>
+      31             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      32             : #include <transform_manager/tf_source.h>
+      33             : #include <transform_manager/tf_mapping_origin.h>
+      34             : 
+      35             : /*//}*/
+      36             : 
+      37             : namespace mrs_uav_managers
+      38             : {
+      39             : 
+      40             : namespace transform_manager
+      41             : {
+      42             : 
+      43             : /*//{ class TransformManager */
+      44             : class TransformManager : public nodelet::Nodelet {
+      45             : 
+      46             :   using Support = estimation_manager::Support;
+      47             : 
+      48             : public:
+      49           7 :   TransformManager() {
+      50           7 :     ch_ = std::make_shared<estimation_manager::CommonHandlers_t>();
+      51             : 
+      52           7 :     ch_->nodelet_name = nodelet_name_;
+      53           7 :     ch_->package_name = package_name_;
+      54           7 :   }
+      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           7 : void TransformManager::onInit() {
+     145             : 
+     146           7 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     147             : 
+     148           7 :   ros::Time::waitForValid();
+     149             : 
+     150           7 :   ROS_INFO("[%s]: initializing", getPrintName().c_str());
+     151             : 
+     152           7 :   broadcaster_ = std::make_shared<mrs_lib::TransformBroadcaster>();
+     153             : 
+     154           7 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getPrintName());
+     155           7 :   ch_->transformer->retryLookupNewest(true);
+     156             : 
+     157          14 :   mrs_lib::ParamLoader param_loader(nh_, getPrintName());
+     158             : 
+     159           7 :   param_loader.loadParam("custom_config", _custom_config_);
+     160           7 :   param_loader.loadParam("platform_config", _platform_config_);
+     161           7 :   param_loader.loadParam("world_config", _world_config_);
+     162             : 
+     163           7 :   if (_custom_config_ != "") {
+     164           7 :     param_loader.addYamlFile(_custom_config_);
+     165             :   }
+     166             : 
+     167           7 :   if (_platform_config_ != "") {
+     168           7 :     param_loader.addYamlFile(_platform_config_);
+     169             :   }
+     170             : 
+     171           7 :   if (_world_config_ != "") {
+     172           7 :     param_loader.addYamlFile(_world_config_);
+     173             :   }
+     174             : 
+     175           7 :   param_loader.addYamlFileFromParam("private_config");
+     176           7 :   param_loader.addYamlFileFromParam("public_config");
+     177           7 :   param_loader.addYamlFileFromParam("estimators_config");
+     178             : 
+     179          14 :   const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+     180             : 
+     181           7 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     182             : 
+     183             :   /*//{ initialize scope timer */
+     184           7 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", ch_->scope_timer.enabled);
+     185          14 :   std::string       filepath;
+     186          14 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/transform_manager_scope_timer.txt";
+     187           7 :   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           7 :   bool   is_origin_param_ok = true;
+     193           7 :   double world_origin_x     = 0;
+     194           7 :   double world_origin_y     = 0;
+     195             : 
+     196           7 :   param_loader.loadParam("world_origin/units", world_origin_units_);
+     197             : 
+     198           7 :   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           7 :   } else if (Support::toLowercase(world_origin_units_) == "latlon") {
+     204             :     double lat, lon;
+     205           7 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getPrintName().c_str());
+     206           7 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     207           7 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     208           7 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     209           7 :     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           7 :   world_origin_.x = world_origin_x;
+     217           7 :   world_origin_.y = world_origin_y;
+     218           7 :   world_origin_.z = 0;
+     219             : 
+     220           7 :   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          14 :   std::string local_origin_parent_frame_id;
+     228           7 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/parent", local_origin_parent_frame_id);
+     229           7 :   ns_local_origin_parent_frame_id_ = ch_->uav_name + "/" + local_origin_parent_frame_id;
+     230             : 
+     231          14 :   std::string local_origin_child_frame_id;
+     232           7 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/child", local_origin_child_frame_id);
+     233           7 :   ns_local_origin_child_frame_id_ = ch_->uav_name + "/" + local_origin_child_frame_id;
+     234             : 
+     235           7 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/enabled", publish_local_origin_tf_);
+     236             :   /*//}*/
+     237             : 
+     238             :   /*//{ load stable_origin parameters */
+     239          14 :   std::string stable_origin_parent_frame_id;
+     240           7 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/parent", stable_origin_parent_frame_id);
+     241           7 :   ns_stable_origin_parent_frame_id_ = ch_->uav_name + "/" + stable_origin_parent_frame_id;
+     242             : 
+     243          14 :   std::string stable_origin_child_frame_id;
+     244           7 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/child", stable_origin_child_frame_id);
+     245           7 :   ns_stable_origin_child_frame_id_ = ch_->uav_name + "/" + stable_origin_child_frame_id;
+     246             : 
+     247           7 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/enabled", publish_stable_origin_tf_);
+     248             :   /*//}*/
+     249             : 
+     250             :   /*//{ load fixed_origin parameters */
+     251          14 :   std::string fixed_origin_parent_frame_id;
+     252           7 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/parent", fixed_origin_parent_frame_id);
+     253           7 :   ns_fixed_origin_parent_frame_id_ = ch_->uav_name + "/" + fixed_origin_parent_frame_id;
+     254             : 
+     255          14 :   std::string fixed_origin_child_frame_id;
+     256           7 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/child", fixed_origin_child_frame_id);
+     257           7 :   ns_fixed_origin_child_frame_id_ = ch_->uav_name + "/" + fixed_origin_child_frame_id;
+     258             : 
+     259           7 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/enabled", publish_fixed_origin_tf_);
+     260             :   /*//}*/
+     261             : 
+     262             :   /*//{ load fcu_untilted parameters */
+     263          14 :   std::string fcu_frame_id;
+     264           7 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/parent", fcu_frame_id);
+     265           7 :   ch_->frames.fcu    = fcu_frame_id;
+     266           7 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + fcu_frame_id;
+     267             : 
+     268          14 :   std::string fcu_untilted_frame_id;
+     269           7 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/child", fcu_untilted_frame_id);
+     270           7 :   ch_->frames.fcu_untilted    = fcu_untilted_frame_id;
+     271           7 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + fcu_untilted_frame_id;
+     272             : 
+     273           7 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/enabled", publish_fcu_untilted_tf_);
+     274             :   /*//}*/
+     275             : 
+     276             :   /*//{ load amsl_origin parameters*/
+     277          14 :   std::string amsl_parent_frame_id, amsl_child_frame_id;
+     278           7 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/enabled", publish_amsl_origin_tf_);
+     279           7 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/parent", amsl_parent_frame_id);
+     280           7 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/child", amsl_child_frame_id);
+     281           7 :   ch_->frames.amsl                = amsl_child_frame_id;
+     282           7 :   ch_->frames.ns_amsl             = ch_->uav_name + "/" + amsl_child_frame_id;
+     283           7 :   ns_amsl_origin_parent_frame_id_ = ch_->uav_name + "/" + amsl_parent_frame_id;
+     284           7 :   ns_amsl_origin_child_frame_id_  = ch_->uav_name + "/" + amsl_child_frame_id;
+     285             : 
+     286             :   /*//}*/
+     287             : 
+     288           7 :   param_loader.loadParam(yaml_prefix + "rtk_antenna/frame_id", ch_->frames.rtk_antenna);
+     289           7 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     290             : 
+     291           7 :   param_loader.loadParam("mrs_uav_managers/estimation_manager/state_estimators", estimator_names_);
+     292           7 :   param_loader.loadParam(yaml_prefix + "tf_sources", tf_source_names_);
+     293             : 
+     294           7 :   param_loader.loadParam(yaml_prefix + "utm_source_priority", utm_source_priority_list_);
+     295          31 :   for (auto utm_source : utm_source_priority_list_) {
+     296          28 :     if (Support::isStringInVector(utm_source, estimator_names_)) {
+     297           4 :       ROS_INFO("[%s]: the source for utm_origin and world origin is: %s", getPrintName().c_str(), utm_source.c_str());
+     298           4 :       utm_source_name_ = utm_source;
+     299           4 :       break;
+     300             :     }
+     301             :   }
+     302             : 
+     303             :   /*//{ initialize tf sources */
+     304           7 :   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          14 :   for (int i = 0; i < int(estimator_names_.size()); i++) {
+     334             : 
+     335          14 :     const std::string estimator_name = estimator_names_[i];
+     336           7 :     const bool        is_utm_source  = estimator_name == utm_source_name_;
+     337           7 :     ROS_INFO("[%s]: loading tf source of estimator: %s", getPrintName().c_str(), estimator_name.c_str());
+     338             : 
+     339           7 :     auto estimator_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/" + estimator_name);
+     340             : 
+     341           7 :     if (_custom_config_ != "") {
+     342           7 :       estimator_param_loader->addYamlFile(_custom_config_);
+     343             :     }
+     344             : 
+     345           7 :     if (_platform_config_ != "") {
+     346           7 :       estimator_param_loader->addYamlFile(_platform_config_);
+     347             :     }
+     348             : 
+     349           7 :     if (_world_config_ != "") {
+     350           7 :       estimator_param_loader->addYamlFile(_world_config_);
+     351             :     }
+     352             : 
+     353           7 :     estimator_param_loader->addYamlFileFromParam("private_config");
+     354           7 :     estimator_param_loader->addYamlFileFromParam("public_config");
+     355           7 :     estimator_param_loader->addYamlFileFromParam("estimators_config");
+     356             : 
+     357           7 :     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           7 :   param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled, false);
+     363             : 
+     364           7 :   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          14 :   mrs_lib::SubscribeHandlerOptions shopts;
+     391           7 :   shopts.nh                 = nh_;
+     392           7 :   shopts.node_name          = getPrintName();
+     393           7 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     394           7 :   shopts.threadsafe         = true;
+     395           7 :   shopts.autostart          = true;
+     396           7 :   shopts.queue_size         = 10;
+     397           7 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     398             : 
+     399           7 :   sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &TransformManager::callbackUavState, this);
+     400             : 
+     401           7 :   sh_height_agl_ = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_agl_in", &TransformManager::callbackHeightAgl, this);
+     402             : 
+     403           7 :   sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in", &TransformManager::callbackAltitudeAmsl, this);
+     404             : 
+     405             :   sh_hw_api_orientation_ =
+     406           7 :       mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "orientation_in", &TransformManager::callbackHwApiOrientation, this);
+     407             : 
+     408           7 :   if (utm_source_name_ == "rtk" || utm_source_name_ == "rtk_garmin") {
+     409           0 :     sh_rtk_gps_ = mrs_lib::SubscribeHandler<mrs_msgs::RtkGps>(shopts, "rtk_gps_in", &TransformManager::callbackRtkGps, this);
+     410             :   } else {
+     411           7 :     sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &TransformManager::callbackGnss, this);
+     412             :   }
+     413             :   /*//}*/
+     414             : 
+     415           7 :   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           7 :   is_initialized_ = true;
+     421           7 :   ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     422           7 : }
+     423             : /*//}*/
+     424             : 
+     425             : /*//{ callbackUavState() */
+     426             : 
+     427        8866 : void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+     428             : 
+     429        8866 :   if (!is_initialized_) {
+     430           0 :     return;
+     431             :   }
+     432             : 
+     433       17732 :   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        8866 :   if (!is_first_frame_id_set_) {
+     437           7 :     first_frame_id_                = msg->header.frame_id;
+     438           7 :     last_frame_id_                 = msg->header.frame_id;
+     439           7 :     pose_fixed_                    = msg->pose;
+     440           7 :     pose_fixed_diff_.orientation.w = 1;
+     441           7 :     is_first_frame_id_set_         = true;
+     442             :   }
+     443             : 
+     444        8866 :   if (publish_local_origin_tf_) {
+     445             :     /*//{ publish local_origin tf*/
+     446        8866 :     geometry_msgs::TransformStamped tf_msg;
+     447        8866 :     tf_msg.header.stamp    = msg->header.stamp;
+     448        8866 :     tf_msg.header.frame_id = ns_local_origin_parent_frame_id_;
+     449        8866 :     tf_msg.child_frame_id  = ns_local_origin_child_frame_id_;
+     450             : 
+     451             :     // transform pose to first frame_id
+     452        8866 :     geometry_msgs::PoseStamped pose;
+     453        8866 :     pose.header = msg->header;
+     454        8866 :     pose.pose   = msg->pose;
+     455             : 
+     456        8866 :     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       17732 :     auto res = ch_->transformer->transformSingle(pose, first_frame_id_.substr(0, first_frame_id_.find("_origin")) + "_local_origin");
+     463             : 
+     464        8866 :     if (res) {
+     465        8866 :       const tf2::Transform      tf       = Support::tf2FromPose(res->pose);
+     466        8866 :       const tf2::Transform      tf_inv   = tf.inverse();
+     467        8866 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     468        8866 :       tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     469        8866 :       tf_msg.transform.rotation          = pose_inv.orientation;
+     470             : 
+     471        8866 :       if (Support::noNans(tf_msg)) {
+     472             :         try {
+     473        8866 :           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        8866 :       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        8866 :   if (publish_stable_origin_tf_) {
+     492             :     /*//{ publish stable_origin tf*/
+     493        8866 :     geometry_msgs::TransformStamped tf_msg;
+     494        8866 :     tf_msg.header.stamp    = msg->header.stamp;
+     495        8866 :     tf_msg.header.frame_id = ns_stable_origin_parent_frame_id_;
+     496        8866 :     tf_msg.child_frame_id  = ns_stable_origin_child_frame_id_;
+     497             : 
+     498             :     // transform pose to first frame_id
+     499        8866 :     geometry_msgs::PoseStamped pose;
+     500        8866 :     pose.header = msg->header;
+     501        8866 :     pose.pose   = msg->pose;
+     502        8866 :     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        8866 :     auto res = ch_->transformer->transformSingle(pose, first_frame_id_);
+     509             : 
+     510        8866 :     if (res) {
+     511        8866 :       const tf2::Transform      tf       = Support::tf2FromPose(res->pose);
+     512        8866 :       const tf2::Transform      tf_inv   = tf.inverse();
+     513        8866 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     514        8866 :       tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     515        8866 :       tf_msg.transform.rotation          = pose_inv.orientation;
+     516             : 
+     517        8866 :       if (Support::noNans(tf_msg)) {
+     518             :         try {
+     519        8866 :           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        8866 :       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        8866 :   if (publish_fixed_origin_tf_) {
+     538             :     /*//{ publish fixed_origin tf*/
+     539        8866 :     if (msg->header.frame_id != last_frame_id_) {
+     540           0 :       ROS_WARN("[%s]: Detected estimator change from %s to %s. Updating offset for fixed origin.", getPrintName().c_str(), last_frame_id_.c_str(),
+     541             :                msg->header.frame_id.c_str());
+     542             : 
+     543           0 :       pose_fixed_diff_ = Support::getPoseDiff(msg->pose, pose_fixed_);
+     544             :     }
+     545             : 
+     546             : 
+     547        8866 :     pose_fixed_ = Support::applyPoseDiff(msg->pose, pose_fixed_diff_);
+     548             : 
+     549       17732 :     geometry_msgs::TransformStamped tf_msg;
+     550        8866 :     tf_msg.header.stamp    = msg->header.stamp;
+     551        8866 :     tf_msg.header.frame_id = ns_fixed_origin_parent_frame_id_;
+     552        8866 :     tf_msg.child_frame_id  = ns_fixed_origin_child_frame_id_;
+     553             : 
+     554        8866 :     const tf2::Transform      tf       = Support::tf2FromPose(pose_fixed_);
+     555        8866 :     const tf2::Transform      tf_inv   = tf.inverse();
+     556        8866 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     557        8866 :     tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     558        8866 :     tf_msg.transform.rotation          = pose_inv.orientation;
+     559             : 
+     560        8866 :     if (Support::noNans(tf_msg)) {
+     561             :       try {
+     562        8866 :         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        8866 :     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        8866 :   if (msg->header.frame_id != last_frame_id_) {
+     578           0 :     const std::string last_estimator_name    = Support::frameIdToEstimatorName(last_frame_id_);
+     579           0 :     const std::string current_estimator_name = Support::frameIdToEstimatorName(msg->header.frame_id);
+     580             : 
+     581           0 :     ROS_INFO("[%s]: Detected estimator switch: %s -> %s", getPrintName().c_str(), last_estimator_name.c_str(), current_estimator_name.c_str());
+     582             : 
+     583           0 :     bool   valid_utm_source_found = false;
+     584             :     size_t potential_utm_source_index;
+     585             : 
+     586           0 :     for (size_t i = 0; i < tf_sources_.size(); i++) {
+     587             : 
+     588             :       // first check if tf source can publish utm origin and is not the switched-from estimator
+     589           0 :       if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() != last_estimator_name) {
+     590             : 
+     591           0 :         valid_utm_source_found     = true;
+     592           0 :         potential_utm_source_index = i;
+     593             : 
+     594             :         // check if switched-to estimator is utm_based, if so, use it
+     595           0 :         if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() == current_estimator_name) {
+     596           0 :           potential_utm_source_index = i;
+     597           0 :           break;
+     598             :         }
+     599             :       }
+     600             :     }
+     601             : 
+     602             : 
+     603             :     // if we found a valid utm source, use it, otherwise stay with the switched-from estimator
+     604           0 :     if (valid_utm_source_found) {
+     605             : 
+     606             :       // stop all estimators from publishing utm source
+     607           0 :       for (size_t i = 0; i < tf_sources_.size(); i++) {
+     608           0 :         if (tf_sources_.at(i)->getIsUtmSource()) {
+     609           0 :           tf_sources_.at(i)->setIsUtmSource(false);
+     610           0 :           ROS_INFO("[%s]: setting is_utm_source of estimator %s to false", getPrintName().c_str(), last_estimator_name.c_str());
+     611             :         }
+     612             :       }
+     613             : 
+     614           0 :       tf_sources_.at(potential_utm_source_index)->setIsUtmSource(true);
+     615           0 :       ROS_INFO("[%s]: setting is_utm_source of estimator %s to true", getPrintName().c_str(), current_estimator_name.c_str());
+     616             :     }
+     617             :   }
+     618             :   /*//}*/
+     619             : 
+     620        8866 :   last_frame_id_ = msg->header.frame_id;
+     621             : }
+     622             : /*//}*/
+     623             : 
+     624             : /*//{ callbackHeightAgl() */
+     625             : 
+     626        5229 : void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg) {
+     627             : 
+     628        5229 :   if (!is_initialized_) {
+     629           0 :     return;
+     630             :   }
+     631             : 
+     632       15687 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackHeightAgl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     633             : 
+     634       10458 :   geometry_msgs::TransformStamped tf_msg;
+     635        5229 :   tf_msg.header.stamp    = msg->header.stamp;
+     636        5229 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     637        5229 :   tf_msg.child_frame_id  = msg->header.frame_id;
+     638             : 
+     639        5229 :   tf_msg.transform.translation.x = 0;
+     640        5229 :   tf_msg.transform.translation.y = 0;
+     641        5229 :   tf_msg.transform.translation.z = -msg->value;
+     642        5229 :   tf_msg.transform.rotation.x    = 0;
+     643        5229 :   tf_msg.transform.rotation.y    = 0;
+     644        5229 :   tf_msg.transform.rotation.z    = 0;
+     645        5229 :   tf_msg.transform.rotation.w    = 1;
+     646             : 
+     647        5229 :   if (Support::noNans(tf_msg)) {
+     648             :     try {
+     649        5229 :       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        5229 :   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        9442 : void TransformManager::callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg) {
+     666             : 
+     667        9442 :   if (!is_initialized_) {
+     668           0 :     return;
+     669             :   }
+     670             : 
+     671       28326 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackAltitudeAmsl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     672             : 
+     673       18884 :   geometry_msgs::TransformStamped tf_msg;
+     674        9442 :   tf_msg.header.stamp    = msg->stamp;
+     675        9442 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     676        9442 :   tf_msg.child_frame_id  = ch_->frames.ns_amsl;
+     677             : 
+     678        9442 :   tf_msg.transform.translation.x = 0;
+     679        9442 :   tf_msg.transform.translation.y = 0;
+     680        9442 :   tf_msg.transform.translation.z = -msg->amsl;
+     681        9442 :   tf_msg.transform.rotation.x    = 0;
+     682        9442 :   tf_msg.transform.rotation.y    = 0;
+     683        9442 :   tf_msg.transform.rotation.z    = 0;
+     684        9442 :   tf_msg.transform.rotation.w    = 1;
+     685             : 
+     686        9442 :   if (Support::noNans(tf_msg)) {
+     687             :     try {
+     688        9442 :       broadcaster_->sendTransform(tf_msg);
+     689             :     }
+     690           0 :     catch (...) {
+     691           0 :       ROS_ERROR("exception caught ");
+     692             :     }
+     693             :   } else {
+     694           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(),
+     695             :                       tf_msg.child_frame_id.c_str());
+     696             :   }
+     697        9442 :   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       27254 : void TransformManager::callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     704             : 
+     705       27254 :   if (!is_initialized_) {
+     706           0 :     return;
+     707             :   }
+     708             : 
+     709       81762 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     710             : 
+     711       27254 :   if (publish_fcu_untilted_tf_) {
+     712       27254 :     publishFcuUntiltedTf(msg);
+     713             :   }
+     714             : }
+     715             : /*//}*/
+     716             : 
+     717             : /*//{ callbackGnss() */
+     718        5766 : void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+     719             : 
+     720        5766 :   if (!is_initialized_) {
+     721        5759 :     return;
+     722             :   }
+     723             : 
+     724        5766 :   if (got_utm_offset_) {
+     725        5759 :     return;
+     726             :   }
+     727             : 
+     728          14 :   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           7 :   mrs_lib::UTM(msg->latitude, msg->longitude, &out_x, &out_y);
+     734             : 
+     735           7 :   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           7 :   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           7 :   geometry_msgs::Point utm_origin;
+     746           7 :   utm_origin.x = out_x;
+     747           7 :   utm_origin.y = out_y;
+     748           7 :   utm_origin.z = msg->altitude;
+     749             : 
+     750           7 :   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          14 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     753           7 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     754           7 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     755             :   }
+     756           7 :   got_utm_offset_ = true;
+     757             : }
+     758             : /*//}*/
+     759             : 
+     760             : /*//{ callbackRtkGps() */
+     761           0 : void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) {
+     762             : 
+     763           0 :   if (!is_initialized_) {
+     764           0 :     return;
+     765             :   }
+     766             : 
+     767           0 :   if (got_utm_offset_) {
+     768           0 :     return;
+     769             :   }
+     770             : 
+     771           0 :   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           0 :   geometry_msgs::PoseStamped rtk_pos;
+     777             : 
+     778           0 :   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           0 :   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           0 :   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           0 :   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           0 :   rtk_pos.header = msg->header;
+     799           0 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+     800           0 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+     801           0 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     802             : 
+     803           0 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+     804           0 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+     805             : 
+     806             : 
+     807             :   // transform the RTK position from antenna to FCU
+     808           0 :   auto res = transformRtkToFcu(rtk_pos);
+     809           0 :   if (res) {
+     810           0 :     rtk_pos.pose = res.value();
+     811             :   } else {
+     812           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+     813           0 :     return;
+     814             :   }
+     815             : 
+     816           0 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &out_x, &out_y);
+     817             : 
+     818           0 :   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           0 :   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           0 :   geometry_msgs::Point utm_origin;
+     829           0 :   utm_origin.x = out_x;
+     830           0 :   utm_origin.y = out_y;
+     831           0 :   utm_origin.z = msg->gps.altitude;
+     832             : 
+     833           0 :   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           0 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     836           0 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     837           0 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     838             :   }
+     839           0 :   got_utm_offset_ = true;
+     840             : }
+     841             : /*//}*/
+     842             : 
+     843             : /*//{ publishFcuUntiltedTf() */
+     844       27254 : void TransformManager::publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     845             : 
+     846       54508 :   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       27254 :     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       27254 :   scope_timer.checkpoint("heading");
+     858             : 
+     859       27254 :   const Eigen::Matrix3d odom_pixhawk_R = mrs_lib::AttitudeConverter(msg->quaternion);
+     860       27254 :   const Eigen::Matrix3d undo_heading_R = mrs_lib::AttitudeConverter(Eigen::AngleAxis(-heading, Eigen::Vector3d(0, 0, 1)));
+     861             : 
+     862       27254 :   const tf2::Quaternion q     = mrs_lib::AttitudeConverter(undo_heading_R * odom_pixhawk_R);
+     863       27254 :   const tf2::Quaternion q_inv = q.inverse();
+     864             : 
+     865       27254 :   scope_timer.checkpoint("q inverse");
+     866             : 
+     867       27254 :   geometry_msgs::TransformStamped tf;
+     868       27254 :   tf.header.stamp            = msg->header.stamp;  // TODO(petrlmat) ros::Time::now()?
+     869       27254 :   tf.header.frame_id         = ch_->frames.ns_fcu;
+     870       27254 :   tf.child_frame_id          = ch_->frames.ns_fcu_untilted;
+     871       27254 :   tf.transform.translation.x = 0.0;
+     872       27254 :   tf.transform.translation.y = 0.0;
+     873       27254 :   tf.transform.translation.z = 0.0;
+     874       27254 :   tf.transform.rotation      = mrs_lib::AttitudeConverter(q_inv);
+     875             : 
+     876       27254 :   scope_timer.checkpoint("tf fill");
+     877       27254 :   if (Support::noNans(tf)) {
+     878       27254 :     broadcaster_->sendTransform(tf);
+     879             :   } else {
+     880           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN encountered in fcu_untilted tf", getPrintName().c_str());
+     881             :   }
+     882       27254 :   scope_timer.checkpoint("tf pub");
+     883             : }
+     884             : /*//}*/
+     885             : 
+     886             : /*//{ transformRtkToFcu() */
+     887           0 : std::optional<geometry_msgs::Pose> TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+     888             : 
+     889           0 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+     890             : 
+     891             :   // inject current orientation into rtk pose
+     892           0 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+     893           0 :   if (res1) {
+     894           0 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+     895             :   } else {
+     896           0 :     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           0 :     return {};
+     899             :   }
+     900             : 
+     901             :   // invert tf
+     902           0 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+     903           0 :   geometry_msgs::PoseStamped utm_in_antenna;
+     904           0 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+     905           0 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+     906           0 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+     907             : 
+     908             :   // transform to fcu
+     909           0 :   geometry_msgs::PoseStamped utm_in_fcu;
+     910           0 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+     911           0 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+     912           0 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+     913             : 
+     914           0 :   if (res2) {
+     915           0 :     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           0 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+     923           0 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+     924             : 
+     925           0 :   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          99 : std::string TransformManager::getPrintName() const {
+     937          99 :   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           7 : 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..e95793748c182ed3b7d310bde486e58ff2144567 GIT binary patch literal 3319 zcmV0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpjiQH(#0gwFztQJRL6eIz`}Fl}9SqI7~p1DMu?2$o0oH zFOWLXj;LAsDDL*u$LB`kq7Er3W#t6WNo4!T~T#v z*`ZRkDZpp^3}|RS#4KHMW%Xl;5*)n@5=bHE<~K$oXzLumm_fr=FgDZlZ|I zM3AjB)8thBLnW}UEdE1Tm&<7FD=j=5R>RWr+EJjLA}kuPm11Oc6xeL0I3jJHiz>ul z3N+)VL_M;a4k>)352w_FHNt3|7be@2Q-NoBOut{BBz+h;vrpi9X{(OYi@3qiQonQbs`M2#+Z+0ZwFi7iLUCJJD@60F6cJ z3P9!U6j>j*4S{Hc>0Yn5FN^=Xz0Ax0@}A$hWZ?ciUzSFot*oQjJ~FZ@a=uxkdUmZj zZO)T{ULpHm7fFp1>K<^!6*%y}KLT{X|EA9b9=-E&z5!v=j#IlpsT zwlB6$#TRNb`-od#;ZItJX2PQk6nRV(3ShKL=kSc>)GP(BQDN2h`Rf=Tw6`_8=4uhf zqR5kN{VVNJt~%EJ$+_9z(c`yYU)t6R57xHMs}Gmf994>pH6UP{!eL__6GkwKZLuJb zCKM&xIk|TIg!QO+FST8-lR*6rk90en^6>1A_K6drSAz*9V4wHTdo@jWp_8V+NZ?3vP-fq7x*m#kxY6W_bZ`x(NV zB+L%9NNtQO-j<}f`yqLf>Fse})35zwq+>E#f!W)7TXQY%yc}B>WH~YdmRS+FJ%=wN zaLEg44>T^$NOWe7i$N4fX^N$66f^fT4DsYLH&TpLHV$V6_+o3LtJxK3TfEwh2k@ve zsq*;7z#Dno0&pUEjmL!mZ{SfaplTJjSb+(8pwEb94#BI-V^STMF%R2FHRSm-!tI09 zIzBnNM?prZwv=id-3OzeD}mc{_%Z@rc0duB2>QTr^fbs*webkOq_FGVM$xnd34I|` zBLZ4ooV7fSTaY@BI5K=AkLnhr&Laliz@uJr!axQ1v;qV4$iW7w#JSn!8?+rv&#Rnb zv1NBv*Ic3h9^aMHx`K@&p3W@%e$(>f)0qVWf9G@tPhEfGbmj-9g=&fQT{iDY&_mN1 zZnmB*{+8*?<;ysb{h$s~Wx#h*!n0{Gm3Mwg7)Ra<#8o>X*n@yBP^@h%rmCyJKvB*u zoFH4}Jjx}Yu|$g6LBIX0_7I7j;pe|T5OmRxBev{_!xUJ}Zl8v^f@`W8_`=b_$#;74 z-Z2gUf^W-Xz&ZfDWp@P!056N)F5}cROSvJe$YdsQdq~pOd7Kx_?;{9+$c~*VpwC+Ms9!k|TP8b!?ah

voB+{o9W9^2r&Q`DUsth(f#2b%H#yvc3}iMqLU z#JG3c=YFySEvCDfq%DaR*Bq+epJHO=yhl#(LYz*lY1PI)1JpfcQ}c22kZL@lK114u z9{e(A?qU&^JSl$rU5`1u3BbC_TZR%Sa+uz0xO;!GY60~HTf>XEH_!siPo#lszs zQ|Rd?1`ii}VNrOXE!O<9dAJwxGopvvfu?TQ9USaM4~H`g6c4vo-PEsr^JP<`9uDlB zk+vxY(hs_iE?pnxqtm^=C%jJ=O7$U~*XR_ueB4db<>ojGsh8IRmB4v|HMR>HJc2zt_OGtIve=2C(Nt`n`!4%wWkiFZ6wj z9#QXm?e<^*ZuTG!DQ2m=AalyV8e%rqZtg==xJ_(2vlp36z$%a1KjtNX(SAO4Ra3XA z+lMmZKoUn4ALNM79Z|RFZdxsTNLeiLwYVGZW$ay&Q4~A*j8R0f6kxd3!9UKy+Waws z_boU65B$21ulpz{;#;Ie2O@a|>KDFV!7Q~ZIo;Aw zC|c8Oe}$P?LCKplh$$D=ix5N6Q9zdw(X(m zkRNw{&sYqW!dZJR$|W7$D}dHP%6y*dcf@gZA4hi;Q{RCT+I8~XuonkTr)%Bklf4k| zsGgbpERVQ3q$8jW+gG1+ab!PJRO%r!%XE2)*s}BD3}18pOYAYT%GB~_h51ifrnu5R z$yLMvycFeaZNFhi2|LottsK-AnWyay0xo%@yP%9I(j=UDftPIy{)Ym2XOh%+yvw$cWhN+8ka@6uGG?+&{I=!By)0cb3=>QXNm2+sm&6pP*^0qOnU5=CL&lUIHr)BorI zAO~rGNwiSU`Yb#@dxQAgasr|tx8zwm0$M4?dypO}A%9zZF&caM*^n@&$tK`u&2=|J zCrBmf3_P>unp56$OfA6ELMD`yALB_9L)G_OkZs8-jzSZ8inBY6^>DU>%*;zATzyX| z;)6v3MV2|(gjPLOr>8SR5B}$cOUP4-=XnfgUr}L!v`zTprhieZ%gFR?`S$<-002ovPDHLkV1gtE BQeyxB literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..8d1c3bf63b --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:543115147.2 %
Date:2023-11-30 22:31:42Functions:313881.6 %
Legend: Lines: + hit + not hit +
+
+ +

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::uav_manager::UavManager::pirouetteSrv()0
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::uav_manager::UavManager::elandSrv()0
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()0
mrs_uav_managers::uav_manager::UavManager::ungripSrv()0
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()1
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()1
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)1
mrs_uav_managers::uav_manager::UavManager::landSrv()1
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()1
mrs_uav_managers::uav_manager::UavManager::disarmSrv()1
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)2
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)5
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)6
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)6
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()6
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)6
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)6
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::uav_manager::UavManager::initialize()7
mrs_uav_managers::uav_manager::UavManager::preinitialize()7
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)7
mrs_uav_managers::uav_manager::UavManager::onInit()7
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)9
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)15
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)16
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)47
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)83
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)107
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)1107
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)1107
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)3330
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)5452
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)8866
+
+
+ + + +
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..821cc0360f --- /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:543115147.2 %
Date:2023-11-30 22:31:42Functions:313881.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()7
mrs_uav_managers::uav_manager::UavManager::initialize()7
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()1
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)6
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::pirouetteSrv()0
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)83
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)47
mrs_uav_managers::uav_manager::UavManager::preinitialize()7
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)1107
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)1107
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)5
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)8866
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)16
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)107
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)3330
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)5452
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)2
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()1
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)15
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)6
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()6
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)6
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)1
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)7
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)9
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)6
mrs_uav_managers::uav_manager::UavManager::onInit()7
mrs_uav_managers::uav_manager::UavManager::landSrv()1
mrs_uav_managers::uav_manager::UavManager::elandSrv()0
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()1
mrs_uav_managers::uav_manager::UavManager::disarmSrv()1
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()0
mrs_uav_managers::uav_manager::UavManager::ungripSrv()0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..f6c3894100 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.html b/mrs_uav_managers/src/uav_manager.cpp.gcov.html new file mode 100644 index 0000000000..63a2c3194c --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.html @@ -0,0 +1,2732 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:543115147.2 %
Date:2023-11-30 22:31:42Functions:313881.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.png b/mrs_uav_managers/src/uav_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1b7a6803d90960d4288404eab7d8dc4177b755d6 GIT binary patch literal 7136 zcmV<68z1C}P)4V{WxQN2)8BtX zul9$}AidO3MnD0uvg=oyx&!v;THG+k9(WUsLO?M_dDrqW^1zLN;<+w>QZqxsi>Hvm znA1c9N-Hv{U}BkX#k;$#@Nq02BTq(=-WV?hptB} zk!A$FFdBSyKtr?uV}Gg#vHVE{0s?Lynr|XmR5inIk9Hh8;dDkI*xn9fbfzM3VAD|L zWAbK6;Q8QRn7A&ym~A(;5up&xC2P|*tE=gn_$Jtd1$O0Qya;z|9me?L$JkN7lGSaT zqWoH_^%(gk+AdOKbSA*q=*gfN?GkXA;1cA1I)U#4lI*`8OqJEbU4zs zhgv=9jb;kSb)E2<=~>4$NLKHi;veC zA0Mi94V7Uat3?RFNmN!+93#vQjzyuUMb|07f^rSXUV<@F0HgmFUCDyyt4lT>u&21V z=bBI=2ElP%XVJ>i%pky$A-xDt5^BwQacKr&W3#-L0D1~){pK!d2X{}FwIGBdi~;?2 zF(?j`BpJguOZkQBo6VdNZX6LoA^~aX5jlno;3lfGtQ2ilEHHDGnG9q}LrS zB-U%*LN-oeA_$u?-jXk745-5x<*Op$KoHA169M_Ibz{VhG6N(aguIskZz+^=jPdtx zGcP`W7^y+#>>RE_HIz#@9Ajya>=^R}Y0ojve_zr|YiW!*;0|MCThcWPHz$C?_QE(j z=9s^;g+YxmI6h+x7Wm@f3e`7CChA&P02E;?t=B55M;9M)1<)hl!Q{hLtR^JWNdtl% zcbPpTpp+PbkGBl(y+1~h>TF+OI3)ozxJ7`o7~?Diyh$+n4{zY37dHVhHz}Y6?!%sF zw98Z@VBr{5#B`XMUB|c&U@xk@pw$6yUpYnr;H@Vy(dUxu_5bY}taCMta|Ha@F$UnC zhSbYr7pehn)pjO^KF3Sg+=MWw+W#(7J%cgg9Vx>i#7DS$qX6Z6Bo=&PYdAa}Ptcav z|I2gz`_HzAZvgO7Y{O$M0|Y>MLnp*We`0U@6w~e1*g`gFiitl;vfviK)-!Mw0?ko8g(pNch(bAn7(US%?qdqwAn1xWOp))9YCZ z=z)dSOl}|hVZ4f|))}cky8H6+)#`wA9b=o|`U7XZb}4Rzbk~hzbQd>mH>61EZ|EP- zio$D)dW$C_ebB>W2t$-8@&=Q>1X!49XY6oa)p#%=XW0 z1UO35F^qjyF^pThvI%b`vSM=pD8$${#x4bKw6PC*d-F!F!PrMTKyLYLj1tW!v82ZeHlbPG!hGeZ` z)=@>KDrXTw1s};A`xE?Q^X=<fvgq`EOKc1~V1uq@BC zg)!Cu!v;x6jA4&O1F>yj55ibr{0nfE2+*r*yAfQUbc=v8fiz>}kiB-AS=at2L3hwR zScZ$_Og=`9UK4V##v!`VnkgipGR|t|`T@9suw;O8jEfyo6;O0DWf?3t&x#tROob6?3~wN&_}xB$?2x3aox@Pz5Jh&a>W#u@1Pa&Tt%W zpF$s*O!2m3ECNb(rh7WR^P0)UNFhhflB96Nr8MtQ^L17~lmNfC`ho6PUGxRh`#oYQ zS)?E`0aX&{;ys9Bz7$xN5|;}uYbjP>d|ft0BxgIA6mjztQ>=cze$ZUsO);v$WdDlA zsjV0@RcWHSaO9+|z%Rqd1IjTbreY(;N7RLC+NE>g{3`O-cd5NT)_UD*!On||=>>a- z_1a9eBqpWkItWf6;yrcEF!zD-AVVGiyvI41V~xIoJ^NJIg)!!tkJxo1djjel@U2vv z^NfgKni#7b$yt1g80i>8h->uMI@0Q$)`#=UhZ{b3IyWBj`$JR$tZiC7AZz2( z-JaA^2nzupLms&DQ;7-zAMdZ8y+xk$e3Y_>EjRNu!|S50H$yz;%FQysv^0bTWJ*KS zC9Jr)aD8{cNY$!Kl2A1}6xkav@_-7A%aceoJ*kw25Tg~mI-nRMq1an9K4_A+k8$r9 zDKKXXF8pe*BjrrsCxl4M0y6Wave7>BM7L^T+$zKcW(&rbI2K9;Te-Htfxho6o(Y^Nh0F`kYOUd4{>`0y@ySI6h6D~UNB9|RbvvL8>!r;71^ zI6mDl9@X*LMs+TQ2=Mb9pTtz2j!zlm!5p7X4{8#Z5yd@omoQz%_BzQ%~ds4&t=70ulw$P1; zupK182ICp1C4h2_@vf;PlG7fFPrWVDS!y<<_9)I1ZAK};C+q!i1*gF_`zrO%@sP-) zK;~~W#<*ra(`LrlbAYebo9H1e(c}t|_kQ#jD+E%ut--5o29cPdIZJ0zPK@LquRl*r9 ziN${L9($YZ7$0F98H<@}a}Q#W&y_2S9%DGMR6e=XcF^Wlc+#T%=+2E{W&|ufq@nMc zx%d`dq@zAHRIOM>*Bi}+`+o5~NEJZos}gTJyMrXWz@)cJc(FXhwmxD8g>7dsBi729 z2Fz(jfCmsWT1T=m9xYJWC*MTBd||l-ciP9=UI#UDHykCKjE!5@O&4V6HF5Nop%Ej6 zN{vyhhF5GKS*zB3)V9l+3g#yE6}$Jkdp;y#mlllW@P`_sAp9UdVB9|d(IRI2I~C1h z)~AB)6tgnmyTmN_z_-rz8}nS~il}rh?QP`hDNN&?QEt8Dt{X7+$+1hsR*kW1_3S~y zv&F`}?v|Yj-t~1i{3g1;lF%dE_&8{n8ZBzqOq^EPW7qs;S32Y^tn(1?XwKRGxew2* z*ADq`&RNoM*Tds55$R<=a;KwRrwc=Xb>?ME|p|IW6gkp!M9kOCq zP&fsCLh>;=(}1b|aXvCqneow8XYo{@e7q9@3zCmPGAXKmi;t>M6S23?N}FutBe$gB zBFkr>TEy`PSmAgASZ270f;NS@O>`N*=81)xZkBs4IizDaSkl;c zb#+H2Ku&2{nD-)-i5&P)A%Cno*H>Rn?R3_N$FHu}2S@|RFTQsLhe4DfAiA~4S0=hY zbX>UcKGG_CWc=|#7iMUHn0MakB5MF+;ZaftNIXhPK7Aum)(BX$^%$^DOx@P+bBF~9 z^xa)`M9sBWa3v46=`aqy{x}4hIh9bxALl6gbWKI5vyarUfKp=2vGdUpVxAe}XtKrE z=Evc9x_<~26=H`hp*_)FpjuK{q+QqrY8bX$VbR2|l$+1~K2nhyYIjnyy_rn8@-fmw z&|>Vm+uLCo9+49A9W-g-%;ZujEu;Z;1%9*^63^d?%B23k&Md({JgCm}!FT|jvHv@@ z8N&r5ldtcX9N+}YJVu{Ujb`7#H!yZZj3mbFC%gEjkTg%}6Dz z4l{#F8}w9XPr8@8mI4;gYSlFwF`-cOc}|aXWwtykv*(2%!VQwy3B^KPW~=bo8>Atu zA3-j{*T|TMu4@?Id^uJ{0j=~LqttEp2TYxDNj}X-EKIwFJXbQ1voOsXT@LU8(+VL> zSttbRooo`@$0HW7&~*R=#}`0-q(eD8IHeNdt3V1 zc{7~kxffVe)Q0hmn_)$cUCJI}c()B7| zAK7(_;-j$^Ij>GWngOm1Lpi;!u;=i6Jb;tym{TW4MDio?KJxQSEf^bog>1RW)K%Cu zE=-xIi&iGwHSC|HIvLhV4lyqUc7WzNh8iGE%>IDsp^BG^IY3z({2>kIct-fH-rXzM zc@HSYNR(dssR1noj0^GNPgrY-`GiC_E@EF_uRWEherBe&nc-bC;?MUjnkjSg_Q;h> zXenMl%P@Mt?v?xCNLBIK&!)Ip@!3zENpZZ9oze)$s`&EIWC{ZdaF9(sM)SU|6tD=Y zT;f7uWT6lUQeApl(gT_g62L@Owh)u8uFMFsDFg!CrE8w1MvQ&ty89DCTIE_?G?kU# zoR9SC(WYF2BTypc+QX0vmZLw6!k!gf?|eaamxKYEpFs+oyw6o+&nM+)+&(HD%bB0C z+E~DN<`p{3;p5CJlmNx;apo1ueEi6~!da|z<`v3-ZG1fQ3Mt?j4?lc7Jc|rJ)*KxL z7{gqxRG)FPGJ8HEZdP#Y&$!w1jGI-v=4m?PW=kn9ea6ko7!MaW>r&fXcvueo=!l2> z+yTFPb_M-;$d5j=6K4kA9mq${w7x=rH{~r}T;uKYmXFj1=;UKjDsw)%yM|b*Pd*Cn zly?94y9pAf;s^P-_u*X}5r>Zcx5aVAg4=fcya|PYqN3GD#lMY@pK*A%5E1`TOVN1y zyyK1E15kN|pXMXWN}7)ZD^EU_FGElHxH6UYU&hA+#jDN+#SuK z*JJEh6X#b2FhHg}^o^^+7hgX8E}i*;>#Q{r{);*z(U`VNlW)`=e`@K0@>meZ#=DS< z$>D2yuPd{F1VHmi0(g($ort-FTf-T0aMyfRLwQpyT3;!n*KqCaB|c~|T{yvn*P-*$ zdu84ROZNiF#m@Am#{=$sv2zcd$uFtyiqQ;^5_Gcbx zz9DoRpbX>3oK6+QDb9tRYzGLX4A4LCleiCN1ZfsjwpptR{v8JJBofI9w0Lyc412Xs zR+tUftGgBMW=+iv;r+9`VWn$$j1g6FZr2$SNh8KVstu1vv?--RTUFOXe4@qgx+eb? z*XW=3pb}!X&t=K4`pe*6-;Cv_1wca?Chh+Lt`7omhod{iiriOIP2uAqtH@Ph6fK4^ zomx4`PB5IV$W;q&-Sfd+^VO~q(0r5tKEcR2KoKzsMiy~9My}A7?wTmHZSK095!Yb! zLZ0S48!!&tH;|BOna)%Ws8cgT7TWGn2e8Kz8EICk!^4{(xW~FQ2T2XV#88iPPIaT{ zwt&iwrVq`;SOxu(r~k1J`-VqeARNx)l)Pc-6TQK8H6OVkw+nLv=_vuqyZ+Gpj7PET z=8}GVWO%wnq4hZgqBTT@6W^zGuKP!Z^SR!-Xg=EHarH~~nvUS724j2A?g zajkAMpNTt++YDY1kItt5W^+Yl&zukc`sEF|i49`rr!Z!9Z}v$_lPXaiwT0 zCPmEfeIs#wLUFVKnB9T$7OF`D;J&yS%(GFvpD+NMFn*u`h-c(J*8r^FZvgt;(~w;$ z%jIYlEB8mS}Ik@nkEWJc;`L#QinL=tf=7%6M0Zo$&L*+?ZO zW%KER%&c!6V|^L6J2izW2-^;q2X?fNe)Dkz`}mDD?EOxOXSK)71=JtqeK3d*=zT1K zRiNtDpC$cZ@56rcE4`17!Gd&QXK!E;nX>CaeT5zvKg3r^)%6+v`Yv@nP;Bo%1Pj6@ zT78}?O-sl0@GHCLKHYQ|=|&$x23V@l))Mm*F?$ewUH8yybbz*tecbb+amJRoN)^=x z>4$>_{kH6ck(#4%iQaDqBQ^WV7VD3xu1k=$R%=!OR8XB;Ghz^5Va@y(r!}LjSxB^< z)~teY?Cc<=){K{MvGk;&&zY%=`=rokkGQ6F0J@@vcKX#!6hMHP2OW z7q&!@wvX}j;!ZCv1^hy5)-h|B6|jUELZ6nlofJy=g7k+nT8Ib7?x$KXhWDqdU585( znFGf1uG4B9vi}2a;{Izj&fzw3d}YCUn!R$H+)b`_E7u((br@gW;9=_`)_bM-g5!U0 WF0i=}ApN`m0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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)4
mrs_uav_state_estimators::AltGeneric::~AltGeneric()4
mrs_uav_state_estimators::AltGeneric::~AltGeneric().24
+
+
+ + + +
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..f479c7dd41 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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)4
mrs_uav_state_estimators::AltGeneric::~AltGeneric()4
mrs_uav_state_estimators::AltGeneric::~AltGeneric().24
+
+
+ + + +
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..5abb951b2c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html @@ -0,0 +1,242 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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           4 :   AltGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+     114           4 :       : AltitudeEstimator<alt_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+     115           4 :   }
+     116             : 
+     117           8 :   ~AltGeneric(void) {
+     118           8 :   }
+     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..388a937971 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().24
+
+
+ + + +
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..7bb2f6f157 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator()0
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().24
+
+
+ + + +
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..3c03d5a779 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html @@ -0,0 +1,130 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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           4 :   AltitudeEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(altitude::type, name, frame_id) {
+      31           4 :   }
+      32             : 
+      33           4 :   virtual ~AltitudeEstimator(void) {
+      34           4 :   }
+      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..27493d5848 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..8e8c963f9f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..6d08883cd4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..302bf8c3c7 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..b37f808f97 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..17c9cc21ae --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..5b970bfbcf --- /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:36065555.0 %
Date:2023-11-30 22:31:42Functions:496773.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::getVelInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getAvgRtkInitZ(double)0
mrs_uav_state_estimators::Correction<2>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)0
mrs_uav_state_estimators::Correction<2>::resetProcessors()0
mrs_uav_state_estimators::Correction<2>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)4
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)5
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)>)6
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)>)8
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const12
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const13
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const16
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const20
mrs_uav_state_estimators::Correction<1>::getAvgRtkInitZ(double)22
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const26
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const38
mrs_uav_state_estimators::Correction<1>::getRawCorrection()113
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()113
mrs_uav_state_estimators::Correction<2>::getRawCorrection()120
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()120
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1004
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1007
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1035
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1035
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1048
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1048
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)2121
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)2151
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)2330
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)2331
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)4608
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)4610
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4621
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> >)4626
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4626
mrs_uav_state_estimators::Correction<2>::isMsgComing()6073
mrs_uav_state_estimators::Correction<2>::isHealthy()6073
mrs_uav_state_estimators::Correction<1>::isMsgComing()8054
mrs_uav_state_estimators::Correction<1>::isHealthy()8054
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)9143
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)9147
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)9147
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)10227
mrs_uav_state_estimators::Correction<2>::setR(double)10272
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)10272
mrs_uav_state_estimators::Correction<2>::getStateId() const10317
mrs_uav_state_estimators::Correction<1>::setR(double)14543
mrs_uav_state_estimators::Correction<1>::getStateId() const14543
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)14588
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)14642
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)20544
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)29185
mrs_uav_state_estimators::Correction<1>::getR()43703
mrs_uav_state_estimators::Correction<2>::getR()51313
+
+
+ + + +
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..46fa79a455 --- /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:36065555.0 %
Date:2023-11-30 22:31:42Functions:496773.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1004
mrs_uav_state_estimators::Correction<1>::isMsgComing()8054
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)2330
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)2121
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>)9143
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&)14588
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)9147
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()113
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)29185
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>)1035
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>)2331
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)2151
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()113
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)5
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)9147
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()43703
mrs_uav_state_estimators::Correction<1>::setR(double)14543
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)14642
mrs_uav_state_estimators::Correction<1>::isHealthy()8054
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)>)8
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1007
mrs_uav_state_estimators::Correction<2>::isMsgComing()6073
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)4608
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> >)4626
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4621
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&)10227
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()120
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)20544
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>)1048
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>)4610
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()120
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)4
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4626
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()51313
mrs_uav_state_estimators::Correction<2>::setR(double)10272
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)10272
mrs_uav_state_estimators::Correction<2>::isHealthy()6073
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)>)6
mrs_uav_state_estimators::Correction<1>::getStateId() const14543
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const38
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const26
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1035
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const16
mrs_uav_state_estimators::Correction<2>::getStateId() const10317
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const13
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const20
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1048
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const12
+
+
+ + + +
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..398e73c5f8 --- /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:36065555.0 %
Date:2023-11-30 22:31:42Functions:496773.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef ESTIMATORS_CORRECTION_H
+       3             : #define ESTIMATORS_CORRECTION_H
+       4             : 
+       5             : #include <Eigen/Dense>
+       6             : 
+       7             : #include <mrs_lib/subscribe_handler.h>
+       8             : #include <mrs_lib/publisher_handler.h>
+       9             : #include <mrs_lib/attitude_converter.h>
+      10             : #include <mrs_lib/param_loader.h>
+      11             : #include <mrs_lib/dynamic_reconfigure_mgr.h>
+      12             : #include <mrs_lib/gps_conversions.h>
+      13             : #include <mrs_lib/mutex.h>
+      14             : #include <mrs_lib/geometry/cyclic.h>
+      15             : 
+      16             : #include <mrs_msgs/RtkGps.h>
+      17             : #include <mrs_msgs/EstimatorCorrection.h>
+      18             : #include <mrs_msgs/Float64Stamped.h>
+      19             : 
+      20             : #include <sensor_msgs/Range.h>
+      21             : #include <nav_msgs/Odometry.h>
+      22             : 
+      23             : #include <std_srvs/SetBool.h>
+      24             : 
+      25             : #include <functional>
+      26             : 
+      27             : #include <mrs_uav_managers/estimation_manager/types.h>
+      28             : #include <mrs_uav_managers/estimation_manager/support.h>
+      29             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      30             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      31             : 
+      32             : #include <mrs_uav_state_estimators/processors/processor.h>
+      33             : #include <mrs_uav_state_estimators/processors/proc_median_filter.h>
+      34             : #include <mrs_uav_state_estimators/processors/proc_saturate.h>
+      35             : #include <mrs_uav_state_estimators/processors/proc_excessive_tilt.h>
+      36             : #include <mrs_uav_state_estimators/processors/proc_tf_to_world.h>
+      37             : 
+      38             : #include <mrs_uav_state_estimators/CorrectionConfig.h>
+      39             : 
+      40             : 
+      41             : namespace mrs_uav_state_estimators
+      42             : {
+      43             : 
+      44             : typedef enum
+      45             : {
+      46             :   LATERAL,
+      47             :   ALTITUDE,
+      48             :   HEADING
+      49             : } EstimatorType_t;
+      50             : const int n_EstimatorType_t = 3;
+      51             : 
+      52             : typedef enum
+      53             : {
+      54             :   UNKNOWN,
+      55             :   ODOMETRY,
+      56             :   POSE,
+      57             :   POSECOV,
+      58             :   RANGE,
+      59             :   RTK_GPS,
+      60             :   POINT,
+      61             :   VECTOR,
+      62             :   QUAT,
+      63             : } MessageType_t;
+      64             : const int n_MessageType_t = 9;
+      65             : 
+      66             : const std::map<std::string, MessageType_t> map_msg_type{{"nav_msgs/Odometry", MessageType_t::ODOMETRY},
+      67             :                                                         {"geometry_msgs/PoseStamped", MessageType_t::POSE},
+      68             :                                                         {"geometry_msgs/PoseWithCovarianceStamped", MessageType_t::POSECOV},
+      69             :                                                         {"sensor_msgs/Range", MessageType_t::RANGE},
+      70             :                                                         {"mrs_msgs/RtkGps", MessageType_t::RTK_GPS},
+      71             :                                                         {"geometry_msgs/PointStamped", MessageType_t::POINT},
+      72             :                                                         {"geometry_msgs/Vector3Stamped", MessageType_t::VECTOR},
+      73             :                                                         {"geometry_msgs/QuaternionStamped", MessageType_t::QUAT}};
+      74             : 
+      75             : template <int n_measurements>
+      76             : class Correction {
+      77             : 
+      78             :   using CommonHandlers_t  = mrs_uav_managers::estimation_manager::CommonHandlers_t;
+      79             :   using PrivateHandlers_t = mrs_uav_managers::estimation_manager::PrivateHandlers_t;
+      80             :   using StateId_t         = mrs_uav_managers::estimation_manager::StateId_t;
+      81             : 
+      82             : public:
+      83             :   typedef Eigen::Matrix<double, n_measurements, 1>         measurement_t;
+      84             :   typedef mrs_lib::DynamicReconfigureMgr<CorrectionConfig> drmgr_t;
+      85             : 
+      86             :   struct MeasurementStamped
+      87             :   {
+      88             :     ros::Time     stamp;
+      89             :     measurement_t value;
+      90             :   };
+      91             : 
+      92             : public:
+      93             :   Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& frame_id, const EstimatorType_t& est_type,
+      94             :              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
+      95             :              std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction);
+      96             : 
+      97             :   std::string getName() const;
+      98             :   std::string getNamespacedName() const;
+      99             :   std::string getPrintName() const;
+     100             : 
+     101             :   double    getR();
+     102             :   void      setR(const double R);
+     103             :   StateId_t getStateId() const;
+     104             : 
+     105             :   bool             isHealthy();
+     106             :   ros::Time        healthy_time_;
+     107             :   std::atomic_bool is_healthy_    = true;
+     108             :   std::atomic_bool is_delay_ok_   = true;
+     109             :   std::atomic_bool is_dt_ok_      = true;
+     110             :   std::atomic_bool is_nan_free_   = true;
+     111             :   std::atomic_bool got_first_msg_ = false;
+     112             : 
+     113             :   int counter_nan_ = 0;
+     114             : 
+     115             :   std::optional<MeasurementStamped> getRawCorrection();
+     116             :   std::optional<MeasurementStamped> getProcessedCorrection();
+     117             : 
+     118             :   void resetProcessors();
+     119             : 
+     120             : private:
+     121             :   std::atomic_bool is_initialized_ = false;
+     122             : 
+     123             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odom_;
+     124             :   void                                          callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     125             :   std::optional<measurement_t>                  getCorrectionFromOdometry(const nav_msgs::OdometryConstPtr msg);
+     126             : 
+     127             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped> sh_pose_s_;
+     128             :   void                                                  callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr msg);
+     129             :   std::optional<measurement_t>                          getCorrectionFromPoseStamped(const geometry_msgs::PoseStampedConstPtr msg);
+     130             : 
+     131             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped> sh_pose_wcs_;
+     132             : 
+     133             :   mrs_lib::SubscribeHandler<mrs_msgs::RtkGps> sh_rtk_;
+     134             :   void                                        callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg);
+     135             :   std::optional<measurement_t>                getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg);
+     136             :   void                                        getAvgRtkInitZ(const double rtk_z);
+     137             :   bool                                        got_avg_init_rtk_z_ = false;
+     138             :   double                                      rtk_init_z_avg_     = 0.0;
+     139             :   int                                         got_rtk_counter_    = 0;
+     140             : 
+     141             :   mrs_lib::SubscribeHandler<geometry_msgs::PointStamped> sh_point_;
+     142             :   void                                                   callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg);
+     143             :   std::optional<measurement_t>                           getCorrectionFromPoint(const geometry_msgs::PointStampedConstPtr msg);
+     144             : 
+     145             :   mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped> sh_vector_;
+     146             :   std::optional<measurement_t>                             getCorrectionFromVector(const geometry_msgs::Vector3StampedConstPtr msg);
+     147             :   void                                                     callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg);
+     148             : 
+     149             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_; // for obtaining heading rate
+     150             :   std::string orientation_topic_;
+     151             : 
+     152             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_quat_;
+     153             :   measurement_t                                               prev_hdg_measurement_;
+     154             :   bool                                                        got_first_hdg_measurement_ = false;
+     155             : 
+     156             :   mrs_lib::SubscribeHandler<sensor_msgs::Range> sh_range_;
+     157             :   std::optional<measurement_t>                  getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg);
+     158             :   void                                          callbackRange(const sensor_msgs::Range::ConstPtr msg);
+     159             :   ros::ServiceServer                            ser_toggle_range_;
+     160             :   bool                                          callbackToggleRange(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     161             :   bool                                          range_enabled_ = true;
+     162             : 
+     163             :   void applyCorrection(const measurement_t& meas, const ros::Time& stamp);
+     164             : 
+     165             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_raw_;
+     166             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_proc_;
+     167             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>      ph_delay_;
+     168             : 
+     169             :   const std::string                  est_name_;
+     170             :   const std::string                  name_;
+     171             :   const std::string                  ns_frame_id_;
+     172             :   const EstimatorType_t              est_type_;
+     173             :   std::shared_ptr<CommonHandlers_t>  ch_;
+     174             :   std::shared_ptr<PrivateHandlers_t> ph_;
+     175             : 
+     176             :   MessageType_t msg_type_;
+     177             :   std::string   msg_topic_;
+     178             :   double        msg_timeout_;
+     179             : 
+     180             :   double     R_;
+     181             :   double     default_R_;
+     182             :   double     R_coeff_;
+     183             :   std::mutex mtx_R_;
+     184             :   StateId_t  state_id_;
+     185             :   bool       is_in_body_frame_ = true;
+     186             : 
+     187             :   std::unique_ptr<drmgr_t> drmgr_;
+     188             : 
+     189             :   std::optional<measurement_t> getCorrectionFromQuat(const geometry_msgs::QuaternionStampedConstPtr msg);
+     190             :   std::optional<measurement_t> getZVelUntilted(const geometry_msgs::Vector3& msg, const std_msgs::Header& header);
+     191             :   std::optional<measurement_t> getVelInFrame(const geometry_msgs::Vector3& vel_in, const std_msgs::Header& source_header, const std::string target_frame);
+     192             : 
+     193             :   std::optional<geometry_msgs::Pose> transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const;
+     194             : 
+     195             :   void   checkMsgDelay(const ros::Time& msg_time);
+     196             :   double msg_delay_limit_;
+     197             :   double msg_delay_warn_limit_;
+     198             : 
+     199             :   double time_since_last_msg_limit_;
+     200             : 
+     201             :   std::shared_ptr<Processor<n_measurements>> createProcessorFromName(const std::string& name, ros::NodeHandle& nh);
+     202             :   bool                                       process(measurement_t& measurement);
+     203             : 
+     204             :   bool             isTimestampOk();
+     205             :   bool             isMsgComing();
+     206             :   std::atomic_bool first_timestamp_ = true;
+     207             :   ros::Time        msg_time_;
+     208             :   ros::Time        prev_msg_time_;
+     209             :   std::mutex       mtx_msg_time_;
+     210             : 
+     211             :   std::vector<std::string>                                                    processor_names_;
+     212             :   std::unordered_map<std::string, std::shared_ptr<Processor<n_measurements>>> processors_;
+     213             : 
+     214             :   std::function<double(int, int)>                            fun_get_state_;
+     215             :   std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction_;
+     216             : 
+     217             :   void publishCorrection(const MeasurementStamped& measurement_stamped, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr);
+     218             :   void publishDelay(const double delay);
+     219             : };
+     220             : 
+     221             : /*//{ constructor */
+     222             : template <int n_measurements>
+     223          14 : 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          14 :       fun_apply_correction_(fun_apply_correction) {
+     235             : 
+     236             :   // | --------------------- load parameters -------------------- |
+     237             : 
+     238          28 :   std::string msg_type_string;
+     239             : 
+     240          14 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + getNamespacedName() + "/");
+     241             : 
+     242          14 :   ph->param_loader->loadParam("message/type", msg_type_string);
+     243          14 :   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          14 :   msg_type_ = map_msg_type.at(msg_type_string);
+     248             : 
+     249          14 :   ph->param_loader->loadParam("message/topic", msg_topic_);
+     250          14 :   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
+     251          14 :   ph->param_loader->loadParam("message/limit/delay", msg_delay_limit_);
+     252          14 :   msg_delay_warn_limit_ = msg_delay_limit_ / 2;  // maybe specify this as a param?
+     253          14 :   ph->param_loader->loadParam("message/limit/time_since_last", time_since_last_msg_limit_);
+     254             : 
+     255             :   int state_id_tmp;
+     256          14 :   ph->param_loader->loadParam("state_id", state_id_tmp);
+     257          14 :   if (state_id_tmp < n_StateId_t) {
+     258          14 :     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          14 :   if (state_id_ == StateId_t::VELOCITY) {
+     265           6 :     ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
+     266             :   }
+     267             : 
+     268          14 :   ph->param_loader->loadParam("noise", R_);
+     269          14 :   ph->param_loader->loadParam("noise_unhealthy_coeff", R_coeff_);
+     270          14 :   default_R_ = R_;
+     271             : 
+     272             :   // | --------------- processors initialization --------------- |
+     273          14 :   ph->param_loader->loadParam("processors", processor_names_);
+     274             : 
+     275          23 :   for (auto proc_name : processor_names_) {
+     276           9 :     processors_[proc_name] = createProcessorFromName(proc_name, nh);
+     277             :   }
+     278             : 
+     279             :   // | ------------- initialize dynamic reconfigure ------------- |
+     280          14 :   drmgr_               = std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), getPrintName());
+     281          14 :   drmgr_->config.noise = R_;
+     282          14 :   drmgr_->update_config(drmgr_->config);
+     283             : 
+     284             :   // | -------------- initialize subscribe handlers ------------- |
+     285          28 :   mrs_lib::SubscribeHandlerOptions shopts;
+     286          14 :   shopts.nh                 = nh;
+     287          14 :   shopts.node_name          = getPrintName();
+     288          14 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     289          14 :   shopts.threadsafe         = true;
+     290          14 :   shopts.autostart          = true;
+     291          14 :   shopts.queue_size         = 10;
+     292          14 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     293             : 
+     294          14 :   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           1 :     case MessageType_t::RANGE: {
+     309           1 :       sh_range_                   = mrs_lib::SubscribeHandler<sensor_msgs::Range>(shopts, msg_topic_, &Correction::callbackRange, this);
+     310           1 :       const std::size_t found     = ros::this_node::getName().find_last_of("/");
+     311           2 :       std::string       node_name = ros::this_node::getName().substr(found + 1);
+     312           2 :       ser_toggle_range_ =
+     313           1 :           nh.advertiseService(ros::this_node::getName() + "/" + getNamespacedName() + "/toggle_range_in", &Correction::callbackToggleRange, this);
+     314           1 :       break;
+     315             :     }
+     316           4 :     case MessageType_t::RTK_GPS: {
+     317           4 :       sh_rtk_ = mrs_lib::SubscribeHandler<mrs_msgs::RtkGps>(shopts, msg_topic_, &Correction::callbackRtk, this);
+     318           4 :       break;
+     319             :     }
+     320           3 :     case MessageType_t::POINT: {
+     321           3 :       sh_point_ = mrs_lib::SubscribeHandler<geometry_msgs::PointStamped>(shopts, msg_topic_, &Correction::callbackPoint, this);
+     322           3 :       break;
+     323             :     }
+     324           6 :     case MessageType_t::VECTOR: {
+     325           6 :       sh_vector_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, msg_topic_, &Correction::callbackVector, this);
+     326           6 :       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          14 :   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          14 :   if (ch_->debug_topics.correction) {
+     351          14 :     ph_correction_raw_  = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/raw", 10);
+     352          14 :     ph_correction_proc_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/proc", 10);
+     353             :   }
+     354          14 :   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          14 :   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          14 :   healthy_time_ = ros::Time(0);
+     365             : 
+     366          14 :   is_initialized_ = true;
+     367          14 : }
+     368             : /*//}*/
+     369             : 
+     370             : /*//{ getName() */
+     371             : template <int n_measurements>
+     372          28 : std::string Correction<n_measurements>::getName() const {
+     373          28 :   return name_;
+     374             : }
+     375             : /*//}*/
+     376             : 
+     377             : /*//{ getNamespacedName() */
+     378             : template <int n_measurements>
+     379          46 : std::string Correction<n_measurements>::getNamespacedName() const {
+     380          46 :   return est_name_ + "/" + name_;
+     381             : }
+     382             : /*//}*/
+     383             : 
+     384             : /*//{ getPrintName() */
+     385             : template <int n_measurements>
+     386          51 : std::string Correction<n_measurements>::getPrintName() const {
+     387          51 :   return ch_->nodelet_name + "/" + est_name_ + "/" + name_;
+     388             : }
+     389             : /*//}*/
+     390             : 
+     391             : /*//{ getR() */
+     392             : template <int n_measurements>
+     393       95016 : double Correction<n_measurements>::getR() {
+     394       95016 :   std::scoped_lock lock(mtx_R_);
+     395       95014 :   default_R_ = drmgr_->config.noise;
+     396      190017 :   return R_;
+     397             : }
+     398             : /*//}*/
+     399             : 
+     400             : /*//{ setR() */
+     401             : template <int n_measurements>
+     402       24815 : void Correction<n_measurements>::setR(const double R) {
+     403       24815 :   std::scoped_lock lock(mtx_R_);
+     404       24815 :   R_ = R;
+     405       24815 : }
+     406             : /*//}*/
+     407             : 
+     408             : /*//{ getStateId() */
+     409             : template <int n_measurements>
+     410       24860 : StateId_t Correction<n_measurements>::getStateId() const {
+     411       24860 :   return state_id_;
+     412             : }
+     413             : /*//}*/
+     414             : 
+     415             : /*//{ isHealthy() */
+     416             : template <int n_measurements>
+     417       14127 : bool Correction<n_measurements>::isHealthy() {
+     418             : 
+     419       14127 :   if (!is_initialized_) {
+     420           0 :     return false;
+     421             :   }
+     422             : 
+     423       14127 :   is_dt_ok_ = isMsgComing();
+     424             : 
+     425       14127 :   if (!is_delay_ok_) {
+     426           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: delay not ok", getPrintName().c_str());
+     427             :   }
+     428             : 
+     429       14127 :   if (!is_healthy_) {
+     430           0 :     if (is_dt_ok_ && is_delay_ok_) {
+     431           0 :       if (healthy_time_ > ros::Time(10)) {
+     432           0 :         is_healthy_ = true;
+     433             :       }
+     434             :     } else {
+     435           0 :       healthy_time_ = ros::Time(0);
+     436             :     }
+     437             :   }
+     438             : 
+     439       14127 :   is_healthy_ = is_healthy_ && is_dt_ok_ && is_delay_ok_;
+     440             : 
+     441       14127 :   return is_healthy_;
+     442             : }
+     443             : /*//}*/
+     444             : 
+     445             : /*//{ getRawCorrection() */
+     446             : template <int n_measurements>
+     447         233 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getRawCorrection() {
+     448             : 
+     449         233 :   if (!is_initialized_) {
+     450           0 :     return {};
+     451             :   }
+     452             : 
+     453         233 :   MeasurementStamped measurement_stamped;
+     454             : 
+     455         233 :   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          46 :     case MessageType_t::RANGE: {
+     516             : 
+     517          46 :       if (!range_enabled_) {
+     518           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: fusing range corrections is disabled", getPrintName().c_str());
+     519          16 :         return {};
+     520             :       }
+     521             : 
+     522          46 :       if (!sh_range_.hasMsg()) {
+     523          16 :         return {};
+     524             :       }
+     525             : 
+     526          30 :       auto msg                  = sh_range_.getMsg();
+     527          30 :       measurement_stamped.stamp = msg->header.stamp;
+     528             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     529             : 
+     530             :       /* if (!is_delay_ok_) { */
+     531             :       /*   return {}; */
+     532             :       /* } */
+     533             : 
+     534          30 :       auto res = getCorrectionFromRange(msg);
+     535          30 :       if (res) {
+     536          30 :         measurement_stamped.value = res.value();
+     537             :       } else {
+     538           0 :         return {};
+     539             :       }
+     540          30 :       break;
+     541             :     }
+     542             : 
+     543          73 :     case MessageType_t::RTK_GPS: {
+     544             : 
+     545          73 :       if (!sh_rtk_.hasMsg()) {
+     546           0 :         ROS_ERROR_THROTTLE(1.0, " no rtk msgs so far");
+     547          14 :         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          53 :     case MessageType_t::POINT: {
+     571             : 
+     572          53 :       if (!sh_point_.hasMsg()) {
+     573          50 :         return {};
+     574             :       }
+     575             : 
+     576           3 :       auto msg                  = sh_point_.getMsg();
+     577           3 :       measurement_stamped.stamp = msg->header.stamp;
+     578             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     579             : 
+     580             :       /* if (!is_delay_ok_) { */
+     581             :       /*   return {}; */
+     582             :       /* } */
+     583           3 :       auto res = getCorrectionFromPoint(msg);
+     584           3 :       if (res) {
+     585           3 :         measurement_stamped.value = res.value();
+     586             :       } else {
+     587           0 :         return {};
+     588             :       }
+     589           3 :       break;
+     590             :     }
+     591             : 
+     592          61 :     case MessageType_t::VECTOR: {
+     593             : 
+     594          61 :       if (!sh_vector_.hasMsg()) {
+     595          55 :         return {};
+     596             :       }
+     597             : 
+     598           9 :       auto msg                  = sh_vector_.getMsg();
+     599           9 :       measurement_stamped.stamp = msg->header.stamp;
+     600             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     601             : 
+     602             :       /* if (!is_delay_ok_) { */
+     603             :       /*   return {}; */
+     604             :       /* } */
+     605           9 :       auto res = getCorrectionFromVector(msg);
+     606           9 :       if (res) {
+     607           6 :         measurement_stamped.value = res.value();
+     608             :       } else {
+     609           3 :         return {};
+     610             :       }
+     611           6 :       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          98 :   is_nan_free_ = true;
+     645         241 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+     646         143 :     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          98 :   got_first_msg_ = true;
+     654          98 :   publishCorrection(measurement_stamped, ph_correction_raw_);
+     655             : 
+     656          98 :   return measurement_stamped;
+     657             : }
+     658             : /*//}*/
+     659             : 
+     660             : /*//{ getProcessedCorrection() */
+     661             : template <int n_measurements>
+     662         233 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getProcessedCorrection() {
+     663             : 
+     664         233 :   MeasurementStamped measurement_stamped;
+     665         233 :   auto               res = getRawCorrection();
+     666         233 :   if (res) {
+     667          98 :     MeasurementStamped measurement_stamped = res.value();
+     668          98 :     if (process(measurement_stamped.value)) {
+     669          69 :       publishCorrection(measurement_stamped, ph_correction_proc_);
+     670          69 :       return measurement_stamped;
+     671             :     } else {
+     672          29 :       return {};  // invalid correction
+     673             :     }
+     674             :   } else {
+     675         135 :     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        2121 : void Correction<n_measurements>::callbackRange(const sensor_msgs::Range::ConstPtr msg) {
+     943             : 
+     944        2121 :   if (!is_initialized_) {
+     945           0 :     return;
+     946             :   }
+     947             : 
+     948        2121 :   auto res = getCorrectionFromRange(msg);
+     949        2121 :   if (res) {
+     950        2121 :     applyCorrection(res.value(), msg->header.stamp);
+     951             :   }
+     952             : }
+     953             : /*//}*/
+     954             : 
+     955             : /*//{ getCorrectionFromRange() */
+     956             : template <int n_measurements>
+     957        2151 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg) {
+     958             : 
+     959        2151 :   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        2151 :   if (!std::isfinite(msg->range)) {
+     965           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: received value: %f. Not using this correction.", getPrintName().c_str(), msg->range);
+     966           0 :     return {};
+     967             :   }
+     968             : 
+     969        4302 :   geometry_msgs::PoseStamped range_point;
+     970             : 
+     971        2151 :   range_point.header           = msg->header;
+     972        2151 :   range_point.pose.position.x  = msg->range;
+     973        2151 :   range_point.pose.position.y  = 0;
+     974        2151 :   range_point.pose.position.z  = 0;
+     975        2151 :   range_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     976             : 
+     977        4302 :   auto res = ch_->transformer->transformSingle(range_point, ch_->frames.ns_fcu_untilted);
+     978             : 
+     979        2151 :   Correction::measurement_t measurement;
+     980             : 
+     981        2151 :   if (res) {
+     982        2151 :     measurement(0) = -res.value().pose.position.z;
+     983        2151 :     return measurement;
+     984             :   } else {
+     985           0 :     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           0 :     return {};
+     988             :   }
+     989             : }
+     990             : /*//}*/
+     991             : 
+     992             : /*//{ callbackRtk() */
+     993             : template <int n_measurements>
+     994        2011 : void Correction<n_measurements>::callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg) {
+     995             : 
+     996        2011 :   if (!is_initialized_) {
+     997           1 :     return;
+     998             :   }
+     999             : 
+    1000        2010 :   auto res = getCorrectionFromRtk(msg);
+    1001        2010 :   if (res) {
+    1002        2002 :     applyCorrection(res.value(), msg->header.stamp);
+    1003             :   }
+    1004             : }
+    1005             : /*//}*/
+    1006             : 
+    1007             : /*//{ getCorrectionFromRtk() */
+    1008             : template <int n_measurements>
+    1009        2083 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg) {
+    1010             : 
+    1011        4166 :   geometry_msgs::PoseStamped rtk_pos;
+    1012             : 
+    1013        2083 :   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        2083 :   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        2083 :   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        2083 :   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        2083 :   rtk_pos.header = msg->header;
+    1034        2083 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+    1035        2083 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+    1036        2083 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1037             : 
+    1038        2083 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+    1039        2083 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+    1040             : 
+    1041        2083 :   Correction::measurement_t measurement;
+    1042             : 
+    1043             :   // transform the RTK position from antenna to FCU
+    1044        2083 :   auto res = transformRtkToFcu(rtk_pos);
+    1045        2083 :   if (res) {
+    1046        2083 :     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        2083 :   switch (est_type_) {
+    1053             : 
+    1054             :     // handle lateral estimators
+    1055        1048 :     case EstimatorType_t::LATERAL: {
+    1056             : 
+    1057        1048 :       switch (state_id_) {
+    1058             : 
+    1059        1048 :         case StateId_t::POSITION: {
+    1060        1048 :           measurement(0) = rtk_pos.pose.position.x;
+    1061        1048 :           measurement(1) = rtk_pos.pose.position.y;
+    1062        1048 :           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        1035 :     case EstimatorType_t::ALTITUDE: {
+    1076             : 
+    1077        1035 :       switch (state_id_) {
+    1078             : 
+    1079        1035 :         case StateId_t::POSITION: {
+    1080        1035 :           measurement(0) = rtk_pos.pose.position.z;
+    1081        1035 :           if (!got_avg_init_rtk_z_) {
+    1082          22 :             getAvgRtkInitZ(measurement(0));
+    1083          22 :             return {};
+    1084             :           }
+    1085        1013 :           measurement(0) -= rtk_init_z_avg_;
+    1086        1013 :           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        6938 : void Correction<n_measurements>::callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg) {
+    1113             : 
+    1114        6938 :   if (!is_initialized_) {
+    1115           0 :     return;
+    1116             :   }
+    1117             : 
+    1118        6938 :   auto res = getCorrectionFromPoint(msg);
+    1119        6938 :   if (res) {
+    1120        6938 :     applyCorrection(res.value(), msg->header.stamp);
+    1121             :   }
+    1122             : }
+    1123             : /*//}*/
+    1124             : 
+    1125             : /*//{ getCorrectionFromPoint() */
+    1126             : template <int n_measurements>
+    1127        6941 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromPoint(
+    1128             :     const geometry_msgs::PointStampedConstPtr msg) {
+    1129             : 
+    1130        6941 :   switch (est_type_) {
+    1131             : 
+    1132             :     // handle lateral estimators
+    1133        4610 :     case EstimatorType_t::LATERAL: {
+    1134             : 
+    1135        4610 :       switch (state_id_) {
+    1136             : 
+    1137        4610 :         case StateId_t::POSITION: {
+    1138        4610 :           measurement_t measurement;
+    1139        4610 :           measurement(0) = msg->point.x;
+    1140        4610 :           measurement(1) = msg->point.y;
+    1141        4610 :           return measurement;
+    1142             :           break;
+    1143             :         }
+    1144             : 
+    1145           0 :         default: {
+    1146           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1147           0 :           return {};
+    1148             :         }
+    1149             :       }
+    1150             :       break;
+    1151             :     }
+    1152             : 
+    1153             :     // handle altitude estimators
+    1154        2331 :     case EstimatorType_t::ALTITUDE: {
+    1155             : 
+    1156        2331 :       switch (state_id_) {
+    1157             : 
+    1158        2331 :         case StateId_t::POSITION: {
+    1159        2331 :           measurement_t measurement;
+    1160        2331 :           measurement(0) = msg->point.z;
+    1161        2331 :           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       13764 : void Correction<n_measurements>::callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+    1188             : 
+    1189       13764 :   if (!is_initialized_) {
+    1190           0 :     return;
+    1191             :   }
+    1192             : 
+    1193       13764 :   auto res = getCorrectionFromVector(msg);
+    1194       13764 :   if (res) {
+    1195       13755 :     applyCorrection(res.value(), msg->header.stamp);
+    1196             :   }
+    1197             : }
+    1198             : /*//}*/
+    1199             : 
+    1200             : /*//{ getCorrectionFromVector() */
+    1201             : template <int n_measurements>
+    1202       13773 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromVector(
+    1203             :     const geometry_msgs::Vector3StampedConstPtr msg) {
+    1204             : 
+    1205       13773 :   switch (est_type_) {
+    1206             : 
+    1207             :     // handle lateral estimators
+    1208        4626 :     case EstimatorType_t::LATERAL: {
+    1209             : 
+    1210        4626 :       switch (state_id_) {
+    1211             : 
+    1212        4626 :         case StateId_t::VELOCITY: {
+    1213        4626 :           auto res = getVelInFrame(msg->vector, msg->header, ns_frame_id_ + "_att_only");
+    1214        4626 :           if (res) {
+    1215        4614 :             measurement_t measurement;
+    1216        4614 :             measurement = res.value();
+    1217        4614 :             return measurement;
+    1218             :           } else {
+    1219          12 :             return {};
+    1220             :           }
+    1221             :           break;
+    1222             :         }
+    1223             : 
+    1224           0 :         default: {
+    1225           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1226           0 :           return {};
+    1227             :         }
+    1228             :       }
+    1229             :       break;
+    1230             :     }
+    1231             : 
+    1232             :     // handle altitude estimators
+    1233        9147 :     case EstimatorType_t::ALTITUDE: {
+    1234             : 
+    1235        9147 :       switch (state_id_) {
+    1236             : 
+    1237        9147 :         case StateId_t::VELOCITY: {
+    1238        9147 :           auto res = getZVelUntilted(msg->vector, msg->header);
+    1239        9147 :           if (res) {
+    1240        9147 :             measurement_t measurement;
+    1241        9147 :             measurement = res.value();
+    1242        9147 :             return measurement;
+    1243             :           } else {
+    1244           0 :             return {};
+    1245             :           }
+    1246             :           break;
+    1247             :         }
+    1248             : 
+    1249           0 :         default: {
+    1250           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1251           0 :           return {};
+    1252             :         }
+    1253             :       }
+    1254             :       break;
+    1255             :     }
+    1256             : 
+    1257             :     // handle heading estimators
+    1258           0 :     case EstimatorType_t::HEADING: {
+    1259             : 
+    1260           0 :       switch (state_id_) {
+    1261             : 
+    1262           0 :           case StateId_t::VELOCITY: {
+    1263             :             try {
+    1264           0 :               if (!sh_orientation_.hasMsg()) {
+    1265           0 :               ROS_INFO_THROTTLE(1.0, "[%s]: %s orientation on topic: %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), orientation_topic_.c_str());
+    1266           0 :                 return {};
+    1267             :               }
+    1268           0 :               measurement_t measurement;
+    1269           0 :               measurement(0) = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+    1270           0 :               return measurement;
+    1271             :             }
+    1272           0 :             catch (...) {
+    1273           0 :               ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromVector())", getPrintName().c_str());
+    1274           0 :               return {};
+    1275             :             }
+    1276             :             break;
+    1277             :           }
+    1278             : 
+    1279           0 :         default: {
+    1280           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1281           0 :           return {};
+    1282             :         }
+    1283             :       }
+    1284             :       break;
+    1285             :     }
+    1286             :   }
+    1287             : 
+    1288           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1289           0 :   return {};
+    1290             : }
+    1291             : /*//}*/
+    1292             : 
+    1293             : /*//{ getCorrectionFromQuat() */
+    1294             : template <int n_measurements>
+    1295           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromQuat(
+    1296             :     const geometry_msgs::QuaternionStampedConstPtr msg) {
+    1297             : 
+    1298           0 :   switch (est_type_) {
+    1299             : 
+    1300             :       // handle lateral estimators
+    1301             :       /* case EstimatorType_t::LATERAL: { */
+    1302             : 
+    1303             :       /*   default: { */
+    1304             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1305             :       /*     return {}; */
+    1306             :       /*   } break; */
+    1307             :       /* } */
+    1308             : 
+    1309             :       /* // handle altitude estimators */
+    1310             :       /* case EstimatorType_t::ALTITUDE: { */
+    1311             : 
+    1312             :       /* default: { */
+    1313             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1314             :       /*     return {}; */
+    1315             :       /*   } break; */
+    1316             :       /* } */
+    1317             : 
+    1318             :       /* // handle heading estimators */
+    1319             :       /* case EstimatorType_t::HEADING: { */
+    1320             : 
+    1321             :       /*   switch (state_id_) { */
+    1322             : 
+    1323             :       /*     /1* default: { *1/ */
+    1324             :       /*       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1325             :       /*       return {}; */
+    1326             :       /*     } */
+    1327             :       /*   } */
+    1328             :     default: {
+    1329           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1330           0 :       return {};
+    1331             :     }
+    1332             :   }
+    1333             : 
+    1334             :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1335             :   return {};
+    1336             : }
+    1337             : /*//}*/
+    1338             : 
+    1339             : /*//{ applyCorrection() */
+    1340             : template <int n_measurements>
+    1341       24815 : void Correction<n_measurements>::applyCorrection(const measurement_t& meas, const ros::Time& stamp) {
+    1342             : 
+    1343             :   {
+    1344       24815 :     std::scoped_lock lock(mtx_msg_time_);
+    1345       24816 :     if (first_timestamp_) {
+    1346          14 :       prev_msg_time_   = stamp - ros::Duration(0.01);
+    1347          14 :       msg_time_        = stamp;
+    1348          14 :       healthy_time_    = ros::Time(0);
+    1349          14 :       first_timestamp_ = false;
+    1350             :     }
+    1351             : 
+    1352       24814 :     prev_msg_time_ = msg_time_;
+    1353       24814 :     msg_time_      = stamp;
+    1354       24814 :     healthy_time_ += msg_time_ - prev_msg_time_;
+    1355             :   }
+    1356             : 
+    1357       24815 :   MeasurementStamped meas_stamped;
+    1358       24815 :   meas_stamped.value = meas;
+    1359       24816 :   meas_stamped.stamp = stamp;
+    1360       24816 :   publishCorrection(meas_stamped, ph_correction_raw_);
+    1361       24815 :   if (process(meas_stamped.value)) {
+    1362       24746 :     publishCorrection(meas_stamped, ph_correction_proc_);
+    1363       24745 :     fun_apply_correction_(meas_stamped, getR(), getStateId());
+    1364             :   }
+    1365       24815 : }
+    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        9147 : 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       18294 :   geometry_msgs::PointStamped vel;
+    1408        9147 :   vel.point.x = msg.x;
+    1409        9147 :   vel.point.y = msg.y;
+    1410        9147 :   vel.point.z = msg.z;
+    1411        9147 :   vel.header  = header;
+    1412             :   /* vel.header.frame_id = ch_->frames.ns_fcu; */
+    1413        9147 :   vel.header.stamp = header.stamp;
+    1414             : 
+    1415       18294 :   auto res = ch_->transformer->transformSingle(vel, ch_->frames.ns_fcu_untilted);
+    1416        9147 :   if (res) {
+    1417        9147 :     measurement_t measurement;
+    1418        9147 :     measurement(0) = res.value().point.z;
+    1419        9147 :     return measurement;
+    1420             :   } else {
+    1421           0 :     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           0 :     return {};
+    1423             :   }
+    1424             : }
+    1425             : /*//}*/
+    1426             : 
+    1427             : /*//{ getVelInFrame() */
+    1428             : template <int n_measurements>
+    1429        4626 : 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        4626 :   measurement_t measurement;
+    1434             : 
+    1435        9252 :   geometry_msgs::Vector3Stamped vel;
+    1436        4626 :   vel.header = source_header;
+    1437        4626 :   vel.vector = vel_in;
+    1438             :   /* body.vector.x = vel.x; */
+    1439             :   /* body.vector.y = vel.y; */
+    1440             :   /* body.vector.z = vel.z; */
+    1441             : 
+    1442        9252 :   geometry_msgs::Vector3Stamped transformed_vel;
+    1443        9252 :   auto                          res = ch_->transformer->transformSingle(vel, target_frame);
+    1444        4626 :   if (res) {
+    1445        4614 :     transformed_vel = res.value();
+    1446        4614 :     measurement(0)  = transformed_vel.vector.x;
+    1447        4614 :     measurement(1)  = transformed_vel.vector.y;
+    1448        4614 :     return measurement;
+    1449             :   } else {
+    1450          12 :     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          12 :     return {};
+    1452             :   }
+    1453             : }
+    1454             : /*//}*/
+    1455             : 
+    1456             : /*//{ transformRtkToFcu() */
+    1457             : template <int n_measurements>
+    1458        2083 : std::optional<geometry_msgs::Pose> Correction<n_measurements>::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+    1459             : 
+    1460        4166 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+    1461             : 
+    1462             :   // inject current orientation into rtk pose
+    1463        4166 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+    1464        2083 :   if (res1) {
+    1465        2083 :     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        2083 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+    1474        4166 :   geometry_msgs::PoseStamped utm_in_antenna;
+    1475        2083 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+    1476        2083 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+    1477        2083 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+    1478             : 
+    1479             :   // transform to fcu
+    1480        4166 :   geometry_msgs::PoseStamped utm_in_fcu;
+    1481        2083 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+    1482        2083 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+    1483        4166 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+    1484             : 
+    1485        2083 :   if (res2) {
+    1486        2083 :     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        2083 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+    1494        2083 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+    1495             : 
+    1496        2083 :   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       14127 : bool Correction<n_measurements>::isMsgComing() {
+    1589             : 
+    1590       14127 :   const ros::Time msg_time = mrs_lib::get_mutexed(mtx_msg_time_, msg_time_);
+    1591       14127 :   const double    delta    = ros::Time::now().toSec() - msg_time.toSec();
+    1592             : 
+    1593       14127 :   if (msg_time.toSec() <= 0.0) {
+    1594           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: current timestamp non-positive: %f", getPrintName().c_str(), msg_time.toSec());
+    1595           0 :     return false;
+    1596             :   }
+    1597             : 
+    1598       14127 :   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       14127 :   return true;
+    1604             : }  // namespace mrs_uav_state_estimators
+    1605             : /*//}*/
+    1606             : 
+    1607             : /*//{ createProcessorFromName() */
+    1608             : template <int n_measurements>
+    1609           9 : std::shared_ptr<Processor<n_measurements>> Correction<n_measurements>::createProcessorFromName(const std::string& name, ros::NodeHandle& nh) {
+    1610             : 
+    1611           9 :   if (name == "median_filter") {
+    1612           1 :     return std::make_shared<ProcMedianFilter<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1613           8 :   } else if (name == "saturate") {
+    1614           5 :     return std::make_shared<ProcSaturate<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_, state_id_, fun_get_state_);
+    1615           3 :   } else if (name == "excessive_tilt") {
+    1616           1 :     return std::make_shared<ProcExcessiveTilt<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1617           2 :   } else if (name == "tf_to_world") {
+    1618           2 :     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       24914 : bool Correction<n_measurements>::process(Correction<n_measurements>::measurement_t& measurement) {
+    1630             : 
+    1631       24914 :   bool ok_flag   = true;
+    1632       24914 :   bool fuse_flag = true;
+    1633             : 
+    1634       38038 :   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       13124 :     auto [is_ok, should_fuse] = processors_[proc_name]->process(measurement);
+    1638       13124 :     ok_flag &= is_ok;
+    1639       13124 :     fuse_flag &= should_fuse;
+    1640             :   }
+    1641       24912 :   if (fuse_flag) {
+    1642       24813 :     if (!ok_flag) {
+    1643           0 :       setR(default_R_ * R_coeff_);
+    1644           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: set R to %.4f", getPrintName().c_str(), default_R_ * R_coeff_);
+    1645           0 :       return true;
+    1646             :     } else {
+    1647       24813 :       setR(default_R_);
+    1648       24815 :       return true;
+    1649             :     }
+    1650             :   }
+    1651          99 :   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       49729 : void Correction<n_measurements>::publishCorrection(const MeasurementStamped&                                 measurement_stamped,
+    1670             :                                                    mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr) {
+    1671             : 
+    1672       49729 :   if (!ch_->debug_topics.correction) {
+    1673           0 :     return;
+    1674             :   }
+    1675             : 
+    1676       99456 :   mrs_msgs::EstimatorCorrection msg;
+    1677       49727 :   msg.header.stamp    = measurement_stamped.stamp;
+    1678       49727 :   msg.header.frame_id = ns_frame_id_;
+    1679       49728 :   msg.name            = name_;
+    1680       49729 :   msg.estimator_name  = est_name_;
+    1681       49728 :   msg.state_id        = state_id_;
+    1682       49728 :   msg.covariance.resize(n_measurements * n_measurements);
+    1683      119985 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+    1684       70264 :     msg.state.push_back(measurement_stamped.value(i));
+    1685       70271 :     msg.covariance[n_measurements * i + i] = getR();
+    1686             :   }
+    1687             : 
+    1688       49721 :   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..9660cbdbf0b3240c3a1723e0f7ae69a4912f666b GIT binary patch literal 5110 zcmV_F@Ss~uQSs~fL*?*O*^#zHa{HfEN_3quZDMf$)A<(D0-EPC}*6Gay zrZ#K`G`9s!@3C!1R$`28e?Xe^5|EV-fsyXL2DD)0x^2Q}ZWv=z*Nljcrcpp=23*wqmxC*Wb&VSxXL9F4uVbbTU58#h+C!Kge)dv;Nt4w--avT{aBS7wj3 z0?^vEW&=mk*y5+XeA6IGfF7u~DA zgRQsshr3QDBK~%p%N;B}D6^};nY%W&rjdaIGbH)<;jbR5MNj&h0qSZ3tl=Wq(5Ptn zmlRPV?UJAoV82IzfAbcmV{#;=e&r+p7>j)tH2z<8Q4@cROp^ml%5rmVg;x) zuA>&JX&Uoj4jIsdgQc+s+<`_!>Q;=@L_lN3{R&*85BmTp6V%6X388D)x5x|t+{X%UK+F0%rfBG6b=Ly~ z$+gJSHeOdyp6UUI;0t>0EhUG37k{eK zXl}M)u8fPxMK(q41v<ypX9&N3TZj9a{xEN^X*^a%qc+?(3pyU@NN3} zZ~3#dvAfr8{P{e=`oE?J)d`gcB{?ATpdHG;8yGi@x=Kq8XuxRbxDimN@@FLI@tg2) zQA0?!pN@Mpqvj|*j7hVPxo0g|8sD{{dKp2$)$K$7u`> zYFgw!Eo04M#AsVgVNiW2(C|Xq;opMmEEdlZCvavA%Tes^3;z~oN_~8h+IWaQHh7BB zi!R$3DWQ(`dRbay(jG>@6~;zyR;U)yi=>Yu9LQ`l8jUtqunz|-3Q#?e6c>nbMc4Nf zIArCe#t;wTSUJ@x_I|(+X7MEHfNCq4;_g`nHHDG42lhSUIFbUV0gVkX17_-;VW!eT zmc}IXiBj%@nE)6hqHqnJ8VfiDjI4hwyH)`wD>Dvh?Rc}f8k@uDwMKT(&f@|P{GJ)d z`UB{E^9uqO?G=3ZQ}y2gqs@sIG;$PX8bu<1g~p$+B^qDLy!XS2h|&3qU`98g9l*@u zAA!$dRVXe2vSJfK!_qvU|DiV+5ip=EEjAN0KH%zui)4V!(7mCcJz%sLmvLGs<;V&$ zbWQU5h(cTbKAuK>ta&1TOOEV#C#B$K%mZm`byAv-I^9VDr|$m`A-nfohtkMX#?u%H z1=M-M>|GzRBfsy#p151wYwZ5%QGisfKGJt!iT?mAB&y}Xg@8Y*7PSq0nZ*;nn~~_M zcEFmeQI15|oOX+0IjX(_E&;f?D0&KjO9Zs870fjHkr9VwK5AqZf%h@B9c7+%nHRWk z)ihG(02X&LL#2HMZqCgCpmAmgv^{w>@yi4cXjGniC((rf?L!?0RR?TI?o;`rLf7Lm zMPWgg7V*>> zD=`Tez*(;Am{|Z+Q-xnBaAJyE^mD{wZxQ%7T14~#vw|+lCc7rhdc^TUwrho@^VMwM ziZ~TX)$8$=;3LAT;%N+Zvbccb zMkYdn1p@`U06r6#z}Nz4;=6O(BrHcNW6y~~KG%HOialf>W2zD@z;Wnm*GHmQO#yy@ zH&gw6TM&n4L(1*9ZIfv8X1ewqwtz%TGeu-G*TzDQ8DmSu$0nX<_B{f=?0eL)Y4JVA z_VjG0-^@;)U%gdKz3)K%AT?E*@HHH}P3tiagP&T`G>7UWp$yP&q?dGV;*hNDtc5~l!7 z`;ZUt;rHElku(V0zO~5K^&KfLFVE8V%yY<^l?lX;v}@vQ%Ac|8kZ=w+ISu4}y&vTO z=)KpcIe-GAE8#ez-GA|;kdr!7aX-SP>>bWr5x`%J0PfesE;Dnm6^eUZZC&57KFSzB z&I-%3dzTe5qJdnWIJ@qsJ@@mh@aP=re^n%@(xWazv69B_(-M{R$Xtm^9?;y@oCSEt zRM?!;U~#MQd`SNB%+m#acee42_%eMcj?^7#AIFI}!H>6IGLvf=UXUZ>w|b{--|kNa ze90S;{=~?{sa2CWc_x?IIBubl>C_rx+>DVAaG&wanZY|Butm$Z*(5Tf#~$DSU1%=`tDPCM>KyKx`PjKZ%?Qa5 zy4Ip*sm`g5j1gllUn}>5mZfuPKdzCPr;VK^jHcDA$A|_&2P{ly-U#?2ensr7NlnaEB#Dlhf4yw)B1uu53%^sUlAW2WnxbL+rBu^)z|YS~T@(BEcI9#RG_K2L zSd3q2GuMQ~0$w>ONjz6h>c7ZIMV>E@yA96VzBK)y1bgm~!X75v%h z%+!Xk{fJw{ifYnLbF}}{$#C3flw=|f?>W5&N^SG@vw)#iq zh;awbhlrkX&EueNVErMM%6?#mDM;}LJC z8m_0gVhXLueQHcmikp4h$&8VBQJVrRom9+Jpp3=o6T9g!;NNBDOGaia&sL8#Gq1?S zj5$1=nLxD?1ypC7C*rGzd(;wsmt8dRW;~J_7s9OzU5lr`*8{HUdY@>oiw`jO@eyYC zeA`fYX)1JOXl$Gdb*0!677}KCx+&~?IlsA5QQrT^QP6fxl!cupjEG8h*?jkqwOJFl zHMRk2E0qMOGPATR*DSU3t9wdCys{boaZp?PLpGyd02gR{;Kp3xdAHp42xQ+774%VxgcW^7XAmH?F$-%U$^^X_#uw-m_4 zBcc9&!alqXGTi(J4zRZj;b+8V@_Eu7mH;>q`~D>Wqn-HnZbH;Z%;yWWlZb>Ifx)J)UzkQi*Velg&oRWv^5xC8 z!I|5amQ$6Is(F!%-B{g}3u?SRq6=Wn?T8C%taDs>L7n>Kn4$T%d<}T`%isD;QU)|$ z{${$=ZRgj4yZC5ZpG3~$F9p7xl$%lB-zxB4Npu~f;lobI0>-Y7!zf~0ex{Bg0My5D z>Dn|l)|(X_SvuJBt4bQhczjExLV17kwz*Ofe=xZ}4S8HE&pElTy>n(Pet^fu+)?0> z!y5sYPZuCDp%D>CrD62WnTGL*cbikka@XPKB88yr5{{cBIo5zj9G1jN!P1xk#itGn zK&fh9=s*7wbZIku0fI+aKBrf<34jAYtH8}~EJ_-K0L?8N0*Y>ThXb8V`caJ zxC-^9S5yzV3N_Lg=3PF^Mabf}XbelN7t?2^(b~0tTGq{UgHw+_#gVZ#&^D_+!!T=u z2mtdJFZe~^aE+Ro?!2;Kj1o_$GofkB5YULx{HUc};AR>R(MRHfv>Ky1MPhDy$9l&^ zd*$>>&rdB?dowqEG~wYUqNxe9bYdnnH#57hct};~|!;thEXq>nW_wsE%t-#tY6T z07msz7Znar>w3&KGYQ7k#pqhi@0kG`1ztH4ifi;i+FyX$xWf0yI~5?{9pCk^5pY*0 zg_e6`5TsvhPwp71KZ!bZmOWCTYeT>H&)NSd>+K3Bm1Ie^O&9%4w1!(T7WJON-;zNv z<>qVBPJnVg;(RS`0!}od->pz*z|A+N!D$v$#Ftht-9eG=%sVu~$mno)B(HqCWT|cB z1&7KlgMJvf%lyh<7y&XEN#dYV*jl>NrM|?45f9{=#8?1wo4JcCZ&;pVjGHWhlVbBe z`v&7u!u4H@E0$iuMV^1nX2Lj9o(-Eq_=t9aZ9bgWLI7yaksdl1{>qV)tkf&7s1r=0 zWm9Yznq;ZjWLIMZ{&<4l&csg4-QA?=8{@R zj3#DGt=m5f2$%^bEs7`0hGUBs&xG}U^y0CW*mmFZ^%s`*H&~uQEu`6(zuLHnrFFN| z%!|&N6gPdK`#xEe2vVX4V9WbX!tRqKkPO(X1q8b(VA;hee*iC(R9(vm%?jB_;(k2? zp7%reXKiMK%gh)xWAoajV?1W>OnUZJ8ghHrz+PmrhwjvEl3f&{Hi``jS$Z?%vaDzz z#_Sdq%LV=08BT@QmF!Au)txXS(Cu6Fgq({u15%-e;an^FeLZTn!JbI>39*KdMUcuuPi=fowV=R1(aiY2`0V77s z8J|2fajfw98CM4y$EluJQDjLSv>^pl0Zt2QVjJI~N1&7m=_8!Ia`4pb0NAQjaITFN zDTlJe$QPT80y^$=YLJr-7&GE5$ysoN1QwF}?kjmizP-Uwli*G)4Vklr=C&X}n#REW Y0|yziOX-va#Q*>R07*qoM6N<$f=eXD5&!@I 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..790f49711f --- /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:2023-11-30 22:31:42Functions: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..b98f50dad1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2023-11-30 22:31:42Functions: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..383ba127c6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html @@ -0,0 +1,240 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2023-11-30 22:31:42Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..184fcd89b0f359d12d16f08ceb9b076b8700fff8 GIT binary patch literal 655 zcmV;A0&x9_P)0{{R38Wk~70000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpSTPn!rauAj>G z&`{Epo6QEOh>?TlaS6n$y7Q}{^By{A + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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)4
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()4
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().24
+
+
+ + + +
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..95391db1e0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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)4
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()4
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().24
+
+
+ + + +
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..00d9f69a28 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html @@ -0,0 +1,191 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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           4 :   HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+      70           4 :       : HeadingEstimator<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+      71           4 :   }
+      72             : 
+      73           8 :   ~HdgPassthrough(void) {
+      74           8 :   }
+      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..0e37266d9b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2023-11-30 22:31:42Functions: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&)4
+
+
+ + + +
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..d47e702ed8 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2023-11-30 22:31:42Functions: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&)4
+
+
+ + + +
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..b928d85724 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2023-11-30 22:31:42Functions: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           4 :   HeadingEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(heading::type, name, frame_id) {
+      23           4 :   }
+      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..6537bcef66 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2023-11-30 22:31:42Functions: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..24463c4a8c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2023-11-30 22:31:42Functions: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..55719b317e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2023-11-30 22:31:42Functions: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..8326c7b63f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2023-11-30 22:31:42Functions: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..6e021c4d88 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2023-11-30 22:31:42Functions: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..a22c874520 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2023-11-30 22:31:42Functions: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..887f64c00a --- /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:39469157.0 %
Date:2023-11-30 22:31:42Functions:689174.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
55.0%55.0%
+ 		55.0 %360 / 65573.1 %49 / 67
<unnamed>55.0 %360 / 65573.1 %49 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html new file mode 100644 index 0000000000..b3f45e54c5 --- /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:39469157.0 %
Date:2023-11-30 22:31:42Functions:689174.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
55.0%55.0%
+ 		55.0 %360 / 65573.1 %49 / 67
<unnamed>55.0 %360 / 65573.1 %49 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html new file mode 100644 index 0000000000..a052a2ee33 --- /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:39469157.0 %
Date:2023-11-30 22:31:42Functions:689174.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
55.0%55.0%
+ 		55.0 %360 / 65573.1 %49 / 67
<unnamed>55.0 %360 / 65573.1 %49 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html new file mode 100644 index 0000000000..8e08de7a94 --- /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:39469157.0 %
Date:2023-11-30 22:31:42Functions:689174.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
55.0%55.0%
+ 		55.0 %360 / 65573.1 %49 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html new file mode 100644 index 0000000000..c3b8f37caa --- /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:39469157.0 %
Date:2023-11-30 22:31:42Functions:689174.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
55.0%55.0%
+ 		55.0 %360 / 65573.1 %49 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html new file mode 100644 index 0000000000..0d604dbd24 --- /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:39469157.0 %
Date:2023-11-30 22:31:42Functions:689174.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
55.0%55.0%
+ 		55.0 %360 / 65573.1 %49 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html new file mode 100644 index 0000000000..67e24d7ddc --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..16987508ec --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..26409e6ecb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..4798483865 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..34b9492670 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..76ba1ab844 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2023-11-30 22:31:42Functions: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..70002deb97 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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> ()>)4
mrs_uav_state_estimators::LatGeneric::~LatGeneric()4
mrs_uav_state_estimators::LatGeneric::~LatGeneric().24
+
+
+ + + +
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..566f52b9d3 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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> ()>)4
mrs_uav_state_estimators::LatGeneric::~LatGeneric()4
mrs_uav_state_estimators::LatGeneric::~LatGeneric().24
+
+
+ + + +
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..c693ef573f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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           4 :   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           4 :       : 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           4 :   }
+     121             : 
+     122           8 :   ~LatGeneric(void) {
+     123           8 :   }
+     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..fbbd82fd89 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().24
+
+
+ + + +
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..e12490fc73 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().24
+
+
+ + + +
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..450418df52 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-11-30 22:31:42Functions: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           4 :   LateralEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, lateral::n_axes>(lateral::type, name, frame_id) {
+      24           4 :   }
+      25             : 
+      26           4 :   ~LateralEstimator(void) {
+      27           4 :   }
+      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..e5397bf981 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().24
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&)4
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().24
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&)4
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().24
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const3164
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&) const4027
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const4027
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const4027
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const4027
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&) const4054
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const4054
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const4054
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const4054
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const4073
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const4073
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const4073
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const61707
+
+
+ + + +
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..061e901d56 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().24
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&)4
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().24
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&)4
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().24
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() const4073
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const4073
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const4073
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&) const4027
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const4027
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const3164
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const4027
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const4027
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&) const4054
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const4054
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const61707
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const4054
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const4054
+
+
+ + + +
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..b20f6558ea --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html @@ -0,0 +1,264 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2023-11-30 22:31:42Functions: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          12 :   PartialEstimator(const std::string &type, const std::string &name, const std::string &frame_id) : Estimator(type, name, frame_id) {
+      62          12 :   }
+      63             : 
+      64          12 :   ~PartialEstimator(void) {
+      65          12 :   }
+      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       12154 : std::vector<double> PartialEstimator<n_states, n_axes>::getStatesAsVector(void) const {
+     103       12154 :   const states_t      states = getStates();
+     104       12154 :   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       56705 :   for (int i = 0; i < states.size(); i++) {
+     109       44551 :     states_vec.push_back(states(i));
+     110             :   }
+     111       24308 :   return states_vec;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ getCovarianceAsvector() */
+     116             : template <int n_states, int n_axes>
+     117       12154 : std::vector<double> PartialEstimator<n_states, n_axes>::getCovarianceAsVector(void) const {
+     118       12154 :   const covariance_t  covariance = getCovarianceMatrix();
+     119       12154 :   std::vector<double> covariance_vec;
+     120             :   /* for (auto cov : covariance.reshaped<Eigen::RowMajor>(covariance.size())) { */
+     121             :   /*   covariance_vec.push_back(*cov); */
+     122             :   /* } */
+     123       56705 :   for (int i = 0; i < covariance.rows(); i++) {
+     124      243030 :     for (int j = 0; j < covariance.cols(); j++) {
+     125      198479 :       covariance_vec.push_back(covariance(i, j));
+     126             :     }
+     127             :   }
+     128       24308 :   return covariance_vec;
+     129             : }
+     130             : /*//}*/
+     131             : 
+     132             : /*//{ stateIdToIndex() */
+     133             : template <int n_states, int n_axes>
+     134       64871 : int PartialEstimator<n_states, n_axes>::stateIdToIndex(const int &state_id_in, const int &axis_in) const {
+     135       64871 :   return state_id_in * _n_axes_ + axis_in;
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ publishOutput() */
+     140             : template <int n_states, int n_axes>
+     141       12154 : void PartialEstimator<n_states, n_axes>::publishOutput() const {
+     142             : 
+     143       12154 :   if (!ch_->debug_topics.output) {
+     144           0 :     return;
+     145             :   }
+     146             : 
+     147       24308 :   mrs_msgs::EstimatorOutput msg;
+     148       12154 :   msg.header.stamp    = ros::Time::now();
+     149       12154 :   msg.header.frame_id = getFrameId();
+     150       12154 :   msg.state           = getStatesAsVector();
+     151       12154 :   msg.covariance      = getCovarianceAsVector();
+     152             : 
+     153       12154 :   ph_output_.publish(msg);
+     154             : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ publishInput() */
+     158             : template <int n_states, int n_axes>
+     159             : template <typename u_t>
+     160        8081 : void PartialEstimator<n_states, n_axes>::publishInput(const u_t &u, const ros::Time& stamp) const {
+     161             : 
+     162        8081 :   if (!ch_->debug_topics.input) {
+     163           0 :     return;
+     164             :   }
+     165             : 
+     166       16162 :   mrs_msgs::Float64ArrayStamped msg;
+     167        8081 :   msg.header.stamp = stamp;
+     168       20216 :   for (int i = 0; i < u.rows(); i++) {
+     169       12135 :     msg.values.push_back(u(i));
+     170             :   }
+     171             : 
+     172        8081 :   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:55100.0 %
Date:2023-11-30 22:31:42Functions: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
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-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..43d2a00f71 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/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/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:55100.0 %
Date:2023-11-30 22:31:42Functions: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
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..074a7c7b4c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + 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:55100.0 %
Date:2023-11-30 22:31:42Functions: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
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..0cf49b5aae --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/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/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:55100.0 %
Date:2023-11-30 22:31:42Functions: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
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-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..df98e2e381 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/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/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:55100.0 %
Date:2023-11-30 22:31:42Functions: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
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..58c58e18e2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html @@ -0,0 +1,102 @@ + + + + + + + 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:55100.0 %
Date:2023-11-30 22:31:42Functions: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
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/state_generic.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func-sort-c.html new file mode 100644 index 0000000000..e957e3db69 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_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/state/state_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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)4
mrs_uav_state_estimators::StateGeneric::~StateGeneric().24
+
+
+ + + +
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..d8ca0ae454 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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)4
mrs_uav_state_estimators::StateGeneric::~StateGeneric()0
mrs_uav_state_estimators::StateGeneric::~StateGeneric().24
+
+
+ + + +
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..7151adbb4a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html @@ -0,0 +1,183 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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           4 :   StateGeneric(const std::string &name, const bool is_core_plugin)
+      79           4 :       : StateEstimator(name, name + "_origin", state_generic::package_name), is_core_plugin_(is_core_plugin) {
+      80           4 :   }
+      81             : 
+      82           4 :   ~StateGeneric(void) {
+      83           4 :   }
+      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..92613c2343 --- /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:11717467.2 %
Date:2023-11-30 22:31:42Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
<unnamed>68.3 %28 / 4133.3 %2 / 6
proc_tf_to_world.h +
74.5%74.5%
+ 		74.5 %35 / 4737.5 %3 / 8
<unnamed>74.5 %35 / 4737.5 %3 / 8
proc_saturate.h +
60.0%60.0%
+ 		60.0 %24 / 4066.7 %4 / 6
<unnamed>60.0 %24 / 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..f5d404ec89 --- /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:11717467.2 %
Date:2023-11-30 22:31:42Functions: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_saturate.h +
60.0%60.0%
+ 		60.0 %24 / 4066.7 %4 / 6
<unnamed>60.0 %24 / 4066.7 %4 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
<unnamed>68.3 %28 / 4133.3 %2 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
<unnamed>70.0 %7 / 1075.0 %6 / 8
proc_tf_to_world.h +
74.5%74.5%
+ 		74.5 %35 / 4737.5 %3 / 8
<unnamed>74.5 %35 / 4737.5 %3 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html new file mode 100644 index 0000000000..d557459063 --- /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:11717467.2 %
Date:2023-11-30 22:31:42Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
<unnamed>68.3 %28 / 4133.3 %2 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
proc_saturate.h +
60.0%60.0%
+ 		60.0 %24 / 4066.7 %4 / 6
<unnamed>60.0 %24 / 4066.7 %4 / 6
proc_tf_to_world.h +
74.5%74.5%
+ 		74.5 %35 / 4737.5 %3 / 8
<unnamed>74.5 %35 / 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..e4a9f91c66 --- /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:11717467.2 %
Date:2023-11-30 22:31:42Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
proc_tf_to_world.h +
74.5%74.5%
+ 		74.5 %35 / 4737.5 %3 / 8
proc_saturate.h +
60.0%60.0%
+ 		60.0 %24 / 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..080965544c --- /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:11717467.2 %
Date:2023-11-30 22:31:42Functions: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_saturate.h +
60.0%60.0%
+ 		60.0 %24 / 4066.7 %4 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
proc_tf_to_world.h +
74.5%74.5%
+ 		74.5 %35 / 4737.5 %3 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html new file mode 100644 index 0000000000..125d1e9da2 --- /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:11717467.2 %
Date:2023-11-30 22:31:42Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b64a252cab730e64e7e46966d4a4e6187ae9facd GIT binary patch literal 656 zcmV;B0&o3^P)2i$Eh$_a1ffbCT**)NUE|Y6jd4E=SUmi7Q$-F96_Fewyni(}s z8H0^6tmnSunwqp;B?-iQBu~`s;jK0u*OC-l;Z5Ys$lp==NYoYb-EkG;j$#=aohb^A zxVIw9f>6Q96v!N|V`^ZI1H|4neH=Q$4G>C#U=C;w|JZ<$n#o3HGK^`ZTPo(aw=>Y% zzB1#x?l9&V0|MQ>Ul@IbO5jqACP8pC?h0+-6628-4W2XVGHK_b0-on5*kxbW=QcA2t_n<dA`)n{kiFQy@)L%bU+m{=J%6!JwI^2f)b#rDD!qJ%x^-mM`xIlI q49GSm>wgvaPj~7ls(-H<<@^E?wHgS_z&(2a0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2023-11-30 22:31:42Functions: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&)1
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)2151
+
+
+ + + +
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..8a1e2dd774 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2023-11-30 22:31:42Functions: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>&)2151
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&)1
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..6db8408924 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html @@ -0,0 +1,192 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2023-11-30 22:31:42Functions: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           1 : 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           1 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      41             : 
+      42             :   // | --------------------- load parameters -------------------- |
+      43           1 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      44             : 
+      45           1 :   ph->param_loader->loadParam("buffer_size", buffer_size_);
+      46           1 :   ph->param_loader->loadParam("max_diff", max_diff_);
+      47             : 
+      48           1 :   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           1 :   const double min_valid = std::numeric_limits<double>::lowest();
+      55           1 :   const double max_valid = std::numeric_limits<double>::max();
+      56             : 
+      57             :   // initialize median filter
+      58           2 :   for (int i = 0; i < n_measurements; i++) {
+      59           1 :     vec_mf_.push_back(mrs_lib::MedianFilter(buffer_size_, min_valid, max_valid, max_diff_));
+      60             :   }
+      61           1 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66        2151 : std::tuple<bool, bool> ProcMedianFilter<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68        2151 :   if (!Processor<n_measurements>::enabled_) {
+      69           0 :     return {true, true};
+      70             :   }
+      71             : 
+      72        2151 :   bool ok_flag     = true;
+      73        2151 :   bool should_fuse = true;
+      74        4302 :   for (int i = 0; i < measurement.rows(); i++) {
+      75        2151 :     vec_mf_[i].add(measurement(i));
+      76        2151 :     if (vec_mf_[i].full()) {
+      77        2052 :       if (!vec_mf_[i].check(measurement(i))) {
+      78           0 :         std::stringstream ss_measurement_string;
+      79           0 :         ss_measurement_string << measurement(i);
+      80           0 :         ss_measurement_string << " ";
+      81           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: measurement[%d]: %s declined by median filter (median: %.2f, max_diff: %.2f).",
+      82             :                           Processor<n_measurements>::getPrintName().c_str(), i, ss_measurement_string.str().c_str(), vec_mf_[i].median(), max_diff_);
+      83           0 :         ok_flag     = false;
+      84           0 :         should_fuse = false;
+      85             :       }
+      86             :     } else {
+      87          99 :       ROS_WARN_THROTTLE(1.0, "[%s]: median filter not full yet", Processor<n_measurements>::getPrintName().c_str());
+      88          99 :       ok_flag     = false;
+      89          99 :       should_fuse = false;
+      90             :     }
+      91             :   }
+      92             : 
+      93        2151 :   return {ok_flag, should_fuse};
+      94             : }
+      95             : /*//}*/
+      96             : 
+      97             : /*//{ reset() */
+      98             : template <int n_measurements>
+      99           0 : void ProcMedianFilter<n_measurements>::reset() {
+     100           0 :   for (auto mf : vec_mf_) {
+     101           0 :     mf.clear();
+     102             :   }
+     103           0 : }
+     104             : /*//}*/
+     105             : 
+     106             : }  // namespace mrs_uav_state_estimators
+     107             : 
+     108             : #endif  // PROCESSORS_PROC_MEDIAN_FILTER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html new file mode 100644 index 0000000000..99218c587d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html @@ -0,0 +1,47 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6923330e664ef60437c8a503a26089bc19866de0 GIT binary patch literal 635 zcmV->0)+jEP)|!_b`VE3^Lar_duazn?*P542Yg*&@;(Y2Dc4#e6hU;_|6R+NifR9fC38Z1>2d zrafk7b^)|k)UBv>lh(@J0r&mP=ECXD!yoWFU&8|g+@~h?V-6_58_JdWh~r7YY=j$` ze}+6-$gm+2U~77&zy}EzNm43lYtl11FdJj61!-HNE|Hp)4-!Y*xE0#=tI0hwaawf3 z8%E8pAiUVuz?-w@GC{gbz3ds=CnB|iOMUU5TS2F2{opi6rerWVnll+N zkR>#)AKzG#BeSO1rcBHDK1Uswfi}Iir!vbP?Hb4%Q-ARJp<$-x@P^dvLHfzR)yCkh z#{e=1=~=p@RcbG@ApOuPQIbOo~h_YFnh{sDB+ V&znIPT!#Pv002ovPDHLkV1lH}D0lz> literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html new file mode 100644 index 0000000000..d469df7350 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:244060.0 %
Date:2023-11-30 22:31:42Functions:4666.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcSaturate<1>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)2
mrs_uav_state_estimators::ProcSaturate<1>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)3
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)1048
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)3164
+
+
+ + + +
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..b9d8d7831f --- /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:244060.0 %
Date:2023-11-30 22:31:42Functions: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>&)3164
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)>)3
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)1048
mrs_uav_state_estimators::ProcSaturate<2>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)2
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html new file mode 100644 index 0000000000..5f4af983f1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html new file mode 100644 index 0000000000..d0e93a6f68 --- /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:244060.0 %
Date:2023-11-30 22:31:42Functions: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           5 : 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           5 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48           5 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      49             : 
+      50           5 :   ph->param_loader->loadParam("start_enabled", this->start_enabled_);
+      51           5 :   this->enabled_ = this->start_enabled_;
+      52           5 :   ph->param_loader->loadParam("keep_enabled", keep_enabled_);
+      53           5 :   ph->param_loader->loadParam("min", saturate_min_);
+      54           5 :   ph->param_loader->loadParam("max", saturate_max_);
+      55           5 :   ph->param_loader->loadParam("limit", innovation_limit_);
+      56             : 
+      57           5 :   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           5 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66        4212 : std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68             :   // if no saturation is required, processing is successful
+      69        4212 :   if (!this->enabled_) {
+      70           0 :     return {true, true};
+      71             :   }
+      72             : 
+      73        4212 :   bool ok_flag     = true;
+      74        4212 :   bool should_fuse = true;
+      75        9362 :   for (int i = 0; i < measurement.rows(); i++) {
+      76        5255 :     const double state = fun_get_state_(state_id_, i);
+      77        5255 :     ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);
+      78             : 
+      79        5255 :     if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
+      80         105 :       return {true, true};  // do not even try to saturate, trigger innovation-based switch to other estimator
+      81             :     }
+      82             : 
+      83        5150 :     if (measurement(i) > state + saturate_max_) {
+      84           0 :       const double saturated = state + saturate_max_;
+      85           0 :       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           0 :       measurement(i) = saturated;
+      88           0 :       ok_flag        = false;
+      89           0 :       should_fuse    = true;
+      90        5150 :     } 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        4107 :   if (!this->keep_enabled_ && ok_flag) {
+     102           0 :     this->enabled_ = false;
+     103             :   }
+     104             : 
+     105        4107 :   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..d88cf9c331d71d5d0e1628473b0acfe14bcc36f6 GIT binary patch literal 676 zcmV;V0$crwP)1 zR9J=WSnIiiAPj{Ka0S=^SI`aY{;OO<024t7)&9HD`c- zfD*mj$SWGnYc>uwoz%}sPmM<6lD7ycwW-*jc@*7a###!i))xcJm)vrz`I3vVIi=?L zr>N;tfjs-+G{=+_(5=eRIHr5K(xK)$DSH8yIiivfF-wt#Mc}p`xECb}H>Osy_@A+Q zAJ~UJZ^bpUNiG}$g>}95#D@dUAj-DHt*g7tY16Q(z>E$W{gp0BRO|A6o4O%k*;-Q? z!zz`uD%)PKb8yndh^g;1lGSk_C7RHfedK$~QX% zBOLiYU15L#x2~w|=mQGSw=8chhPl57oUQ9&SD!u2_4L}%P-q#v;W3G4hcDghAy*BU0X_X9B{{0KCuQqVb5pPKL)#vxEzBt>l~gcTWJyTE$P^&eD4( zKH$cueiNIuCJ()##k0K-L)q+;in+0~uHW=Wv@)4$=^fad%2B>)zVc?m{T)3z-ZSN! z=9%!cB3cfQX%C;H`vjtdf!S22Ouh`E(e + + + + + + 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:354774.5 %
Date:2023-11-30 22:31:42Functions: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&)2
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)1007
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)4610
+
+
+ + + +
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..fee06d6fe2 --- /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:354774.5 %
Date:2023-11-30 22:31:42Functions: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>)1007
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)4610
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&)2
+
+
+ + + +
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..4a2628087e --- /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:354774.5 %
Date:2023-11-30 22:31:42Functions: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           2 : 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           2 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      50             : 
+      51           2 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      52             : 
+      53           2 :   ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+      54             : 
+      55           2 :   gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+      56             : 
+      57           2 :   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           2 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64           2 :   shopts.nh                 = nh;
+      65           2 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66           2 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      67           2 :   shopts.threadsafe         = true;
+      68           2 :   shopts.autostart          = true;
+      69           2 :   shopts.queue_size         = 10;
+      70           2 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72           2 :   sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcTfToWorld::callbackGnss, this);
+      73             : 
+      74           2 :   is_initialized_ = true;
+      75           2 : }
+      76             : /*//}*/
+      77             : 
+      78             : /*//{ callbackGnss() */
+      79             : template <int n_measurements>
+      80        1007 : void ProcTfToWorld<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+      81             : 
+      82        1007 :   if (!is_initialized_) {
+      83           0 :     return;
+      84             :   }
+      85             : 
+      86        1007 :   if (got_gnss_) {
+      87        1005 :     return;
+      88             :   }
+      89             : 
+      90           2 :   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           2 :   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           2 :   mrs_lib::UTM(msg->latitude, msg->longitude, &gnss_x_, &gnss_y_);
+      99           2 :   got_gnss_ = true;
+     100             : }
+     101             : /*//}*/
+     102             : 
+     103             : /*//{ process() */
+     104             : template <int n_measurements>
+     105        4610 : std::tuple<bool, bool> ProcTfToWorld<n_measurements>::process(measurement_t& measurement) {
+     106             : 
+     107        4610 :   if (!Processor<n_measurements>::enabled_) {
+     108           0 :     return {true, true};
+     109             :   }
+     110             : 
+     111        4610 :   if (!got_gnss_) {
+     112           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+     113           0 :     return {false, false};
+     114             :   }
+     115             : 
+     116        4610 :   if (!is_gnss_offset_calculated_) {
+     117             : 
+     118           2 :     gnss_x_                    = (gnss_x_ - measurement(0)) - Processor<n_measurements>::ch_->world_origin.x;
+     119           2 :     gnss_y_                    = (gnss_y_ - measurement(1)) - Processor<n_measurements>::ch_->world_origin.y;
+     120           2 :     is_gnss_offset_calculated_ = true;
+     121             :   }
+     122             : 
+     123        4610 :   measurement(0) += gnss_x_;
+     124        4610 :   measurement(1) += gnss_y_;
+     125        4610 :   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..77272cb7d2d9bb6594b04b10e0d69c74d16c8925 GIT binary patch literal 685 zcmV;e0#f~nP)v0{{R3&zjKJ0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp;0#i$JzG z4yhaR4CA7OH5=K$2J#MEjbdO_W5q(Ot14BiLi?IqAv|i#f?AcjhhBcInrSt48I4U= z%=6{;FVJitTZ-b!%R&+0h#>^!p&%0lDw;F|GH1`;6}S#}?z`BkV)ot8S1usj zNuqf0b%a+NjHQUJZk#dhq^egKqn$v2bBqA)yjcen$Y(S)D@_4Bj!zWpPWw5pBVEV8 z?nVYYx+0FV4?y5VQ6{P>8f2B1%?j1Ugt>-a`W1`$YdhemUZ7l>HTCVvsgt`1O#& z7oi(R-`9+meW9fCKe}3c4U{K7gu5|4Abr!c4*&bsOm|1A5+oP8TY19c>w^voq08JPleH(v1 zDaM0>KPj-*b2-au7QSWhzO}7YUL1s~17x<$A$U8A3#erIyV+{MudmLGA0Yn#0D?SZ TT>#kq00000NkvXXu0mjfA-6Ic 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..a2f7163427 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2023-11-30 22:31:42Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Processor<1>::toggle(bool)0
mrs_uav_state_estimators::Processor<2>::toggle(bool)0
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const2
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const2
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&)4
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&)5
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const6
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const8
+
+
+ + + +
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..3771cdbbbf --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2023-11-30 22:31:42Functions: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&)5
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&)4
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const2
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const8
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const2
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const6
+
+
+ + + +
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..0953821aef --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2023-11-30 22:31:42Functions: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           9 :   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           9 :       : correction_name_(correction_name), name_(name), ch_(ch), ph_(ph) {
+      30           9 :   }  // 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          14 : std::string Processor<n_measurements>::getNamespacedName() const {
+      52          14 :   return correction_name_ + "/" + name_;
+      53             : }
+      54             : /*//}*/
+      55             : 
+      56             : /*//{ getPrintName() */
+      57             : template <int n_measurements>
+      58           4 : std::string Processor<n_measurements>::getPrintName() const {
+      59           4 :   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/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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::AltGeneric::isConverged()4
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)4
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)24
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const28
mrs_uav_state_estimators::AltGeneric::start()53
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const55
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const3164
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) const3164
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const4023
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)4027
mrs_uav_state_estimators::AltGeneric::setDt(double const&)4027
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const4027
mrs_uav_state_estimators::AltGeneric::getStates() const4027
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)4125
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const8046
mrs_uav_state_estimators::AltGeneric::generateA()8058
mrs_uav_state_estimators::AltGeneric::generateB()8058
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&)14518
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) const14518
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)14519
mrs_uav_state_estimators::AltGeneric::getState(int const&) const20599
+
+
+ + + +
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..b273f28e0a --- /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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::AltGeneric::isConverged()4
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)4125
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&)14518
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)14519
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)4027
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)4
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&)4027
mrs_uav_state_estimators::AltGeneric::start()53
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)24
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::generateA()8058
mrs_uav_state_estimators::AltGeneric::generateB()8058
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const55
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const8046
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const4023
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const28
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const4027
mrs_uav_state_estimators::AltGeneric::getState(int const&) const20599
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const3164
mrs_uav_state_estimators::AltGeneric::getStates() const4027
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) const14518
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) const3164
+
+
+ + + +
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..dc29214b6c --- /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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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           4 : void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16           4 :   ch_ = ch;
+      17           4 :   ph_ = ph;
+      18             : 
+      19           4 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // clang-format off
+      22           4 :   dt_ = 0.01;
+      23           4 :   input_coeff_ = 10;
+      24           4 :   default_input_coeff_ = 10;
+      25             : 
+      26           4 :   generateA();
+      27           4 :   generateB();
+      28             : 
+      29           4 :   H_ <<
+      30           4 :     1, 0, 0;
+      31             : 
+      32             :   // clang-format on
+      33             : 
+      34             :   // | --------------- initialize parameter loader -------------- |
+      35             : 
+      36           4 :   if (is_core_plugin_) {
+      37             : 
+      38           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      39           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      40             :   }
+      41             : 
+      42           4 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      43             : 
+      44             :   // | --------------------- load parameters -------------------- |
+      45             : 
+      46           4 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      47           4 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      48           4 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      49           4 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      50           4 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      51           4 :   if (is_repredictor_enabled_) {
+      52           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      53             :   }
+      54             : 
+      55             :   // | --------------- corrections initialization --------------- |
+      56           4 :   ph->param_loader->loadParam("corrections", correction_names_);
+      57             : 
+      58          12 :   for (auto corr_name : correction_names_) {
+      59           8 :     corrections_.push_back(std::make_shared<Correction<alt_generic::n_measurements>>(
+      60        3180 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::ALTITUDE, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      61       14518 :         [this](const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      62       14518 :           return this->doCorrection(meas, R, state);
+      63             :         }));
+      64             :   }
+      65             : 
+      66           4 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      67             : 
+      68             :   // | ----------- initialize process noise covariance ---------- |
+      69           4 :   Q_ = Q_t::Zero();
+      70             :   double tmp_noise;
+      71           4 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      72           4 :   Q_(POSITION, POSITION) = tmp_noise;
+      73           4 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      74           4 :   Q_(VELOCITY, VELOCITY) = tmp_noise;
+      75           4 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      76           4 :   Q_(ACCELERATION, ACCELERATION) = tmp_noise;
+      77             : 
+      78             :   // | ------- check if all parameters loaded successfully ------ |
+      79           4 :   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           4 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&AltGeneric::callbackReconfigure, this, _1, _2));
+      87           4 :   drmgr_->config.pos = Q_(POSITION, POSITION);
+      88           4 :   drmgr_->config.vel = Q_(VELOCITY, VELOCITY);
+      89           4 :   drmgr_->config.acc = Q_(ACCELERATION, ACCELERATION);
+      90           4 :   drmgr_->update_config(drmgr_->config);
+      91             : 
+      92             :   // | --------------- Kalman filter intialization -------------- |
+      93           4 :   const x_t        x0 = x_t::Zero();
+      94           4 :   const P_t        P0 = 1e3 * P_t::Identity();
+      95           4 :   const statecov_t sc0({x0, P0});
+      96           4 :   sc_ = sc0;
+      97             : 
+      98           4 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+      99           4 :   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           4 :   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           8 :   mrs_lib::SubscribeHandlerOptions shopts;
+     121           4 :   shopts.nh                 = nh;
+     122           4 :   shopts.node_name          = getPrintName();
+     123           4 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     124           4 :   shopts.threadsafe         = true;
+     125           4 :   shopts.autostart          = true;
+     126           4 :   shopts.queue_size         = 10;
+     127           4 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     128             : 
+     129           4 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     130             : 
+     131             :   // | ---------------- publishers initialization --------------- |
+     132           4 :   if (ch_->debug_topics.input) {
+     133           4 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     134             :   }
+     135           4 :   if (ch_->debug_topics.output) {
+     136           4 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     137             :   }
+     138           4 :   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           4 :   if (changeState(INITIALIZED_STATE)) {
+     145           4 :     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           4 : }
+     150             : /*//}*/
+     151             : 
+     152             : /*//{ start() */
+     153          53 : bool AltGeneric::start(void) {
+     154             : 
+     155          53 :   if (isInState(READY_STATE)) {
+     156             :     /* timer_update_.start(); */
+     157           4 :     changeState(STARTED_STATE);
+     158           4 :     return true;
+     159             : 
+     160             :   } else {
+     161          49 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     162          49 :     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        4125 : void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
+     213             : 
+     214             : 
+     215        4125 :   if (!isInitialized()) {
+     216          98 :     return;
+     217             :   }
+     218             : 
+     219        4125 :   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           1 :     case READY_STATE: {
+     227           1 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     228           1 :       break;
+     229             :     }
+     230             : 
+     231          93 :     case INITIALIZED_STATE: {
+     232             :       // initialize the estimator with current corrections
+     233         117 :       for (auto correction : corrections_) {
+     234         113 :         auto res = correction->getProcessedCorrection();
+     235         113 :         if (res) {
+     236          24 :           auto measurement_stamped = res.value();
+     237          24 :           setState(measurement_stamped.value(0), correction->getStateId());
+     238          24 :           ROS_INFO("[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     239             :         } else {
+     240          89 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     241             :                             correction->getNamespacedName().c_str());
+     242          89 :           return;
+     243             :         }
+     244             :       }
+     245           4 :       changeState(READY_STATE);
+     246           4 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     247           4 :       break;
+     248             :     }
+     249             : 
+     250           4 :     case STARTED_STATE: {
+     251           4 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     252             : 
+     253           4 :       if (isConverged()) {
+     254           4 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     255           4 :         changeState(RUNNING_STATE);
+     256             :       }
+     257           4 :       break;
+     258             :     }
+     259             : 
+     260        4027 :     case RUNNING_STATE: {
+     261       12081 :       for (auto correction : corrections_) {
+     262        8054 :         if (!correction->isHealthy()) {
+     263           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     264           0 :           changeState(ERROR_STATE);
+     265             :         }
+     266             :       }
+     267        4027 :       break;
+     268             :     }
+     269             : 
+     270           0 :     case STOPPED_STATE: {
+     271           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     272           0 :       break;
+     273             :     }
+     274             : 
+     275           0 :     case ERROR_STATE: {
+     276           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     277           0 :       ros::Time t_now = ros::Time::now();
+     278           0 :       if (is_error_state_first_time_) {
+     279           0 :         prev_time_in_error_state_  = t_now;
+     280           0 :         is_error_state_first_time_ = false;
+     281           0 :         error_state_duration_      = ros::Duration(0.0);
+     282             :       }
+     283           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     284             : 
+     285             : 
+     286             :       // check if all corrections are healthy now
+     287           0 :       bool all_corrections_healthy = true;
+     288           0 :       for (auto correction : corrections_) {
+     289           0 :         if (!correction->isHealthy()) {
+     290           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     291           0 :           all_corrections_healthy = false;
+     292             :         }
+     293             :       }
+     294             : 
+     295           0 :       if (all_corrections_healthy && innovation_ok_) {
+     296             :         // initialize the estimator again if corrections become healthy
+     297           0 :         if (error_state_duration_.toSec() > 5.0) {
+     298           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     299           0 :           changeState(INITIALIZED_STATE);
+     300           0 :           is_error_state_first_time_ = true;
+     301           0 :         }
+     302             :       } else {
+     303           0 :         is_error_state_first_time_ = true;
+     304             :       }
+     305             : 
+     306           0 :       prev_time_in_error_state_ = t_now;
+     307             : 
+     308           0 :       break;
+     309             :     }
+     310             :   }
+     311             : 
+     312        4036 :   if (sh_control_input_.newMsg()) {
+     313        3233 :     is_input_ready_ = true;
+     314             :   }
+     315             : 
+     316             :   // check age of input
+     317        4036 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     318           0 :     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           0 :     is_input_ready_ = false;
+     321             :   }
+     322             : 
+     323        4036 :   if (!isRunning() && !isStarted()) {
+     324           5 :     return;
+     325             :   }
+     326             : 
+     327        4031 :   if (first_iter_) {
+     328           4 :     first_iter_ = false;
+     329           4 :     return;
+     330             :   }
+     331             : 
+     332        4027 :   double dt = (event.current_real - event.last_real).toSec();
+     333        4027 :   if (dt <= 0.0) {
+     334           0 :     return;
+     335             :   }
+     336             : 
+     337        4027 :   if (!is_repredictor_enabled_) {  // repredictor requires constant dt TODO: how to handle repredictor + variable rate?
+     338        4027 :     setDt(dt);
+     339             :   }
+     340             : 
+     341             :   // prediction step
+     342        4027 :   u_t       u;
+     343        4027 :   ros::Time input_stamp;
+     344        4027 :   if (is_input_ready_) {
+     345        3470 :     mrs_msgs::EstimatorInputConstPtr msg            = sh_control_input_.getMsg();
+     346        3470 :     const tf2::Vector3               des_acc_global = getAccGlobal(msg, 0);  // we don't care about heading
+     347        3470 :     input_stamp                                     = msg->header.stamp;
+     348        3470 :     setInputCoeff(default_input_coeff_);
+     349        3470 :     u(0) = des_acc_global.getZ();
+     350             :   } else {
+     351         557 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     352         557 :     input_stamp = ros::Time::now();
+     353         557 :     setInputCoeff(0);
+     354         557 :     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        4027 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     367        4027 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     368             : 
+     369             :   try {
+     370             :     // Apply the prediction step
+     371        8054 :     std::scoped_lock lock(mutex_lkf_);
+     372        4027 :     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        4027 :       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        4027 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     385             : 
+     386             :   // publishing
+     387        4027 :   publishInput(u, input_stamp);
+     388        4027 :   publishOutput();
+     389        4027 :   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       14519 : void AltGeneric::doCorrection(const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     489       14519 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     490       14518 : }
+     491             : /*//}*/
+     492             : 
+     493             : /*//{ doCorrection() */
+     494       14518 : void AltGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     495             : 
+     496       14518 :   if (!isInitialized()) {
+     497          90 :     return;
+     498             :   }
+     499             : 
+     500             :   // we do not want to perform corrections until the estimator is initialized
+     501       14512 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     502          90 :     return;
+     503             :   }
+     504             : 
+     505             :   // for position state check the innovation
+     506       14425 :   if (H_idx == POSITION) {
+     507        5366 :     std::scoped_lock lock(mtx_innovation_);
+     508             : 
+     509        5366 :     is_mitigating_jump_ = false;
+     510        5366 :     innovation_(0)      = z(0) - getState(POSITION);
+     511             : 
+     512        5366 :     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        5366 :     innovation_ok_ = true;
+     541             :   }
+     542             : 
+     543       14425 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     544             :   try {
+     545             :     // Apply the correction step
+     546             :     {
+     547       28858 :       std::scoped_lock lock(mutex_lkf_);
+     548       14429 :       H_        = H_t::Zero();
+     549       14429 :       H_(H_idx) = 1;
+     550       14429 :       lkf_->H   = H_;
+     551       14429 :       if (is_repredictor_enabled_) {
+     552             : 
+     553           0 :         lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     554             :       } else {
+     555       14429 :         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       14429 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     565             : }  // namespace mrs_uav_state_estimators
+     566             : /*//}*/
+     567             : 
+     568             : /*//{ isConverged() */
+     569           4 : bool AltGeneric::isConverged() {
+     570             : 
+     571             :   // TODO: check convergence by rate of change of determinant
+     572             : 
+     573           4 :   return true;
+     574             : }
+     575             : /*//}*/
+     576             : 
+     577             : /*//{ getState() */
+     578        3164 : double AltGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     579        3164 :   return getState(stateIdToIndex(state_id_in, 0));
+     580             : }
+     581             : 
+     582       20599 : double AltGeneric::getState(const int &state_idx_in) const {
+     583       20599 :   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          24 : void AltGeneric::setState(const double &state_in, const int &state_idx_in) {
+     593          24 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     594          24 : }
+     595             : /*//}*/
+     596             : 
+     597             : /*//{ getStates() */
+     598        4027 : AltGeneric::states_t AltGeneric::getStates(void) const {
+     599        8054 :   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        8046 : double AltGeneric::getCovariance(const int &state_idx_in) const {
+     615        8046 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     616             : }
+     617             : /*//}*/
+     618             : 
+     619             : /*//{ getCovarianceMatrix() */
+     620        4027 : AltGeneric::covariance_t AltGeneric::getCovarianceMatrix(void) const {
+     621        8054 :   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        4023 : double AltGeneric::getInnovation(const int &state_idx) const {
+     633        4023 :   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        4027 : void AltGeneric::setDt(const double &dt) {
+     643        4027 :   dt_ = dt;
+     644        4027 :   generateA();
+     645        4027 :   generateB();
+     646        8054 :   std::scoped_lock lock(mutex_lkf_);
+     647        4027 :   lkf_->A = A_;
+     648        4027 :   lkf_->B = B_;
+     649        4027 : }
+     650             : /*//}*/
+     651             : 
+     652             : /*//{ setInputCoeff() */
+     653        4027 : void AltGeneric::setInputCoeff(const double &input_coeff) {
+     654        4027 :   input_coeff_ = input_coeff;
+     655        4027 :   generateA();
+     656        4027 :   generateB();
+     657        8054 :   std::scoped_lock lock(mutex_lkf_);
+     658        4027 :   lkf_->A = A_;
+     659        4027 :   lkf_->B = B_;
+     660        4027 : }
+     661             : /*//}*/
+     662             : 
+     663             : /*//{ generateA() */
+     664        8058 : void AltGeneric::generateA() {
+     665             : 
+     666             :   // clang-format off
+     667        8058 :     A_ <<
+     668        8058 :       1, dt_, 0.5 * dt_ * dt_,
+     669        8058 :       0, 1, dt_,
+     670        8058 :       0, 0, 1-(input_coeff_ * dt_);
+     671             :   // clang-format on
+     672        8058 : }
+     673             : /*//}*/
+     674             : 
+     675             : /*//{ generateB() */
+     676        8058 : void AltGeneric::generateB() {
+     677             : 
+     678             :   // clang-format off
+     679        8058 :     B_ <<
+     680             :       0,
+     681        8058 :       0,
+     682        8058 :       (input_coeff_ * dt_);
+     683             :   // clang-format on
+     684        8058 : }
+     685             : /*//}*/
+     686             : 
+     687             : /*//{ callbackReconfigure() */
+     688           4 : void AltGeneric::callbackReconfigure(AltitudeEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     689             : 
+     690           4 :   if (!isInitialized()) {
+     691           4 :     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          28 : std::string AltGeneric::getNamespacedName() const {
+     704          56 :   return parent_state_est_name_ + "/" + getName();
+     705             : }
+     706             : /*//}*/
+     707             : 
+     708             : /*//{ getPrintName() */
+     709          55 : std::string AltGeneric::getPrintName() const {
+     710         110 :   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..bbe258c1d467301b5aaf239b5e9b3e793adeb5cb GIT binary patch literal 2783 zcmV<53Ly1~P)w8{e1A=4RZ-F{wOycYYD7*&au{r613D`4)#j#IZ@Q&u9J=!?^E~e;6j^7^aUg zA9E?7erY%6IeqqD=8NcO1VT3G=#WuhgZ8KcYyx+a=So2R(zAloZpIz5z49p*UZ`Es zM!k|T7{PLl-TFL%vd!0EJt6TChSQA>KotWIg6pwn$6*x={zezC(dqFDW*WYAhBNFI z<7Y&0fp{1bQv=KmF#((zD;+15XQWe3Sj^nNiga9-O&&IWu`a{1E~?Q%@l3grQspe< zu?7T@Dk!CAI+*47h2)PRl169Ta7$we@TXDWTLDND_vBv{Wf%Wy?$BlZO z?Cto*%8i5~XiCFa4T**tKz8_b3PAU5-tG9B8I$-(vvzZ4|MMDwBV$rv%o%ov#f)Hk z?D4Hq_ zg`!t_kknwZQe*F9g*loKVMYQ7=0*UFs!(y=gJ-WmMve7UoacL>!fK)RyjCy0>&&h@ z*4lMl$&tQN9oYeQ$Ouzau0su%wV{I%kQ+|F37}42_1b!@ysvWXz}n!t2z&IlW&dE4 zndB0D6jN!^aBL4->uDQ+uqtg+nySE;m(~ zu&`#O6P45znFl##0W$``K&=X3PE8Tegc*gk%cYxuMvIJ_wB)!+fXacEuF22IaBr*% zMTa3(Y|tIU+Gsdi+QIxpMQ9jVEo7dX$bPk2yqfCVlWmV5UL) za7ceOEmP7ld^nCa8xMmsbX2l>8a|rjDU?q?|X*BlhZy z4xjcOm1B>u)gb^TBZ%c}0+O>&PozDz7_PSzY9(u&sV9b7%%}=kS!Z;rbpVnXa~GJ* z*l@}Splt=DJ$qD3Ph7fXS@gE&uNMkYAi|8`=X+ZZrhh}{=nb4v52f7p$YXS`$ME4T z)IU?mW$RHS1q25~0E|cJDx)(jZW;wLy;$!bUoKHmBX|}Mx(pV#M*=2FZeIX-HprC$ zbwTcJf*T#FjX0*K$g`;|^D-!LS?(}&!6emN{jMthJ^~jM1f1T0@k!MBu{nV4PM!8_<=rPY}PRIjkBR9LvO-fNbm$Z;k znx7E-fU70eD0$i|cW7Z33l&nzoOld~!PI?rAai{b|TxJwV%~6%+)Z+gH!Be@QRmzzh^|g0Xsy%=SP<^K& zw=(g%4Q&H)fOO>)(Mhng{8%gN@@<}E)s380$JK8gK%yy^+poCuC0w` ztc%eR#VHtgmpsB9$TzBF74qpTJ)yfi_M&i7{2q>bCS&SYfsujlfTJ`?1~|N+113%V zDvnSGSdLfrm0P;}X`VbfIQ0lkO+`ml?urBhFz20SMPF=BcbK|)D7YytQw0*9R8bII zWy2ZY6lmZWswa72Bp8g_>>)(NF#NULr1ZiXdZ8|_=f&pa}H0LWnV?3FP++G^YW&>Jj#&#Roa&n#bk6FSpf@rh|!yCRF$msaGn%Ozm3cF|b4)340BR*H5#wGS2Gqn!DY zlExpbk}ki5Gnw3Oe@T_}UoEaOa-U*^TYMu#2nEuDku1%gpofo|y~`^;x(YjvhSy4) z^m0q|p|1D@#VO)Ie=C+Ap!motWE5&O1*BT z?|rlHZ-91b?w5N238FX1alS(8TX>aYE+yk(WWqy(jY}sJZc(o8vCQNCFlC3TDb)ff z08YN$wtTPX#L~Fe9b-f@>}5Q z9`$~>=4kI~iCNTG3nT~sam{?Qo#boGdBY82i(ZrX7OZ)+l`PD#UR+vbaj*R(bOQyl z8D*b_((z&P2+moHJZX*__BiN$QYBonW^?_@^lgs^>I#cm=R;Wr>>!V>VCVpkFIr{s zIHPGjkr@C@<%JIfsHs)<+?C36Qc-X1h6H@m_ZCi5UaJg5U~0o!USF&Ue`!tAkoce? z;Wq|8t%iz^t`^^0pnWC3dJG;lxYgZ5`Rc@1)Lok$BA<-nCKaghtwQ%b zZ2H=6c`JvghI*{kgO#$_?ut?i4f_ZWiGG7THkrdCQ7YrF(!wUztdLpIv*@9-TA1!V zos!1~wXh=L#OMlMN_OwNi`VM$BTK=)JM-l0CMO@1^A8JfbnN(5Bf$z?4cO`g_JEpQ z>H)vNolz(kV;z}GPT8+)sXgEEkA*eBZWby$(yO0!)c-Oa<*$ng$tx3owLk+<@r-u@ z43b*8?Gm5RX=UR6@%lPmtCsEpZ1LFx{vuv0eB9a#!{Igj&;P9=e)L_b{GJ8;Ugs-? zaP-H3sa + + + + + + 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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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 +
55.5%55.5%
+ 		55.5 %206 / 37173.3 %22 / 30
<unnamed>55.5 %206 / 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..c49a11b0d8 --- /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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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 +
55.5%55.5%
+ 		55.5 %206 / 37173.3 %22 / 30
<unnamed>55.5 %206 / 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..0fac8b633c --- /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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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 +
55.5%55.5%
+ 		55.5 %206 / 37173.3 %22 / 30
<unnamed>55.5 %206 / 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..84230b5cc7 --- /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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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 +
55.5%55.5%
+ 		55.5 %206 / 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..e51045e837 --- /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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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 +
55.5%55.5%
+ 		55.5 %206 / 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..868ee2837c --- /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:20637155.5 %
Date:2023-11-30 22:31:42Functions: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 +
55.5%55.5%
+ 		55.5 %206 / 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..45ecb6c1f4 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2023-11-30 22:31:42Functions: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..b7a132f61e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2023-11-30 22:31:42Functions: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..e6e758e1ef --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html @@ -0,0 +1,754 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2023-11-30 22:31:42Functions: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..e88893d396 --- /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:8513164.9 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::HdgPassthrough::start()7
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const8
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const23
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)4073
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const4073
mrs_uav_state_estimators::HdgPassthrough::getStates() const4073
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)4148
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)9108
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)9411
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const15626
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)18519
+
+
+ + + +
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..3fde20e183 --- /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:8513164.9 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)4073
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)4148
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)9411
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>)9108
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::start()7
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)18519
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]() const23
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]() const8
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const4073
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const15626
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getStates() const4073
+
+
+ + + +
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..f2346ad5fd --- /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:8513164.9 %
Date:2023-11-30 22:31:42Functions: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           4 : void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16           4 :   ch_ = ch;
+      17           4 :   ph_ = ph;
+      18             : 
+      19           4 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21           4 :   hdg_state_      = states_t::Zero();
+      22           4 :   hdg_covariance_ = covariance_t::Zero();
+      23             : 
+      24             :   // | --------------- param loader initialization --------------- |
+      25             : 
+      26           4 :   if (is_core_plugin_) {
+      27             : 
+      28           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      29           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32           4 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      33             : 
+      34             :   // | --------------------- load parameters -------------------- |
+      35           4 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      36             : 
+      37           4 :   ph->param_loader->loadParam("topics/orientation", orient_topic_);
+      38           4 :   ph->param_loader->loadParam("topics/angular_velocity", ang_vel_topic_);
+      39             : 
+      40           4 :   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           4 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerUpdate, this, false, false);  // not running after init
+      47           4 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerCheckHealth, this);
+      48             : 
+      49             :   // | --------------- subscribers initialization --------------- |
+      50             :   // subscriber to odometry
+      51           8 :   mrs_lib::SubscribeHandlerOptions shopts;
+      52           4 :   shopts.nh                 = nh;
+      53           4 :   shopts.node_name          = getPrintName();
+      54           4 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      55           4 :   shopts.threadsafe         = true;
+      56           4 :   shopts.autostart          = true;
+      57           4 :   shopts.queue_size         = 10;
+      58           4 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      59             : 
+      60           8 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orient_topic_,
+      61           4 :                                                                                 &HdgPassthrough::callbackOrientation, this);
+      62           8 :   sh_ang_vel_     = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, "/" + ch_->uav_name + "/" + ang_vel_topic_,
+      63           4 :                                                                          &HdgPassthrough::callbackAngularVelocity, this);
+      64             : 
+      65             :   // | ---------------- publishers initialization --------------- |
+      66           4 :   if (ch_->debug_topics.output) {
+      67           4 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+      68             :   }
+      69           4 :   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           4 :   if (changeState(INITIALIZED_STATE)) {
+      76           4 :     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           4 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84           7 : bool HdgPassthrough::start(void) {
+      85             : 
+      86           7 :   if (isInState(READY_STATE)) {
+      87           4 :     timer_update_.start();
+      88           4 :     changeState(STARTED_STATE);
+      89           4 :     return true;
+      90             : 
+      91             :   } else {
+      92           3 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+      93           3 :     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        9411 : void HdgPassthrough::callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     129             : 
+     130        9411 :   if (!isInitialized()) {
+     131           0 :     return;
+     132             :   }
+     133             : 
+     134             :   double hdg;
+     135             :   try {
+     136        9411 :     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        9411 :   setState(hdg, POSITION);
+     143             : 
+     144        9411 :   if (!isError()) {
+     145        9411 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, hdg, last_valid_hdg_);
+     146             :   }
+     147             : }
+     148             : /*//}*/
+     149             : 
+     150             : /*//{ callbackAngularVelocity() */
+     151        9108 : void HdgPassthrough::callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+     152             : 
+     153        9108 :   if (!isInitialized() || !sh_orientation_.hasMsg()) {
+     154           0 :     return;
+     155             :   }
+     156             : 
+     157             :   double hdg_rate;
+     158             :   try {
+     159        9108 :     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        9108 :   setState(hdg_rate, VELOCITY);
+     166             : }
+     167             : /*//}*/
+     168             : 
+     169             : /* timerUpdate() //{*/
+     170        4073 : void HdgPassthrough::timerUpdate(const ros::TimerEvent &event) {
+     171             : 
+     172             : 
+     173        4073 :   if (!isInitialized()) {
+     174           0 :     return;
+     175             :   }
+     176             : 
+     177        4073 :   publishOutput();
+     178        4073 :   publishDiagnostics();
+     179             : }
+     180             : /*//}*/
+     181             : 
+     182             : /*//{ timerCheckHealth() */
+     183        4148 : void HdgPassthrough::timerCheckHealth(const ros::TimerEvent &event) {
+     184             : 
+     185        4148 :   if (!isInitialized()) {
+     186           0 :     return;
+     187             :   }
+     188             : 
+     189        4148 :   if (isInState(INITIALIZED_STATE) && is_orient_ready_ && is_ang_vel_ready_) {
+     190             : 
+     191           4 :     changeState(READY_STATE);
+     192           4 :     ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     193             :   }
+     194             : 
+     195        4148 :   if (isInState(STARTED_STATE)) {
+     196             : 
+     197           4 :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator Running", getPrintName().c_str());
+     198           4 :     changeState(RUNNING_STATE);
+     199             :   }
+     200             : 
+     201        4148 :   if (sh_orientation_.hasMsg()) {
+     202        4148 :     is_orient_ready_ = true;
+     203             :   } else {
+     204           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation yet", getPrintName().c_str());
+     205             :   }
+     206             : 
+     207        4148 :   if (sh_ang_vel_.hasMsg()) {
+     208        4085 :     is_ang_vel_ready_ = true;
+     209             :   } else {
+     210          63 :     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       15626 : double HdgPassthrough::getState(const int &state_idx_in) const {
+     221       15626 :   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       18519 : void HdgPassthrough::setState(const double &state_in, const int &state_idx_in) {
+     231             : 
+     232       18519 :   const double prev_hdg_state   = mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     233       18519 :   prev_hdg_state_(state_idx_in) = prev_hdg_state;
+     234       18519 :   mrs_lib::set_mutexed(mtx_hdg_state_, state_in, hdg_state_(state_idx_in));
+     235             : 
+     236       18519 :   const double innovation = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(state_in), mrs_lib::geometry::radians(prev_hdg_state));
+     237       18519 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_(state_idx_in));
+     238       18519 : }
+     239             : /*//}*/
+     240             : 
+     241             : /*//{ getStates() */
+     242        4073 : HdgPassthrough::states_t HdgPassthrough::getStates(void) const {
+     243        4073 :   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        4073 : HdgPassthrough::covariance_t HdgPassthrough::getCovarianceMatrix(void) const {
+     265        4073 :   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           8 : std::string HdgPassthrough::getNamespacedName() const {
+     293          16 :   return parent_state_est_name_ + "/" + getName();
+     294             : }
+     295             : /*//}*/
+     296             : 
+     297             : /*//{ getPrintName() */
+     298          23 : std::string HdgPassthrough::getPrintName() const {
+     299          46 :   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..087266d0e3a3ed8494acc09be64e18612b33c2e8 GIT binary patch literal 1187 zcmV;U1YG-xP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpN?ZP&+7YuoIP_lj3Js&DJP6{Twk7$!yL+8}hNKshQuq>;G_x^gX zZu<-O{<;q!#6^T*aWXBBBziS)m5K6hXE|Ek)Bc zHL}N1w8tz`059;98&eG|;n%h&E6}OgnNg5(Zk6Dhu}2MMXs&af(lEr!83c^Q_5{Oj zUSf?<%F$o;VE@M9zE_S)RnLQET|Pt9E5>h-nuvMT%!%r1b`=bFrcp|uNTYp&q7jJ& zL>+7!8fA4#)@YJh8t~#UaNNf=wiK?96ly1H_4sSl5%b=9ba+^KIGKEKEI!MLfrCi7-!Z{Ql3K`u}#2q_-4WhE1o9D(hA#UxpDQ-EP zw8=XS_Aa`2viR;IP7Psytte+Ytw%JSnQvYJvR#k(Gc|?6VHtNVhA+9Rp$1IL;_tib zr0CUmxT|e!UA-Ltpt}Zh;WriPUEB?>@QO_{8L{FD2E9k??b!He;TKXIZLkSzL~&G9 z`&W$u#mARKyBZ$>y?cHAt#YR5Af-r-V!F9}G{o~=>j8l8Ns-^*ax|oFR9@lQ;65A4 zj6L0%Z*mCcAZ$gnB7i z%PGrJdd(>DmNE@P&;Sr6b)B3=8oOUt_S(mH`6+4Kl$TGDFcfHb?L03`doVwt2P5bvFzaY0cl3UG!3$Sanb{La(J8RZcQw|( z(TSn|%w&Z!_}q{c + + + + + + 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:8546618.2 %
Date:2023-11-30 22:31:42Functions: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 +
64.9%64.9%
+ 		64.9 %85 / 13152.2 %12 / 23
<unnamed>64.9 %85 / 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..44c4c8a3eb --- /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:8546618.2 %
Date:2023-11-30 22:31:42Functions: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 +
64.9%64.9%
+ 		64.9 %85 / 13152.2 %12 / 23
<unnamed>64.9 %85 / 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..f4a67e2577 --- /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:8546618.2 %
Date:2023-11-30 22:31:42Functions: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 +
64.9%64.9%
+ 		64.9 %85 / 13152.2 %12 / 23
<unnamed>64.9 %85 / 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..3e97b26621 --- /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:8546618.2 %
Date:2023-11-30 22:31:42Functions: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 +
64.9%64.9%
+ 		64.9 %85 / 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..f363d350a0 --- /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:8546618.2 %
Date:2023-11-30 22:31:42Functions: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 +
64.9%64.9%
+ 		64.9 %85 / 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..8300ef6f58 --- /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:8546618.2 %
Date:2023-11-30 22:31:42Functions: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 +
64.9%64.9%
+ 		64.9 %85 / 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..9346ccc743 --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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 +
57.5%57.5%
+ 		57.5 %233 / 40583.3 %25 / 30
<unnamed>57.5 %233 / 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..e398655596 --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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 +
57.5%57.5%
+ 		57.5 %233 / 40583.3 %25 / 30
<unnamed>57.5 %233 / 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..322eff7da3 --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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 +
57.5%57.5%
+ 		57.5 %233 / 40583.3 %25 / 30
<unnamed>57.5 %233 / 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..5e7087e1f6 --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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 +
57.5%57.5%
+ 		57.5 %233 / 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..cbd678b341 --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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 +
57.5%57.5%
+ 		57.5 %233 / 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..c7115163b6 --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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 +
57.5%57.5%
+ 		57.5 %233 / 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..40caef00dd --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::LatGeneric::isConverged()4
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)4
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const26
mrs_uav_state_estimators::LatGeneric::start()27
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const36
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)90
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)90
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) const2091
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)4054
mrs_uav_state_estimators::LatGeneric::setDt(double const&)4054
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const4054
mrs_uav_state_estimators::LatGeneric::getStates() const4054
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)4137
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const8046
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const8046
mrs_uav_state_estimators::LatGeneric::generateA()8112
mrs_uav_state_estimators::LatGeneric::generateB()8112
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&)10227
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)10227
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) const10227
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const16092
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const16092
mrs_uav_state_estimators::LatGeneric::getState(int const&) const37407
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const37407
+
+
+ + + +
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..177c0397d9 --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::LatGeneric::isConverged()4
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)4137
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&)10227
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)10227
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)4054
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)4
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&)4054
mrs_uav_state_estimators::LatGeneric::start()27
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)90
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)90
mrs_uav_state_estimators::LatGeneric::generateA()8112
mrs_uav_state_estimators::LatGeneric::generateB()8112
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const36
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const16092
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const16092
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const8046
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const8046
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const26
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const4054
mrs_uav_state_estimators::LatGeneric::getState(int const&) const37407
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const37407
mrs_uav_state_estimators::LatGeneric::getStates() const4054
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) const10227
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) const2091
+
+
+ + + +
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..69bc204ccd --- /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:23340557.5 %
Date:2023-11-30 22:31:42Functions: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           4 : void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      14             : 
+      15           4 :   ch_ = ch;
+      16           4 :   ph_ = ph;
+      17             : 
+      18           4 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      19             : 
+      20             :   // clang-format off
+      21           4 :   dt_ = 0.01;
+      22           4 :   input_coeff_ = 10;
+      23           4 :   default_input_coeff_ = 10;
+      24             : 
+      25           4 :   generateA();
+      26           4 :   generateB();
+      27             : 
+      28           4 :   H_ <<
+      29           4 :     1, 0, 0, 0, 0, 0,
+      30           4 :     0, 1, 0, 0, 0, 0;
+      31             : 
+      32             :   // clang-format on
+      33             : 
+      34             :   // | --------------- initialize parameter loader -------------- |
+      35             : 
+      36           4 :   if (is_core_plugin_) {
+      37             : 
+      38           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      39           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      40             :   }
+      41             : 
+      42           4 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      43             : 
+      44             :   // | --------------------- load parameters -------------------- |
+      45           4 :   ph->param_loader->loadParam("hdg_source_topic", hdg_source_topic_);
+      46           4 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      47           4 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      48           4 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      49           4 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      50           4 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      51           4 :   if (is_repredictor_enabled_) {
+      52           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      53             :   }
+      54             : 
+      55             :   // | --------------- corrections initialization --------------- |
+      56           4 :   ph->param_loader->loadParam("corrections", correction_names_);
+      57             : 
+      58          10 :   for (auto corr_name : correction_names_) {
+      59           6 :     corrections_.push_back(std::make_shared<Correction<lat_generic::n_measurements>>(
+      60        2103 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::LATERAL, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      61       10227 :         [this](const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      62       10227 :           return this->doCorrection(meas, R, state);
+      63             :         }));
+      64             :   }
+      65             : 
+      66             :   // | ------- check if all parameters loaded successfully ------ |
+      67           4 :   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           4 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      73             : 
+      74             :   // | ----------- initialize process noise covariance ---------- |
+      75           4 :   Q_ = Q_t::Zero();
+      76             :   double tmp_noise;
+      77           4 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      78           4 :   Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X)) = tmp_noise;
+      79           4 :   Q_(stateIdToIndex(POSITION, AXIS_Y), stateIdToIndex(POSITION, AXIS_Y)) = tmp_noise;
+      80           4 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      81           4 :   Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X)) = tmp_noise;
+      82           4 :   Q_(stateIdToIndex(VELOCITY, AXIS_Y), stateIdToIndex(VELOCITY, AXIS_Y)) = tmp_noise;
+      83           4 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      84           4 :   Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X)) = tmp_noise;
+      85           4 :   Q_(stateIdToIndex(ACCELERATION, AXIS_Y), stateIdToIndex(ACCELERATION, AXIS_Y)) = tmp_noise;
+      86             : 
+      87             :   // | ------------- initialize dynamic reconfigure ------------- |
+      88             :   drmgr_ =
+      89           4 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&LatGeneric::callbackReconfigure, this, _1, _2));
+      90           4 :   drmgr_->config.pos = Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X));
+      91           4 :   drmgr_->config.vel = Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X));
+      92           4 :   drmgr_->config.acc = Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X));
+      93           4 :   drmgr_->update_config(drmgr_->config);
+      94             : 
+      95             :   // | --------------- Kalman filter intialization -------------- |
+      96           4 :   const x_t        x0 = x_t::Zero();
+      97           4 :   const P_t        P0 = 1e3 * P_t::Identity();
+      98           4 :   const statecov_t sc0({x0, P0});
+      99           4 :   sc_ = sc0;
+     100             : 
+     101           4 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     102           4 :   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           4 :   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           8 :   mrs_lib::SubscribeHandlerOptions shopts;
+     125           4 :   shopts.nh                 = nh;
+     126           4 :   shopts.node_name          = getPrintName();
+     127           4 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     128           4 :   shopts.threadsafe         = true;
+     129           4 :   shopts.autostart          = true;
+     130           4 :   shopts.queue_size         = 10;
+     131           4 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     132             : 
+     133           4 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     134             :   sh_hdg_state_ =
+     135           4 :       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           4 :   if (ch_->debug_topics.input) {
+     139           4 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     140             :   }
+     141           4 :   if (ch_->debug_topics.output) {
+     142           4 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     143             :   }
+     144           4 :   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           4 :   if (changeState(INITIALIZED_STATE)) {
+     151           4 :     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           4 : }
+     156             : /*//}*/
+     157             : 
+     158             : /*//{ start() */
+     159          27 : bool LatGeneric::start(void) {
+     160             : 
+     161          27 :   if (isInState(READY_STATE)) {
+     162             :     /* timer_update_.start(); */
+     163           4 :     changeState(STARTED_STATE);
+     164           4 :     return true;
+     165             : 
+     166             :   } else {
+     167          23 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     168          23 :     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        4137 : void LatGeneric::timerUpdate(const ros::TimerEvent &event) {
+     219             : 
+     220        4137 :   if (!isInitialized()) {
+     221          83 :     return;
+     222             :   }
+     223             : 
+     224        4137 :   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           1 :     case READY_STATE: {
+     232           1 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     233           1 :       break;
+     234             :     }
+     235             : 
+     236          79 :     case INITIALIZED_STATE: {
+     237             :       // initialize the estimator with current corrections
+     238         124 :       for (auto correction : corrections_) {
+     239         120 :         auto res = correction->getProcessedCorrection();
+     240         120 :         if (res) {
+     241          45 :           auto measurement_stamped = res.value(); 
+     242          45 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     243          45 :           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
+     244          45 :           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          75 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getNamespacedName().c_str());
+     248          75 :           return;
+     249             :         }
+     250             :       }
+     251           4 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     252           4 :       changeState(READY_STATE);
+     253           4 :       break;
+     254             :     }
+     255             : 
+     256           4 :     case STARTED_STATE: {
+     257           4 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     258           4 :       if (isConverged()) {
+     259           4 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     260           4 :         changeState(RUNNING_STATE);
+     261             :       }
+     262           4 :       break;
+     263             :     }
+     264             : 
+     265        4053 :     case RUNNING_STATE: {
+     266       10126 :       for (auto correction : corrections_) {
+     267        6073 :         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        4053 :       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        4062 :   if (sh_control_input_.newMsg()) {
+     319        3212 :     is_input_ready_ = true;
+     320             :   }
+     321             : 
+     322             :   // check age of input
+     323        4062 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {  // TODO: parametrize, if older than say 1 second, eland
+     324           0 :     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           0 :     is_input_ready_ = false;
+     327             :   }
+     328             : 
+     329        4062 :   if (fun_get_hdg_()) {
+     330        4059 :     is_hdg_state_ready_ = true;
+     331             :   }
+     332             : 
+     333        4062 :   if (!isRunning() && !isStarted()) {
+     334           4 :     return;
+     335             :   }
+     336             : 
+     337        4058 :   if (first_iter_) {
+     338           4 :     first_iter_ = false;
+     339           4 :     return;
+     340             :   }
+     341             : 
+     342             :   // obtain dt for state prediction
+     343        4054 :   double dt = (event.current_real - event.last_real).toSec();
+     344        4054 :   if (dt <= 0.0) {  // sometimes the timer ticks twice simultaneously in simulation - we ignore the second tick
+     345           0 :     return;
+     346             :   }
+     347             : 
+     348        4054 :   if (!is_repredictor_enabled_) {  // repredictor requires constant dt TODO: how to handle repredictor + variable rate?
+     349        4054 :     setDt(dt);
+     350             :   }
+     351             : 
+     352             :   // obtain unbiased desired control acceleration in the estimator frame that will be used as input to the estimator
+     353        4054 :   u_t       u;
+     354        4054 :   ros::Time input_stamp;
+     355        4054 :   if (is_input_ready_ && is_hdg_state_ready_) {
+     356        3470 :     auto res = fun_get_hdg_();
+     357        3470 :     if (!res) {
+     358           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not obtain heading", getPrintName().c_str());
+     359           0 :       return;
+     360             :     }
+     361             : 
+     362        3470 :     const tf2::Vector3 des_acc_global = getAccGlobal(sh_control_input_.getMsg(), res.value());
+     363        3470 :     input_stamp                       = sh_control_input_.getMsg()->header.stamp;
+     364        3470 :     setInputCoeff(default_input_coeff_);
+     365        3470 :     u(0) = des_acc_global.getX();
+     366        3470 :     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         584 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     370         584 :     input_stamp = ros::Time::now();
+     371         584 :     setInputCoeff(0);
+     372         584 :     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        4054 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     386        4054 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     387             : 
+     388             :   // prediction step
+     389             :   try {
+     390             :     // Apply the prediction step
+     391        8108 :     std::scoped_lock lock(mutex_lkf_);
+     392        4054 :     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        4054 :       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        4054 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     406             : 
+     407             :   // publishing
+     408        4054 :   publishInput(u, input_stamp);
+     409        4054 :   publishOutput();
+     410        4054 :   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       10227 : void LatGeneric::doCorrection(const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     514       10227 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     515       10227 : }
+     516             : /*//}*/
+     517             : 
+     518             : /*//{ doCorrection() */
+     519       10227 : void LatGeneric::doCorrection(const z_t &z, const double R, const StateId_t &state_id, const ros::Time &meas_stamp) {
+     520             : 
+     521       10227 :   if (!isInitialized()) {
+     522          28 :     return;
+     523             :   }
+     524             : 
+     525             :   // we do not want to perform corrections until the estimator is initialized
+     526       10224 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     527          26 :     return;
+     528             :   }
+     529             : 
+     530             :   // for position state check the innovation
+     531       10197 :   if (state_id == POSITION) {
+     532             :     {
+     533        5589 :       std::scoped_lock lock(mtx_innovation_);
+     534             : 
+     535        5589 :       is_mitigating_jump_ = false;
+     536        5589 :       innovation_(0)      = z(0) - getState(POSITION, AXIS_X);
+     537        5589 :       innovation_(1)      = z(1) - getState(POSITION, AXIS_Y);
+     538             : 
+     539        5589 :       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        5589 :       innovation_ok_ = true;
+     572             :     }
+     573             :   }
+     574             : 
+     575       10197 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     576             :   try {
+     577             :     // Apply the correction step
+     578       20398 :     std::scoped_lock lock(mutex_lkf_);
+     579       10199 :     H_                           = H_t::Zero();
+     580       10199 :     H_(AXIS_X, state_id * 2)     = 1;
+     581       10199 :     H_(AXIS_Y, state_id * 2 + 1) = 1;
+     582       10199 :     lkf_->H                      = H_;
+     583             : 
+     584       10199 :     if (is_repredictor_enabled_) {
+     585             : 
+     586           0 :       lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[state_id]);
+     587             :     } else {
+     588       10199 :       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       10199 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     597             : }  // namespace mrs_uav_state_estimators
+     598             : /*//}*/
+     599             : 
+     600             : /*//{ isConverged() */
+     601           4 : bool LatGeneric::isConverged() {
+     602             : 
+     603             :   // TODO: check convergence by rate of change of determinant
+     604             : 
+     605           4 :   return true;
+     606             : }
+     607             : /*//}*/
+     608             : 
+     609             : /*//{ getState() */
+     610       37407 : double LatGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     611       37407 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     612             : }
+     613             : 
+     614       37407 : double LatGeneric::getState(const int &state_idx_in) const {
+     615       37407 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     616             : }
+     617             : /*//}*/
+     618             : 
+     619             : /*//{ setState() */
+     620          90 : void LatGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     621          90 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     622          90 : }
+     623             : 
+     624          90 : void LatGeneric::setState(const double &state_in, const int &state_idx_in) {
+     625          90 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     626          90 : }
+     627             : /*//}*/
+     628             : 
+     629             : /*//{ getStates() */
+     630        4054 : LatGeneric::states_t LatGeneric::getStates(void) const {
+     631        8108 :   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       16092 : double LatGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     643       16092 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     644             : }
+     645             : 
+     646       16092 : double LatGeneric::getCovariance(const int &state_idx_in) const {
+     647       16092 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     648             : }
+     649             : /*//}*/
+     650             : 
+     651             : /*//{ getCovarianceMatrix() */
+     652        4054 : LatGeneric::covariance_t LatGeneric::getCovarianceMatrix(void) const {
+     653        8108 :   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        8046 : double LatGeneric::getInnovation(const int &state_idx) const {
+     665        8046 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     666             : }
+     667             : 
+     668        8046 : double LatGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     669        8046 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     670             : }
+     671             : /*//}*/
+     672             : 
+     673             : /*//{ setDt() */
+     674        4054 : void LatGeneric::setDt(const double &dt) {
+     675        4054 :   dt_ = dt;
+     676        4054 :   generateA();
+     677        4054 :   generateB();
+     678        8108 :   std::scoped_lock lock(mutex_lkf_);
+     679        4054 :   lkf_->A = A_;
+     680        4054 :   lkf_->B = B_;
+     681        4054 : }
+     682             : /*//}*/
+     683             : 
+     684             : /*//{ setInputCoeff() */
+     685        4054 : void LatGeneric::setInputCoeff(const double &input_coeff) {
+     686        4054 :   input_coeff_ = input_coeff;
+     687        4054 :   generateA();
+     688        4054 :   generateB();
+     689        8108 :   std::scoped_lock lock(mutex_lkf_);
+     690        4054 :   lkf_->A = A_;
+     691        4054 :   lkf_->B = B_;
+     692        4054 : }
+     693             : /*//}*/
+     694             : 
+     695             : /*//{ generateA() */
+     696        8112 : void LatGeneric::generateA() {
+     697             : 
+     698             :   // clang-format off
+     699        8112 :     A_ <<
+     700        8112 :       1, 0, dt_, 0, 0.5 * dt_ * dt_, 0,
+     701        8112 :       0, 1, 0, dt_, 0, 0.5 * dt_ * dt_,
+     702        8112 :       0, 0, 1, 0, dt_, 0,
+     703        8112 :       0, 0, 0, 1, 0, dt_,
+     704        8112 :       0, 0, 0, 0, 1-(input_coeff_ * dt_), 0,
+     705        8112 :       0, 0, 0, 0, 0, 1-(input_coeff_ * dt_);
+     706             :   // clang-format on
+     707        8112 : }
+     708             : /*//}*/
+     709             : 
+     710             : /*//{ generateB() */
+     711        8112 : void LatGeneric::generateB() {
+     712             : 
+     713             :   // clang-format off
+     714        8112 :     B_ <<
+     715        8112 :       0, 0,
+     716        8112 :       0, 0,
+     717        8112 :       0, 0,
+     718        8112 :       0, 0,
+     719        8112 :       input_coeff_ * dt_, 0,
+     720        8112 :       0, input_coeff_ * dt_;
+     721             :   // clang-format on
+     722        8112 : }
+     723             : /*//}*/
+     724             : 
+     725             : /*//{ callbackReconfigure() */
+     726           4 : void LatGeneric::callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     727             : 
+     728           4 :   if (!isInitialized()) {
+     729           4 :     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          26 : std::string LatGeneric::getNamespacedName() const {
+     745          52 :   return parent_state_est_name_ + "/" + getName();
+     746             : }
+     747             : /*//}*/
+     748             : 
+     749             : /*//{ getPrintName() */
+     750          36 : std::string LatGeneric::getPrintName() const {
+     751          72 :   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..b1d0e18ba99c12b333e70d156d0e8e9508778b35 GIT binary patch literal 2961 zcmV;C3vTp@P)!WwqG^b+pML)=Nzu)KdeV?Z@ z#{CxNoA0O07G`{nV^Vnx&v*{1)f#8Zl1_i}lxQ<|8Tfd(M>BkQm;7Q{jAL3J-aq`Q zfc8g+;avGUzN{Z&pAiVzp<_VCfDKxs4X_J5OrA3V?T>Z_PKOy^kn!s@K;s&L*C(oB zwo$iY3}&!g^RVJCpq1_%tQRC_!U#$<0BGX$L2x_P>^RJa$8QYr8toppWTxS*!Wju^ zc&<-$RKb8Gh8ZKc2v}Qe0_YfXo+p)Oq*E?f%))O)IE_=)-`F8S8~%iDMREcNNT! zewpT%iQcwt-*@6o?9>#$09SZ#+mfrn+3;xXkNFNj(V`>37pMWS9bvyAfVTa^zI_}| z-UFUb1?TYSxYs@R49M?xX)g?~0kC!Ew#kGE%t+YBuJc;Akc3*2XHWNB#m({Oo4>l%@m&8nCU*l+{N!2_`70IEleWXhqG65qzn^4&+v z41!ppHfYuOq%|{-Hk9HqhuGDZA5jJ!O;t>XKB>v5mX7Jiu~}|;dwGYBMpiG=!;+g_ zMsiZwqwWGyH(n;xP%_mfxC?>wqJvoHDbyJQKK(Ts$2866=mV1x#M1eMq>D+Jvc|r} zwOR_Zl67|JH-=fvs0wYe&e&Az03~RM9CcsN#6~dWk6fle3{^8N9su&)3?a;G+E|#GUBpc zVfG;LrUN#`5r_Up9@twhmm0}M=fl+0a7mRE=V6sK8XtW~I7qf^@2hlJF!=fZVaePR zphX@}&zl{baeWTffw;?MvlRQlNPq@9E=Fd*gN%+(SYs+C7KgduQ3K|xCUns7GlqwA zk$4N}`mtdyRlK>DlQ5# zJ9VN*f{Has7er=a1(L=H{941K3E0Mm#tq=p2Dp=8r3N&yhSD*}0*!xm1ZWy|UQaO! zDD6UX%7io;w^H9VE>S|Jq$e^SOuQNCXvm1YZ+n6aNi&{nd(5Lo8XS8{BSfAZ<@Zb1 z)t=>op<~3nra2)Gq>aMtTWx!qa=fI4jMDrG!4Ehsu|~<$fAWA94zbW6Wkb1?(~bS< zy@hq1OY%Taol9@vzaV0dw(PKGom2@R;{h!u0GpT{ZjUOHe#E{=vc(a>s7F=&)OdYC zqh!Il7qKHYtI@{=Ntf)ddUdIvO$^B{b-Tt?-|qu{2AL^#;oYL@C`rbU4nwU9v}=Jh zp3K~ zmlCnCv5$-@PF3nknK4Y+?b=zY-8K1o8;UNFmtqp1i&CGcfum7|vVCL&fDlvx$&lzs zHY&Bh#|UT}DGe7y?l!0D6ltSL~NWbZd09c5RX&b+wK*ecHokAQ>TOkTJKOB`3ExFX&9M0)i?S+ZFjH z?G4TY;XFlMmu7>=$r$aeF*6LFx|e#U26B5=a<3@k5Zrz_qX~h+miMZ?%9@Z6fF%l^ z@rwS(s+%I-B#>=amMiA`3?B%Lt3r2)q)$?fK2*8p06Y7Kz5ns59wf;eqw%bhJ>V1t zfg8^jj`bjY#$jKd1n-j?Sbj}0{!eoDrS?F3hQ9$B9Un-hLW&gdr~&Os>BD|AyH^Xi z^*67aBZc{dZYehF+Dcgzv-beEvL%%SR(Pw@nReQ;#$%X@@$94XrpV=*``wD`**SZT zID4ZAdviFgXDJM8yi{j~E0C$EuoD3yl?qMD>s0(!)UoQKYjgHWZFr<|W&->u!M~&+ z_7{us>kr{fCAZIiq8R_}_M`Uk^unU8 zkpsuqyV8xnUKci73X6aY1PPoo`MU~>2|M*;jRSo_?SSQli>xvmdU$M7)TPxBR}^KO69qoTMkJqGwZ zulM!QZ?NE%6xp#h4{?pv(O-6r=-)q$GZoU0a&(t4313{=VvaDiV{L?6Olv$i#NR!O zEK#`~?+em6-IhUCxEg6x$=HiLtX&g3!ij=UtMi+=6gQa2;z@6O>{${ zjyKCeS6Ervcm?~|s^MR-k9dt9C7(L>F{|Tu$YYl|JQA%k{wOW%V$HO=m1<~sscB(3 zA^D56kQiM_3+Yu7L%cQ*O$#r!6j#aP_|=o~W)W_U1E1Lx#`%D~6hH*DtkMqn1@4SN zxftun95t*y`${Wm{LexSaF~S#kCbSkg0&SH`=^o_KX)tUd*WrP=`#uV|Ca$>IA|9? zU`+=Ur)mL(shAm)_&?Y?ZOM$F6&zp!kZ;CKfd7lxnPT?+fLUlX!9c}N{lhnYl$aSj zd)7TlDha#5?`o4cItxwi_1gIUd-KM30WzTJt+xR;v8MUS+e7g=Qq7oxBvZ+*g|m(o z;KO`sG5dI|U}}GAux9UWq*yz84ec800000NkvXX Hu0mjfLhz-Z literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html new file mode 100644 index 0000000000..6daa5b9df8 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()1
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()1
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().21
+
+
+ + + +
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..1a68f9eac6 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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           1 :   GpsGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+      15           1 :   }
+      16             : 
+      17           2 :   ~GpsGarmin(void) {
+      18           2 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_garmin
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25           1 : 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..b54ebd5c70 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html @@ -0,0 +1,164 @@ + + + + + + + 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:20730867.2 %
Date:2023-11-30 22:31:42Functions:192479.2 %
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 +
65.5%65.5%
+ 		65.5 %192 / 29358.3 %7 / 12
<unnamed>65.5 %192 / 29358.3 %7 / 12
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
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html new file mode 100644 index 0000000000..16037213b9 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html @@ -0,0 +1,164 @@ + + + + + + + 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:20730867.2 %
Date:2023-11-30 22:31:42Functions:192479.2 %
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 +
65.5%65.5%
+ 		65.5 %192 / 29358.3 %7 / 12
<unnamed>65.5 %192 / 29358.3 %7 / 12
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
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-detail.html b/mrs_uav_state_estimators/src/estimators/state/index-detail.html new file mode 100644 index 0000000000..ad5c069826 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail.html @@ -0,0 +1,164 @@ + + + + + + + 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:20730867.2 %
Date:2023-11-30 22:31:42Functions:192479.2 %
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
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
state_generic.cpp +
65.5%65.5%
+ 		65.5 %192 / 29358.3 %7 / 12
<unnamed>65.5 %192 / 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..161a389d16 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-sort-f.html @@ -0,0 +1,132 @@ + + + + + + + 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:20730867.2 %
Date:2023-11-30 22:31:42Functions:192479.2 %
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 +
65.5%65.5%
+ 		65.5 %192 / 29358.3 %7 / 12
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html new file mode 100644 index 0000000000..1fea152be4 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html @@ -0,0 +1,132 @@ + + + + + + + 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:20730867.2 %
Date:2023-11-30 22:31:42Functions:192479.2 %
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 +
65.5%65.5%
+ 		65.5 %192 / 29358.3 %7 / 12
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index.html b/mrs_uav_state_estimators/src/estimators/state/index.html new file mode 100644 index 0000000000..088a1b2faa --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index.html @@ -0,0 +1,132 @@ + + + + + + + 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:20730867.2 %
Date:2023-11-30 22:31:42Functions:192479.2 %
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
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
state_generic.cpp +
65.5%65.5%
+ 		65.5 %192 / 29358.3 %7 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + 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..76ea48ac81 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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..fc0aa3e50e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions: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..58e8910ded --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2023-11-30 22:31:42Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a73b427303050221946009b0afa810dd7c98d9b4 GIT binary patch literal 229 zcmeAS@N?(olHy`uVBq!ia0vp^0YEIt!VDxsGPYO&DTx4|5ZC|z|E~gqp;PJr literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..83b6fb6fcd --- /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:19229365.5 %
Date:2023-11-30 22:31:42Functions: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&)4
mrs_uav_state_estimators::StateGeneric::start()57
mrs_uav_state_estimators::StateGeneric::updateUavState()4023
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)4149
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)4149
mrs_uav_state_estimators::StateGeneric::getHeading() const7532
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()() const7532
+
+
+ + + +
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..1c7385fb85 --- /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:19229365.5 %
Date:2023-11-30 22:31:42Functions: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&)4
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&)4149
mrs_uav_state_estimators::StateGeneric::updateUavState()4023
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)4149
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::start()57
mrs_uav_state_estimators::StateGeneric::getHeading() const7532
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()() const7532
+
+
+ + + +
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..bc4b92a5d6 --- /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:19229365.5 %
Date:2023-11-30 22:31:42Functions: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           4 : void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      12             : 
+      13           4 :   ch_ = ch;
+      14           4 :   ph_ = ph;
+      15             : 
+      16           8 :   ros::NodeHandle nh(parent_nh);
+      17             : 
+      18           4 :   if (is_core_plugin_) {
+      19             : 
+      20           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      21           4 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      22             :   }
+      23             : 
+      24           4 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26           4 :   ph->param_loader->loadParam("estimators/lateral/name", est_lat_name_);
+      27           4 :   ph->param_loader->loadParam("estimators/altitude/name", est_alt_name_);
+      28           4 :   ph->param_loader->loadParam("estimators/heading/name", est_hdg_name_);
+      29           4 :   ph->param_loader->loadParam("estimators/heading/passthrough", is_hdg_passthrough_);
+      30             : 
+      31           4 :   ph->param_loader->loadParam("override_frame_id/enabled", is_override_frame_id_);
+      32           4 :   if (is_override_frame_id_) {
+      33           0 :     ph->param_loader->loadParam("override_frame_id/frame_id", frame_id_);
+      34             :   }
+      35             : 
+      36           8 :   std::string topic_orientation;
+      37           4 :   ph->param_loader->loadParam("topics/orientation", topic_orientation);
+      38           4 :   topic_orientation_ = "/" + ch_->uav_name + "/" + topic_orientation;
+      39           8 :   std::string topic_angular_velocity;
+      40           4 :   ph->param_loader->loadParam("topics/angular_velocity", topic_angular_velocity);
+      41           4 :   topic_angular_velocity_ = "/" + ch_->uav_name + "/" + topic_angular_velocity;
+      42             : 
+      43           4 :   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           4 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      49             : 
+      50             :   // | ------------------ timers initialization ----------------- |
+      51           4 :   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           4 :   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
+      54             : 
+      55             :   // | --------------- subscribers initialization --------------- |
+      56           8 :   mrs_lib::SubscribeHandlerOptions shopts;
+      57           4 :   shopts.nh                 = nh;
+      58           4 :   shopts.node_name          = getPrintName();
+      59           4 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      60           4 :   shopts.threadsafe         = true;
+      61           4 :   shopts.autostart          = true;
+      62           4 :   shopts.queue_size         = 10;
+      63           4 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      64             : 
+      65           4 :   sh_hw_api_orient_  = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, topic_orientation_);
+      66           4 :   sh_hw_api_ang_vel_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, topic_angular_velocity_);
+      67             : 
+      68             :   // | ---------------- publishers initialization --------------- |
+      69           4 :   ph_odom_     = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);
+      70           4 :   ph_attitude_ = mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped>(nh, Support::toSnakeCase(getName()) + "/attitude", 10);
+      71             : 
+      72           4 :   if (ch_->debug_topics.state) {
+      73           4 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      74             :   }
+      75           4 :   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           4 :   if (ch_->debug_topics.innovation) {
+      80           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      81             :   }
+      82           4 :   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           8 :   std::vector<double> max_altitudes;
+      88             : 
+      89           4 :   if (is_hdg_passthrough_) {
+      90           4 :     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           4 :   est_hdg_->initialize(nh, ch_, ph_);
+      95           4 :   max_altitudes.push_back(est_hdg_->getMaxFlightZ());
+      96             : 
+      97        7536 :   est_lat_ = std::make_unique<LatGeneric>(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); });
+      98           4 :   est_lat_->initialize(nh, ch_, ph_);
+      99           4 :   max_altitudes.push_back(est_lat_->getMaxFlightZ());
+     100             : 
+     101           4 :   est_alt_ = std::make_unique<AltGeneric>(est_alt_name_, frame_id_, getName(), is_core_plugin_);
+     102           4 :   est_alt_->initialize(nh, ch_, ph_);
+     103           4 :   max_altitudes.push_back(est_alt_->getMaxFlightZ());
+     104             : 
+     105           4 :   max_flight_z_ = *std::min_element(max_altitudes.begin(), max_altitudes.end());
+     106             : 
+     107             :   // | ------------------ initialize published messages ------------------ |
+     108           4 :   uav_state_init_.header.frame_id = ns_frame_id_;
+     109           4 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+     110             : 
+     111           4 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+     112           4 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+     113           4 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+     114             : 
+     115           4 :   innovation_init_.header.frame_id         = ns_frame_id_;
+     116           4 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+     117           4 :   innovation_init_.pose.pose.orientation.w = 1.0;
+     118             : 
+     119             :   // | ------------------ finish initialization ----------------- |
+     120             : 
+     121           4 :   if (changeState(INITIALIZED_STATE)) {
+     122           4 :     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           4 : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ start() */
+     130          57 : bool StateGeneric::start(void) {
+     131             : 
+     132          57 :   if (isInState(READY_STATE)) {
+     133             : 
+     134             :     bool est_lat_start_successful, est_alt_start_successful, est_hdg_start_successful;
+     135             : 
+     136          57 :     if (est_lat_->isStarted() || est_lat_->isRunning()) {
+     137          30 :       est_lat_start_successful = true;
+     138             :     } else {
+     139          27 :       est_lat_start_successful = est_lat_->start();
+     140             :     }
+     141             : 
+     142          57 :     if (est_alt_->isStarted() || est_alt_->isRunning()) {
+     143           4 :       est_alt_start_successful = true;
+     144             :     } else {
+     145          53 :       est_alt_start_successful = est_alt_->start();
+     146             :     }
+     147             : 
+     148          57 :     if (est_hdg_->isStarted() || est_hdg_->isRunning()) {
+     149          50 :       timer_pub_attitude_.start();
+     150          50 :       est_hdg_start_successful = true;
+     151             :     } else {
+     152           7 :       est_hdg_start_successful = est_hdg_->start();
+     153             :     }
+     154             : 
+     155          57 :     if (est_lat_start_successful && est_alt_start_successful && est_hdg_start_successful) {
+     156             :       /* timer_update_.start(); */
+     157           4 :       changeState(STARTED_STATE);
+     158           4 :       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          53 :   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        4149 : void StateGeneric::timerUpdate(const ros::TimerEvent &event) {
+     209             : 
+     210             : 
+     211        4149 :   if (!isInitialized()) {
+     212         126 :     return;
+     213             :   }
+     214             : 
+     215        8252 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     216             : 
+     217        4126 :   switch (getCurrentSmState()) {
+     218             : 
+     219           0 :     case UNINITIALIZED_STATE: {
+     220           0 :       break;
+     221             :     }
+     222          46 :     case INITIALIZED_STATE: {
+     223             : 
+     224          46 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     225           4 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     226           4 :           changeState(READY_STATE);
+     227           4 :           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          42 :         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          42 :         return;
+     235             :       }
+     236             : 
+     237           4 :       break;
+     238             :     }
+     239             : 
+     240          55 :     case READY_STATE: {
+     241          55 :       break;
+     242             :     }
+     243             : 
+     244           6 :     case STARTED_STATE: {
+     245             : 
+     246           6 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     247             : 
+     248           6 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     249           4 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     250           4 :         changeState(RUNNING_STATE);
+     251             :       } else {
+     252           2 :         return;
+     253             :       }
+     254           4 :       break;
+     255             :     }
+     256             : 
+     257        4019 :     case RUNNING_STATE: {
+     258        4019 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     259           0 :         changeState(ERROR_STATE);
+     260             :       }
+     261        4019 :       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        4082 :   if (!isRunning() && !isStarted()) {
+     277          59 :     return;
+     278             :   }
+     279             : 
+     280        4023 :   updateUavState();
+     281             : 
+     282        4023 :   publishUavState();
+     283        4023 :   publishOdom();
+     284        4023 :   publishCovariance();
+     285        4023 :   publishInnovation();
+     286        4023 :   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        4149 : void StateGeneric::timerPubAttitude(const ros::TimerEvent &event) {
+     366             : 
+     367        4149 :   if (!isInitialized()) {
+     368          75 :     return;
+     369             :   }
+     370             : 
+     371        8252 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerPubAttitude", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     372             : 
+     373        4126 :   if (!sh_hw_api_orient_.hasMsg()) {
+     374           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());
+     375           0 :     return;
+     376             :   }
+     377             : 
+     378        4126 :   if (!est_hdg_->isRunning() && !isError()) {
+     379          52 :     ROS_WARN_THROTTLE(1.0, "[%s]: cannot publish attitude, heading estimator is not running", getPrintName().c_str());
+     380          52 :     return;
+     381             :   }
+     382             : 
+     383        4074 :   scope_timer.checkpoint("checks");
+     384             : 
+     385        4074 :   const ros::Time time_now = ros::Time::now();
+     386             : 
+     387        4074 :   geometry_msgs::QuaternionStamped att;
+     388        4074 :   att.header.stamp    = time_now;
+     389        4074 :   att.header.frame_id = ns_frame_id_ + "_att_only";
+     390             : 
+     391             :   double hdg;
+     392        4074 :   if (isError()) {
+     393           0 :     hdg = est_hdg_->getLastValidHdg();
+     394             :   } else {
+     395        4074 :     hdg = est_hdg_->getState(POSITION);
+     396             :   }
+     397             : 
+     398        4074 :   auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     399        4074 :   if (res) {
+     400        4074 :     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        4074 :   scope_timer.checkpoint("rotate");
+     407             : 
+     408        4074 :   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        4074 :   scope_timer.checkpoint("nan check");
+     414             : 
+     415        4074 :   ph_attitude_.publish(att);
+     416        4074 :   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        4023 : void StateGeneric::updateUavState() {
+     432             : 
+     433        4023 :   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        4023 :   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        8046 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::updateUavState", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     443             : 
+     444        4023 :   const ros::Time time_now = ros::Time::now();
+     445             : 
+     446        4023 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     447        4023 :   uav_state.header.stamp       = time_now;
+     448             : 
+     449             :   // do not rotate orientation if passthrough hdg
+     450        4023 :   if (est_hdg_name_ == "hdg_passthrough") {
+     451           0 :     uav_state.pose.orientation = sh_hw_api_orient_.getMsg()->quaternion;
+     452             :   } else {
+     453             :     double hdg;
+     454        4023 :     if (isError()) {
+     455           0 :       hdg = est_hdg_->getLastValidHdg();
+     456             :     } else {
+     457        4023 :       hdg = est_hdg_->getState(POSITION);
+     458             :     }
+     459             : 
+     460        4023 :     auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     461        4023 :     if (res) {
+     462        4023 :       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        4023 :   scope_timer.checkpoint("rotate orientation");
+     470             : 
+     471        4023 :   uav_state.velocity.angular = sh_hw_api_ang_vel_.getMsg()->vector;
+     472             : 
+     473        4023 :   uav_state.pose.position.x = est_lat_->getState(POSITION, AXIS_X);
+     474        4023 :   uav_state.pose.position.y = est_lat_->getState(POSITION, AXIS_Y);
+     475        4023 :   uav_state.pose.position.z = est_alt_->getState(POSITION);
+     476             : 
+     477        4023 :   uav_state.velocity.linear.x = est_lat_->getState(VELOCITY, AXIS_X);  // in global frame
+     478        4023 :   uav_state.velocity.linear.y = est_lat_->getState(VELOCITY, AXIS_Y);  // in global frame
+     479        4023 :   uav_state.velocity.linear.z = est_alt_->getState(VELOCITY);          // in global frame
+     480             : 
+     481        4023 :   uav_state.acceleration.linear.x = est_lat_->getState(ACCELERATION, AXIS_X);  // in global frame
+     482        4023 :   uav_state.acceleration.linear.y = est_lat_->getState(ACCELERATION, AXIS_Y);  // in global frame
+     483        4023 :   uav_state.acceleration.linear.z = est_alt_->getState(ACCELERATION);          // in global frame
+     484             : 
+     485        4023 :   scope_timer.checkpoint("fill uav state");
+     486             : 
+     487        8046 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     488        4023 :   scope_timer.checkpoint("uav state to odom");
+     489             : 
+     490        8046 :   nav_msgs::Odometry innovation = innovation_init_;
+     491        4023 :   innovation.header.stamp       = time_now;
+     492             : 
+     493        4023 :   innovation.pose.pose.position.x = est_lat_->getInnovation(POSITION, AXIS_X);
+     494        4023 :   innovation.pose.pose.position.y = est_lat_->getInnovation(POSITION, AXIS_Y);
+     495        4023 :   innovation.pose.pose.position.z = est_alt_->getInnovation(POSITION);
+     496             : 
+     497        4023 :   is_mitigating_jump_ = est_alt_->isMitigatingJump() || est_lat_->isMitigatingJump() || est_hdg_->isMitigatingJump();
+     498             : 
+     499        4023 :   scope_timer.checkpoint("innovation");
+     500             : 
+     501        8046 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     502        4023 :   pose_covariance.header.stamp  = time_now;
+     503        4023 :   twist_covariance.header.stamp = time_now;
+     504             : 
+     505        4023 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     506        4023 :   pose_covariance.values.resize(n_states * n_states);
+     507        4023 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(POSITION, AXIS_X);
+     508        4023 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(POSITION, AXIS_Y);
+     509        4023 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(POSITION);
+     510             : 
+     511        4023 :   twist_covariance.values.resize(n_states * n_states);
+     512        4023 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(VELOCITY, AXIS_X);
+     513        4023 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(VELOCITY, AXIS_Y);
+     514        4023 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(VELOCITY);
+     515             : 
+     516        4023 :   scope_timer.checkpoint("covariance");
+     517             : 
+     518        4023 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     519        4023 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     520        4023 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     521        4023 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     522        4023 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     523             : }
+     524             : /*//}*/
+     525             : 
+     526             : /*//{ getHeading() */
+     527        7532 : std::optional<double> StateGeneric::getHeading() const {
+     528        7532 :   if (!est_hdg_->isRunning()) {
+     529           3 :     return {};
+     530             :   }
+     531        7529 :   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..2c4639ea169968e1ee482cdb48846751637d629b GIT binary patch literal 1960 zcmV;Z2UqxsP)))ZGD-wUFz$*Is$z9r&7tU4sJd=Aq&q}N)B0Xo;hhAKQg9d3RhX(C$=sl zy(P?pT!S>RFr4$e_fYN1+3v#V0G(h3z*uk*P&)Om};Qv_O}U-gjB#Cerw5@^IoMQn7H;aSH{ub6j!CD9_Y%gqvce&J`{g`uucn zb~DD27)cA|jE(?tJ<8e%x;b;$ez|DhOeDA?oql20L5vp|OnUszR#&$L53tmK4hrdx@-YIK!Pr0NRf$UP*z%z!dH$)VouNNjJx|&?|L)|GDH0T>uxvg7CG8EBdYWw! zQXNTF*nCphcKhCE1>VsVb3|Ktt$@tXlCQ6rIiG=(MpBxr9ZX$~sJ?#?Y)u1BaXfwh zZn9;-F&U0=mn=%1qJnWGV8tJeEpUqHOpAKjCf8PAE|5m&%wO-1qAxm(WWm4t6~DjV zLTlXamVpQ8;ASXbamqAVhG=-_N~Y|zVFYHYnLxvs5mVt2(o7jJy&12_&#Q83Su22p z6f^P~W$`Ob=&t?1+tO}-g3!fo>nb1}NF3u4DXw@Xho8g4t2596Vy=5y1~FpGUWK>; zp(2&53nPcr*)rOaW^g4Q`9#nG?}?3IM3xmcq|ik3OBx!awGja8!QNoDIeZtZ0dhjC zQ(T?)A7$-vJ$CM;Sjqs#A&WDog<_0aK7+?aLUPDcQ0Uo}5hf@$^P{UBWC>za?>TCjIlzFFmNYbno?7frb4IYauS{})YW}_wU z#upX6O=$Y7r~B5_;NRY%rJM1eP-s>#-?@DS0ybbQ!`=ccXIJ<}&Zw?}TTYd5M_q+$ z)#(?W`!#2!7<6bab8RLhokn$|@n-4*1fW?OFiqFC?sZ!p~W4+qL zNSmxrglV-ir+_h%AVq$s^LsM`int~>BObp?RcCh2xUXE-3}7+#AQ8n3n1$v$9QR$` zW4=T^Z|AXT75D8XM62+3-Y=wE=SsM9Cl)pN>DQCDwrKrT_dX&$3Qf5wO&;!O6V0%j zzf1a_g->+S&g&>6b*7)G+>898Kf~7L<#<#3;o2B_ZKrw-W8^(~F6r?iU66%h^uFyF zO*egkJlbtLK%Da4Q)^2f9v&~LB@ai+BYV>>p2~tAN4S})pF%n}m7$t5n?4 zl)_8H#ChO>A=Z1IPmvKUd)(-9P|BTYU+q%$tqT9?;U@qXOwIZS6l zu#$qyy#*}y&V@Kq?oD7f%-&;1%DrJ*5bEWgQxCR7gMCvf`tWlf1}yg+V107G_eM7# zx$}=+kIdv^*O^mT)DvGfSpr)knNu8ywj%d>h&>k4IfcJRJr4LKKC!po?Hjj#$ zjRQ7d)C6B9XR-(Vgb#goc#SmC7a^TA3nFf$9z;)D+ci#4!1Xqd$u#Kq059nz@;!`! zueel-0ShL~squ9WcNN?bJnZ8L$o^wpc*t1p*-9C}15E2RGSvvco0cKUTx)u=9)mo;VwC~icHK+`kl~SB$pe!lz?IA!T>+%ei};AnM|pL9tglnnoy4rF zY&mOxejTti{v4#F3y=r=1ckS@n&_@6d)<9`jg9z-XsLNc35CZ_Y+*}>dws%@JiEur uwOGR5t1bL-0$F@(mut3*dFVN{(D)B^h&w>=SXc=F0000 + + + + + + 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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
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 +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 18
<unnamed>43.0 %61 / 14233.3 %6 / 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..d8aee66b3d --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
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 +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 18
<unnamed>43.0 %61 / 14233.3 %6 / 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..6f28509981 --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
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 +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 18
<unnamed>43.0 %61 / 14233.3 %6 / 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..9bd374a819 --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
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 +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 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..d0f59d323d --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
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 +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 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..659f415c72 --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
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 +
43.0%43.0%
+ 		43.0 %61 / 14233.3 %6 / 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..85290a5b06 --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
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::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> 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::deactivate()1
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
(anonymous namespace)::ProxyExec0::ProxyExec0()3
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>)3
mrs_uav_trackers::joy_tracker::JoyTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)13
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
+
+
+ + + +
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..b365a3aeb9 --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()3
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()1
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>)3
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&)13
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
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&)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::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
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..9ada1d55e2 --- /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:6114243.0 %
Date:2023-11-30 22:31:42Functions:61833.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 <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           3 : 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           3 :   this->common_handlers_  = common_handlers;
+     136           3 :   this->private_handlers_ = private_handlers;
+     137             : 
+     138           3 :   _uav_name_ = common_handlers->uav_name;
+     139             : 
+     140           3 :   nh_ = nh;
+     141             : 
+     142           3 :   ros::Time::waitForValid();
+     143             : 
+     144             :   // --------------------------------------------------------------
+     145             :   // |                     loading parameters                     |
+     146             :   // --------------------------------------------------------------
+     147             : 
+     148             :   // | ---------- loading params using the parent's nh ---------- |
+     149             : 
+     150           6 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     151             : 
+     152           3 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154           3 :   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           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/joy_tracker.yaml");
+     162           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/joy_tracker.yaml");
+     163             : 
+     164           6 :   const std::string yaml_prefix = "mrs_uav_trackers/joy_tracker/";
+     165             : 
+     166           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     167             : 
+     168           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_tilt", _max_tilt_);
+     169             : 
+     170           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     171             : 
+     172             :   // load channels
+     173           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/pitch", _channel_pitch_);
+     174           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/roll", _channel_roll_);
+     175           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/heading", _channel_heading_);
+     176           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/throttle", _channel_throttle_);
+     177             : 
+     178             :   // load channel multipliers
+     179           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/pitch", _channel_mult_pitch_);
+     180           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/roll", _channel_mult_roll_);
+     181           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/heading", _channel_mult_heading_);
+     182           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/throttle", _channel_mult_throttle_);
+     183             : 
+     184           3 :   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           3 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "JoyTracker", _profiler_enabled_);
+     192             : 
+     193             :   // | ----------------------- subscribers ---------------------- |
+     194             : 
+     195           3 :   mrs_lib::SubscribeHandlerOptions shopts;
+     196           3 :   shopts.nh              = nh_;
+     197           3 :   shopts.node_name       = "JoyTracker";
+     198           3 :   shopts.queue_size      = 1;
+     199           3 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     200             : 
+     201           3 :   sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick");
+     202             : 
+     203             :   // | --------------------- finish the init -------------------- |
+     204             : 
+     205           3 :   last_update = ros::Time(0);
+     206             : 
+     207           3 :   is_initialized_ = true;
+     208             : 
+     209           3 :   ROS_INFO("[JoyTracker]: initialized");
+     210             : 
+     211           3 :   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           1 : void JoyTracker::deactivate(void) {
+     277             : 
+     278           1 :   is_active_ = false;
+     279             : 
+     280           1 :   ROS_INFO("[JoyTracker]: deactivated");
+     281           1 : }
+     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        5071 : 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       15213 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     300       15213 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("JoyTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     301             : 
+     302             :   {
+     303        5071 :     std::scoped_lock lock(mutex_uav_state_);
+     304             : 
+     305        5071 :     uav_state_ = uav_state;
+     306             : 
+     307        5071 :     got_uav_state_ = true;
+     308             :   }
+     309             : 
+     310        5071 :   double dt = (ros::Time::now() - last_update).toSec();
+     311             : 
+     312        5071 :   last_update = ros::Time::now();
+     313             : 
+     314             :   // up to this part the update() method is evaluated even when the tracker is not active
+     315        5071 :   if (!is_active_) {
+     316        5071 :     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           1 : const std_srvs::SetBoolResponse::ConstPtr JoyTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     388             : 
+     389           2 :   std_srvs::SetBoolResponse res;
+     390           2 :   std::stringstream         ss;
+     391             : 
+     392           1 :   if (cmd->data != callbacks_enabled_) {
+     393             : 
+     394           0 :     callbacks_enabled_ = cmd->data;
+     395             : 
+     396           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     397           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     398             : 
+     399             :   } else {
+     400             : 
+     401           1 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     402           1 :     ROS_WARN_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     403             :   }
+     404             : 
+     405           1 :   res.message = ss.str();
+     406           1 :   res.success = true;
+     407             : 
+     408           2 :   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          13 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr JoyTracker::setConstraints([
+     465             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     466             : 
+     467          13 :   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           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr JoyTracker::setTrajectoryReference([
+     493             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     494           0 :   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           3 : 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..3628c4f90e405b55819055065724b7b4d8709f37 GIT binary patch literal 1765 zcmV3t53EAPUAv~oGID{Py(Sn7iVz^5u|S&uav9m`@n!5J?e>9~QQbnSam4o= zyh%rKKBM(er6;4bs?S(9P}sFrRp)gBGp_FkM#i~MZLSRO8FgLZexPe*Jo1)c45L-$ z8f;C5TZcgIAo^gY{c{xGW_&@hiL^0Ik?s?*HDJyAcm~*E{Dg*j3Ix?gMolp2>X4C) z9MK)UtOA_`B4uJm$AGlS0&g%$HVQyqR>GArV3bVOydBB=9E0C~vVe*UypRl_wleaS z=AwNE0-JIe3(bmG2Wpwt6P}g2Ke=|wuoe(Lqm%It<5l2^r~n0BgIf(?yBSH|ag}K_ z4$7s5YilJMe5U+>C*i=uszF#XDjfWssL1u?jF(WHT9fuPVi-eZ^h`#vOBps$ptV?> zG$V3c1=FH)2;*kW*fx0?lOO;NG>_#}@G3L8Gz?%_Nd&BPtx+<<1=hTWmyFEZ9D_2J zcU?$;EQ!f$VZ;r4Xm{LdjsYJ+Z|Q|;+*TbhKCoG!kk_}Zbc)xXILLTo-62`P>O9HG~6-hj4~{9-8=c4PmhffP%3$5yw8RAr$}xjJZbGM>4L%8Kp8_k%Coe zv$|f(W9@+p7kE-~wO-^}SBrbT!G96R_nus!y_%0{Z;NJO0@oAG;y_|e?w>?%hD8dt z3Wh}7Yedsy{ejf!%%`#$kY_YN_p$(7FJL~UA~G(q#~~oIImQGTHbeW!D7$^7x<|&j zPsuz&t#8g!2HUoLI6v=aTuNE612R%@17x>H4v|Nm*qsf9(6b%iAgn^0c+v3+r0$ZV z9CRg&Ajv+6@uWPgjHR^!mJei*B>I8nV!J+)(F7ro2 zw(liz`HT}c_}VPxIAUE$3Asbv1^vyqE&wFx__0+Xu0vx zE91Q)qczu#Xm*vXDR2rKoweI>$SJoOZO*6A$K$S@)&v-eE0OUU2Ej-&MkcLHN=B+o z#^{nEq%IDTZ8xv2i(e}j&&40O%Vq&h^-wjsRE7{r*}Uf0vLpMPmC3uHzVK}dNn zlX`GJJd;qi>@}Bj(+ARPb~4QF`oSR_c2^}$dB53oQ#@zt>-CG~%)p!76Lm7#-EcFi z>}HFU|}12CCd-nJ*~qxg&!_LmllVAVlOgHiZOKOw_$u!i=0HIO>I%Q;jmBGI)_jpf&aclbzi=YX@7ff4 zsD4bA7koyv>AL3+hMG>LV2jelNx#fHAPuD;2i=Vn1pCq3;a73CthQobO(}^?zrf=Z z-Uzz_W+g@bLuIooa?7Xo035mhnA$rD-+RkpJjw`0?23g4@ZtL9T3A44gZZ0c_`t8m z^X#|_Z)*743_g!rIR9zK^)aBE@xL9%81laz$Hdcr-j0LLx>7|&D);ee;aa#ve9o1- zD-~y1Z%Mh-(1A&Mc*>VMQ-#!S;mfANAfsmt9vR)2dq@6~>!X~(Bcs32;7{cgDT}iV zetl3fII;LE46c>`dhHl?!Z?rk&ig#a|MO51m|bN2%5k?pF6=;-JlFW!00000NkvXX Hu0mjfcR5Y# literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..7226c7aa44 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
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 +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 31
<unnamed>68.2 %404 / 59261.3 %19 / 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..cb879ede94 --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
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 +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 31
<unnamed>68.2 %404 / 59261.3 %19 / 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..2b38552eea --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
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 +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 31
<unnamed>68.2 %404 / 59261.3 %19 / 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..a39296b13a --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
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 +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 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..20ed637931 --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
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 +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 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..c56de30ebd --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
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 +
68.2%68.2%
+ 		68.2 %404 / 59261.3 %19 / 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..31834ced77 --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
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::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)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::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> 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> >&)1
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)1
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)2
(anonymous namespace)::ProxyExec0::ProxyExec0()3
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()3
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>)3
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)7
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)9
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)11
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)13
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()81
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()81
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()146
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()224
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()1094
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()1318
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)1460
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
+
+
+ + + +
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..8bbee10ad0 --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()3
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()3
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>)3
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)7
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> >&)1
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> >&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)13
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()1318
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)1
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()1094
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()224
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()81
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)11
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()81
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)9
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> 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::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)2
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()146
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)1460
+
+
+ + + +
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..5458f09029 --- /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:40459268.2 %
Date:2023-11-30 22:31:42Functions:193161.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_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           3 : 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           3 :   this->common_handlers_  = common_handlers;
+     215           3 :   this->private_handlers_ = private_handlers;
+     216             : 
+     217           3 :   _uav_name_ = common_handlers->uav_name;
+     218             : 
+     219           3 :   nh_ = nh;
+     220             : 
+     221           3 :   ros::Time::waitForValid();
+     222             : 
+     223             :   // --------------------------------------------------------------
+     224             :   // |                     loading parameters                     |
+     225             :   // --------------------------------------------------------------
+     226             : 
+     227             :   // | ---------- loading params using the parent's nh ---------- |
+     228             : 
+     229           6 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     230             : 
+     231           3 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     232             : 
+     233           3 :   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           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/landoff_tracker.yaml");
+     241           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/landoff_tracker.yaml");
+     242             : 
+     243           6 :   const std::string yaml_prefix = "mrs_uav_trackers/landoff_tracker/";
+     244             : 
+     245           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_speed", _horizontal_speed_);
+     246           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_acceleration", _horizontal_acceleration_);
+     247             : 
+     248           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     249           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_acceleration", _vertical_acceleration_);
+     250             : 
+     251           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_speed", _takeoff_speed_);
+     252           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_acceleration", _takeoff_acceleration_);
+     253             : 
+     254           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_speed", _landing_speed_);
+     255           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_acceleration", _landing_acceleration_);
+     256             : 
+     257           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_speed", _elanding_speed_);
+     258           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_acceleration", _elanding_acceleration_);
+     259             : 
+     260           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     261           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_gain", _heading_gain_);
+     262             : 
+     263           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "main_timer_rate", _main_timer_rate_);
+     264             : 
+     265           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "landing_reference", _landing_reference_);
+     266             : 
+     267           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_position_difference", _max_position_difference_);
+     268             : 
+     269           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains", _takeoff_disable_lateral_gains_);
+     270           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains_z", _takeoff_disable_lateral_gains_z_);
+     271             : 
+     272           3 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     273           0 :     ROS_ERROR("[LandoffTracker]: Could not load all parameters!");
+     274           0 :     return false;
+     275             :   }
+     276             : 
+     277           3 :   _tracker_dt_ = 1.0 / double(_main_timer_rate_);
+     278             : 
+     279           3 :   ROS_INFO("[LandoffTracker]: tracker_dt: %f", _tracker_dt_);
+     280             : 
+     281           3 :   state_x_       = 0;
+     282           3 :   state_y_       = 0;
+     283           3 :   state_z_       = 0;
+     284           3 :   state_heading_ = 0;
+     285             : 
+     286           3 :   speed_x_       = 0;
+     287           3 :   speed_y_       = 0;
+     288           3 :   speed_heading_ = 0;
+     289             : 
+     290           3 :   current_horizontal_speed_ = 0;
+     291           3 :   current_vertical_speed_   = 0;
+     292             : 
+     293           3 :   current_horizontal_acceleration_ = 0;
+     294           3 :   current_vertical_acceleration_   = 0;
+     295             : 
+     296           3 :   current_vertical_direction_ = 0;
+     297             : 
+     298           3 :   current_state_vertical_  = LANDED_STATE;
+     299           3 :   previous_state_vertical_ = LANDED_STATE;
+     300             : 
+     301           3 :   current_state_horizontal_  = LANDED_STATE;
+     302           3 :   previous_state_horizontal_ = LANDED_STATE;
+     303             : 
+     304             :   // | ------------------------ profiler ------------------------ |
+     305             : 
+     306           3 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "LandoffTracker", _profiler_enabled_);
+     307             : 
+     308             :   // | ------------------------ services ------------------------ |
+     309             : 
+     310           3 :   service_takeoff_ = nh_.advertiseService("takeoff", &LandoffTracker::callbackTakeoff, this);
+     311           3 :   service_land_    = nh_.advertiseService("land", &LandoffTracker::callbackLand, this);
+     312           3 :   service_eland_   = nh_.advertiseService("eland", &LandoffTracker::callbackELand, this);
+     313             : 
+     314             :   // | ------------------------- timers ------------------------- |
+     315             : 
+     316           3 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &LandoffTracker::timerMain, this, false, false);
+     317             : 
+     318             :   // | ----------------------- finish init ---------------------- |
+     319             : 
+     320           3 :   is_initialized_ = true;
+     321             : 
+     322           3 :   ROS_INFO("[LandoffTracker]: initialized");
+     323             : 
+     324           3 :   return true;
+     325             : }
+     326             : 
+     327             : //}
+     328             : 
+     329             : /* //{ activate() */
+     330             : 
+     331           2 : std::tuple<bool, std::string> LandoffTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     332             : 
+     333           4 :   std::stringstream ss;
+     334             : 
+     335           2 :   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           4 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     344             : 
+     345             :   double uav_heading;
+     346             : 
+     347             :   try {
+     348           2 :     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           4 :     std::scoped_lock lock(mutex_goal_);
+     363             : 
+     364             :     // the last command is usable
+     365           2 :     state_x_       = uav_state.pose.position.x;
+     366           2 :     state_y_       = uav_state.pose.position.y;
+     367           2 :     state_z_       = uav_state.pose.position.z;
+     368           2 :     state_heading_ = uav_heading;
+     369             : 
+     370           2 :     speed_x_         = uav_state.velocity.linear.x;
+     371           2 :     speed_y_         = uav_state.velocity.linear.y;
+     372           2 :     current_heading_ = atan2(speed_y_, speed_x_);
+     373             : 
+     374           2 :     current_horizontal_speed_ = sqrt(pow(speed_x_, 2) + pow(speed_y_, 2));
+     375             : 
+     376           2 :     current_vertical_speed_     = fabs(uav_state.velocity.linear.z);
+     377           2 :     current_vertical_direction_ = uav_state.velocity.linear.z > 0 ? +1 : -1;
+     378             : 
+     379           2 :     current_horizontal_acceleration_ = 0;
+     380           2 :     current_vertical_acceleration_   = 0;
+     381             : 
+     382           2 :     goal_heading_ = uav_heading;
+     383             : 
+     384           2 :     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           2 :     std::scoped_lock lock(mutex_state_);
+     395             : 
+     396           2 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     397           2 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     398           2 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     399           2 :     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           2 :     std::scoped_lock lock(mutex_state_);
+     410             : 
+     411           2 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     412           2 :     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           2 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     421             : 
+     422           2 :     goal_x_ = state_x_ + stop_dist_x;
+     423           2 :     goal_y_ = state_y_ + stop_dist_y;
+     424           2 :     goal_z_ = state_z_ + vertical_stop_dist;
+     425             :   }
+     426             : 
+     427           2 :   landing_        = false;
+     428           2 :   taking_off_     = false;
+     429           2 :   is_active_      = true;
+     430           2 :   have_goal_      = false;
+     431           2 :   cause_failsafe_ = false;
+     432             : 
+     433           2 :   timer_main_.start();
+     434             : 
+     435             :   {
+     436           4 :     std::scoped_lock lock(mutex_goal_);
+     437             : 
+     438           2 :     ROS_INFO("[LandoffTracker]: stopping goal: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     439             :   }
+     440             : 
+     441           2 :   changeState(STOP_MOTION_STATE);
+     442             : 
+     443           2 :   ss << "activated";
+     444           2 :   ROS_INFO_STREAM("[LandoffTracker]: " << ss.str());
+     445             : 
+     446           2 :   return std::tuple(true, ss.str());
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : /* //{ deactivate() */
+     452             : 
+     453           3 : void LandoffTracker::deactivate(void) {
+     454             : 
+     455           3 :   is_active_                = false;
+     456           3 :   landing_                  = false;
+     457           3 :   taking_off_               = false;
+     458           3 :   current_state_vertical_   = IDLE_STATE;
+     459           3 :   current_state_horizontal_ = IDLE_STATE;
+     460             : 
+     461           3 :   timer_main_.stop();
+     462             : 
+     463           3 :   ROS_INFO("[LandoffTracker]: deactivated");
+     464           3 : }
+     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        5071 : 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       15213 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     483       15213 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     484             : 
+     485             :   {
+     486        5071 :     std::scoped_lock lock(mutex_uav_state_);
+     487             : 
+     488        5071 :     uav_state_ = uav_state;
+     489             : 
+     490        5071 :     got_uav_state_ = true;
+     491             :   }
+     492             : 
+     493             :   // up to this part the update() method is evaluated even when the tracker is not active
+     494        5071 :   if (!is_active_ || cause_failsafe_) {
+     495        3651 :     return {};
+     496             :   }
+     497             : 
+     498        1420 :   position_output_.header.stamp    = ros::Time::now();
+     499        1420 :   position_output_.header.frame_id = uav_state_.header.frame_id;
+     500             : 
+     501             :   {
+     502        1420 :     std::scoped_lock lock(mutex_state_);
+     503             : 
+     504        1420 :     position_output_.position.x = state_x_;
+     505        1420 :     position_output_.position.y = state_y_;
+     506        1420 :     position_output_.position.z = state_z_;
+     507        1420 :     position_output_.heading    = state_heading_;
+     508             : 
+     509        1420 :     position_output_.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     510        1420 :     position_output_.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     511        1420 :     position_output_.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     512        1420 :     position_output_.heading_rate = speed_heading_;
+     513             : 
+     514        1420 :     position_output_.use_position_vertical   = 1;
+     515        1420 :     position_output_.use_position_horizontal = 1;
+     516        1420 :     position_output_.use_heading             = 1;
+     517        1420 :     position_output_.use_heading_rate        = 1;
+     518        1420 :     position_output_.use_velocity_vertical   = 1;
+     519        1420 :     position_output_.use_velocity_horizontal = 1;
+     520             :   }
+     521             : 
+     522             :   {
+     523        2840 :     std::scoped_lock lock(mutex_last_control_output_);
+     524             : 
+     525        1420 :     last_control_output_ = last_control_output;
+     526             :   }
+     527             : 
+     528        1420 :   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        1420 :     position_output_.disable_position_gains = false;
+     532             :   }
+     533             : 
+     534        1420 :   if (taking_off_) {
+     535         446 :     position_output_.disable_antiwindups = true;
+     536             :   } else {
+     537         974 :     position_output_.disable_antiwindups = false;
+     538             :   }
+     539             : 
+     540        1420 :   return {position_output_};
+     541             : }
+     542             : 
+     543             : //}
+     544             : 
+     545             : /* //{ getStatus() */
+     546             : 
+     547         146 : const mrs_msgs::TrackerStatus LandoffTracker::getStatus() {
+     548             : 
+     549         146 :   mrs_msgs::TrackerStatus tracker_status;
+     550             : 
+     551         146 :   tracker_status.active            = is_active_;
+     552         146 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     553             : 
+     554         146 :   bool hovering = current_state_vertical_ == HOVER_STATE && current_state_horizontal_ == HOVER_STATE;
+     555         146 :   bool idling   = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     556             : 
+     557         146 :   tracker_status.have_goal = landing_ || taking_off_ || !(hovering || idling);
+     558             : 
+     559         146 :   tracker_status.tracking_trajectory = false;
+     560             : 
+     561         146 :   return tracker_status;
+     562             : }
+     563             : 
+     564             : //}
+     565             : 
+     566             : /* //{ enableCallbacks() */
+     567             : 
+     568           1 : const std_srvs::SetBoolResponse::ConstPtr LandoffTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+     569             : 
+     570           2 :   std_srvs::SetBoolResponse res;
+     571           2 :   std::stringstream         ss;
+     572             : 
+     573           1 :   if (cmd->data != callbacks_enabled_) {
+     574             : 
+     575           0 :     callbacks_enabled_ = cmd->data;
+     576             : 
+     577           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     578           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     579             : 
+     580             :   } else {
+     581             : 
+     582           1 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     583           1 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     584             :   }
+     585             : 
+     586           1 :   res.message = ss.str();
+     587           1 :   res.success = true;
+     588             : 
+     589           2 :   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          13 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LandoffTracker::setConstraints([
+     755             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd) {
+     756             : 
+     757             : 
+     758          13 :   mrs_lib::set_mutexed(mutex_constraints_, cmd->constraints, constraints_);
+     759             : 
+     760          13 :   ROS_INFO("[LandoffTracker]: updating constraints");
+     761             : 
+     762          26 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     763          13 :   res.success = true;
+     764          13 :   res.message = "constraints updated";
+     765             : 
+     766          26 :   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           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LandoffTracker::setTrajectoryReference([
+     792             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+     793           0 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     794             : }
+     795             : 
+     796             : //}
+     797             : 
+     798             : // | ----------------- state machine routines ----------------- |
+     799             : 
+     800             : /* //{ changeStateHorizontal() */
+     801             : 
+     802           9 : void LandoffTracker::changeStateHorizontal(States_t new_state) {
+     803             : 
+     804           9 :   previous_state_horizontal_ = current_state_horizontal_;
+     805           9 :   current_state_horizontal_  = new_state;
+     806             : 
+     807           9 :   switch (current_state_horizontal_) {
+     808             : 
+     809           3 :     case STOPPING_STATE: {
+     810             : 
+     811           6 :       std::scoped_lock lock(mutex_state_);
+     812           3 :       current_horizontal_speed_ = 0;
+     813             : 
+     814           3 :       break;
+     815             :     };
+     816             : 
+     817           6 :     default: {
+     818             : 
+     819           6 :       break;
+     820             :     }
+     821             :   }
+     822             : 
+     823           9 :   ROS_INFO("[LandoffTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+     824           9 : }
+     825             : 
+     826             : //}
+     827             : 
+     828             : /* //{ changeStateVertical() */
+     829             : 
+     830          11 : void LandoffTracker::changeStateVertical(States_t new_state) {
+     831             : 
+     832          11 :   previous_state_vertical_ = current_state_vertical_;
+     833          11 :   current_state_vertical_  = new_state;
+     834             : 
+     835          11 :   switch (current_state_vertical_) {
+     836             : 
+     837           2 :     case HOVER_STATE: {
+     838           2 :       taking_off_ = false;
+     839           2 :       break;
+     840             :     }
+     841             : 
+     842           9 :     default: {
+     843           9 :       break;
+     844             :     }
+     845             :   }
+     846             : 
+     847          11 :   ROS_INFO("[LandoffTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+     848          11 : }
+     849             : 
+     850             : //}
+     851             : 
+     852             : /* //{ changeState() */
+     853             : 
+     854           7 : void LandoffTracker::changeState(States_t new_state) {
+     855             : 
+     856           7 :   changeStateVertical(new_state);
+     857           7 :   changeStateHorizontal(new_state);
+     858           7 : }
+     859             : 
+     860             : //}
+     861             : 
+     862             : // | --------------------- motion routines -------------------- |
+     863             : 
+     864             : /* //{ stopHorizontalMotion() */
+     865             : 
+     866          81 : void LandoffTracker::stopHorizontalMotion(void) {
+     867             : 
+     868             :   {
+     869         162 :     std::scoped_lock lock(mutex_state_);
+     870             : 
+     871          81 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     872             : 
+     873          81 :     if (current_horizontal_speed_ < 0) {
+     874           1 :       current_horizontal_speed_        = 0;
+     875           1 :       current_horizontal_acceleration_ = 0;
+     876             :     } else {
+     877          80 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     878             :     }
+     879             :   }
+     880          81 : }
+     881             : 
+     882             : //}
+     883             : 
+     884             : /* //{ stopVerticalMotion() */
+     885             : 
+     886          81 : void LandoffTracker::stopVerticalMotion(void) {
+     887             : 
+     888             :   {
+     889         162 :     std::scoped_lock lock(mutex_state_);
+     890             : 
+     891          81 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     892             : 
+     893          81 :     if (current_vertical_speed_ < 0) {
+     894          50 :       current_vertical_speed_        = 0;
+     895          50 :       current_vertical_acceleration_ = 0;
+     896             :     } else {
+     897          31 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     898             :     }
+     899             :   }
+     900          81 : }
+     901             : 
+     902             : //}
+     903             : 
+     904             : /* //{ accelerateVertical() */
+     905             : 
+     906        1094 : void LandoffTracker::accelerateVertical(void) {
+     907             : 
+     908             :   // copy member variables
+     909        1094 :   auto [current_vertical_speed, state_z] = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_, state_z_);
+     910        1094 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     911        1094 :   auto constraints                       = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     912             : 
+     913             :   double used_acceleration;
+     914             :   double used_speed;
+     915             : 
+     916        1094 :   if (taking_off_) {
+     917             : 
+     918         224 :     used_speed        = _takeoff_speed_;
+     919         224 :     used_acceleration = _takeoff_acceleration_;
+     920             : 
+     921         224 :     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         224 :     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         870 :   } else if (landing_) {
+     932             : 
+     933         870 :     if (elanding_) {
+     934             : 
+     935           0 :       used_speed        = _elanding_speed_;
+     936           0 :       used_acceleration = _elanding_acceleration_;
+     937             : 
+     938             :     } else {
+     939             : 
+     940         870 :       used_speed        = _landing_speed_;
+     941         870 :       used_acceleration = _landing_acceleration_;
+     942             : 
+     943         870 :       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         870 :       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        1094 :   double tar_z = goal_z - state_z;
+     963             : 
+     964             :   // set the right vertical direction
+     965             :   {
+     966        1094 :     std::scoped_lock lock(mutex_state_);
+     967             : 
+     968        1094 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+     969             :   }
+     970             : 
+     971        1094 :   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        1094 :   double vertical_t_stop    = current_vertical_speed / used_acceleration;
+     975        1094 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+     976        1094 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+     977             : 
+     978             :   {
+     979        2188 :     std::scoped_lock lock(mutex_state_);
+     980             : 
+     981        1094 :     current_vertical_speed_ += used_acceleration * _tracker_dt_;
+     982             : 
+     983        1094 :     if (current_vertical_speed_ >= used_speed) {
+     984         106 :       current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     985         106 :       current_vertical_acceleration_ = 0;
+     986             :     } else {
+     987         988 :       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        1094 :   if (!elanding_ && !landing_) {
+     997         224 :     if (fabs(state_z + stop_dist_z - goal_z) < (2 * (used_speed * _tracker_dt_))) {
+     998             : 
+     999             :       {
+    1000           1 :         std::scoped_lock lock(mutex_state_);
+    1001             : 
+    1002           1 :         current_vertical_acceleration_ = 0;
+    1003             :       }
+    1004             : 
+    1005           1 :       changeStateVertical(DECELERATING_STATE);
+    1006             :     }
+    1007             :   }
+    1008        1094 : }
+    1009             : 
+    1010             : //}
+    1011             : 
+    1012             : /* //{ decelerateVertical() */
+    1013             : 
+    1014         224 : void LandoffTracker::decelerateVertical(void) {
+    1015             : 
+    1016             :   double used_acceleration;
+    1017             : 
+    1018         224 :   if (taking_off_) {
+    1019             : 
+    1020         224 :     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         448 :     std::scoped_lock lock(mutex_state_);
+    1039             : 
+    1040         224 :     current_vertical_speed_ -= used_acceleration * _tracker_dt_;
+    1041             : 
+    1042         224 :     if (current_vertical_speed_ < 0) {
+    1043           1 :       current_vertical_speed_ = 0;
+    1044             :     } else {
+    1045         223 :       current_vertical_acceleration_ = -used_acceleration;
+    1046             :     }
+    1047             :   }
+    1048             : 
+    1049         224 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1050             : 
+    1051         224 :   if (current_vertical_speed == 0) {
+    1052             : 
+    1053             :     {
+    1054           1 :       std::scoped_lock lock(mutex_state_);
+    1055             : 
+    1056           1 :       current_vertical_acceleration_ = 0;
+    1057             :     }
+    1058             : 
+    1059           1 :     changeStateVertical(STOPPING_STATE);
+    1060             :   }
+    1061         224 : }
+    1062             : 
+    1063             : //}
+    1064             : 
+    1065             : /* //{ stopHorizontal() */
+    1066             : 
+    1067        1318 : void LandoffTracker::stopHorizontal(void) {
+    1068             : 
+    1069             :   {
+    1070        1318 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1071             : 
+    1072        1318 :     double new_state_x = 0.95 * state_x_ + 0.05 * goal_x_;
+    1073        1318 :     double new_state_y = 0.95 * state_y_ + 0.05 * goal_y_;
+    1074             : 
+    1075        1318 :     double dist_x = new_state_x - state_x_;
+    1076        1318 :     double dist_y = new_state_y - state_y_;
+    1077             : 
+    1078        1318 :     double dt = 1.0 / _main_timer_rate_;
+    1079             : 
+    1080        1318 :     if (std::abs(dist_x / dt) > 1.0) {
+    1081           0 :       dist_x = mrs_lib::signum(dist_x) * (1.0 * dt);
+    1082             :     }
+    1083             : 
+    1084        1318 :     if (std::abs(dist_y / dt) > 1.0) {
+    1085           0 :       dist_y = mrs_lib::signum(dist_y) * (1.0 * dt);
+    1086             :     }
+    1087             : 
+    1088        1318 :     state_x_ += dist_x;
+    1089        1318 :     state_y_ += dist_y;
+    1090             : 
+    1091        1318 :     current_horizontal_acceleration_ = 0;
+    1092             :   }
+    1093        1318 : }
+    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        1460 : void LandoffTracker::timerMain(const ros::TimerEvent& event) {
+    1127             : 
+    1128        1460 :   std::scoped_lock lock(mutex_main_timer_);
+    1129             : 
+    1130        1460 :   if (!is_active_) {
+    1131           0 :     return;
+    1132             :   }
+    1133             : 
+    1134             :   // copy member variables
+    1135        2920 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1136        1460 :   auto [state_x, state_y, state_z, current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] = mrs_lib::get_mutexed(
+    1137        1460 :       mutex_state_, state_x_, state_y_, state_z_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+    1138        1460 :   auto [goal_x, goal_y, goal_z] = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1139        2920 :   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        1460 :   uav_x = uav_state.pose.position.x;
+    1143        1460 :   uav_y = uav_state.pose.position.y;
+    1144        1460 :   uav_z = uav_state.pose.position.z;
+    1145             : 
+    1146        4380 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _main_timer_rate_, 0.002, event);
+    1147        4380 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1148             : 
+    1149        1460 :   bool takeoff_saturated = false;
+    1150             : 
+    1151        1460 :   if (taking_off_) {
+    1152             : 
+    1153             :     // calculate the vector
+    1154         467 :     double err_x      = uav_x - state_x;
+    1155         467 :     double err_y      = uav_y - state_y;
+    1156         467 :     double err_z      = uav_z - state_z;
+    1157         467 :     double error_size = sqrt(pow(err_x, 2) + pow(err_y, 2) + pow(err_z, 2));
+    1158             : 
+    1159         467 :     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         467 :     if (last_control_output.diagnostics.ramping_up) {
+    1181             : 
+    1182          19 :       ROS_INFO_THROTTLE(1.0, "[LandoffTracker]: waiting for the controller to rampup");
+    1183          19 :       takeoff_saturated = true;
+    1184             :     }
+    1185             :   }
+    1186             : 
+    1187        1460 :   if (!takeoff_saturated) {
+    1188             : 
+    1189        1441 :     switch (current_state_horizontal_) {
+    1190             : 
+    1191          81 :       case STOP_MOTION_STATE: {
+    1192             : 
+    1193          81 :         stopHorizontalMotion();
+    1194          81 :         break;
+    1195             :       }
+    1196             : 
+    1197        1318 :       case STOPPING_STATE: {
+    1198             : 
+    1199        1318 :         stopHorizontal();
+    1200        1318 :         break;
+    1201             :       }
+    1202             : 
+    1203          42 :       default: {
+    1204             : 
+    1205          42 :         break;
+    1206             :       }
+    1207             :     }
+    1208             : 
+    1209        1441 :     switch (current_state_vertical_) {
+    1210             : 
+    1211          81 :       case STOP_MOTION_STATE: {
+    1212             : 
+    1213          81 :         stopVerticalMotion();
+    1214          81 :         break;
+    1215             :       }
+    1216             : 
+    1217        1094 :       case ACCELERATING_STATE: {
+    1218             : 
+    1219        1094 :         accelerateVertical();
+    1220        1094 :         break;
+    1221             :       }
+    1222             : 
+    1223         224 :       case DECELERATING_STATE: {
+    1224             : 
+    1225         224 :         decelerateVertical();
+    1226         224 :         break;
+    1227             :       }
+    1228             : 
+    1229           0 :       case STOPPING_STATE: {
+    1230             : 
+    1231           0 :         stopVertical();
+    1232           0 :         break;
+    1233             :       }
+    1234             : 
+    1235          42 :       default: {
+    1236             : 
+    1237          42 :         break;
+    1238             :       }
+    1239             :     }
+    1240             :   }
+    1241             : 
+    1242        1460 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1243          82 :     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           3 :       if (have_goal_) {
+    1250             : 
+    1251           2 :         changeStateVertical(ACCELERATING_STATE);
+    1252           2 :         changeStateHorizontal(STOPPING_STATE);
+    1253             : 
+    1254             :       } else {
+    1255             : 
+    1256           1 :         changeState(STOPPING_STATE);
+    1257             : 
+    1258             :         {
+    1259           1 :           std::scoped_lock lock(mutex_state_);
+    1260             : 
+    1261           1 :           current_horizontal_speed_ = 0;
+    1262           1 :           current_vertical_speed_   = 0;
+    1263             :         }
+    1264             :       }
+    1265             :     }
+    1266             :   }
+    1267             : 
+    1268        1460 :   if (current_state_vertical_ == STOPPING_STATE && current_state_horizontal_ == STOPPING_STATE) {
+    1269             : 
+    1270           2 :     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           2 :     } 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           2 :         std::scoped_lock lock(mutex_state_);
+    1283             : 
+    1284           2 :         if (!taking_off_) {
+    1285           1 :           state_x_ = goal_x;
+    1286           1 :           state_y_ = goal_y;
+    1287           1 :           state_z_ = goal_z;
+    1288             :         }
+    1289             : 
+    1290           2 :         current_horizontal_speed_ = 0;
+    1291           2 :         current_vertical_speed_   = 0;
+    1292             :       }
+    1293             : 
+    1294           2 :       changeState(HOVER_STATE);
+    1295             : 
+    1296           2 :       have_goal_ = false;
+    1297             :     }
+    1298             :   }
+    1299             : 
+    1300        1460 :   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        1460 :   if (!takeoff_saturated) {
+    1318             :     {
+    1319        1441 :       std::scoped_lock lock(mutex_state_);
+    1320             : 
+    1321        1441 :       state_x_ += cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1322        1441 :       state_y_ += sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1323        1441 :       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        2920 :     std::scoped_lock lock(mutex_state_);
+    1334             : 
+    1335             :     double current_heading_rate;
+    1336             : 
+    1337        1460 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1338           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1339             :     else
+    1340        1460 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1341             : 
+    1342        1460 :     if (current_heading_rate > _heading_rate_) {
+    1343           0 :       current_heading_rate = _heading_rate_;
+    1344        1460 :     } 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        1460 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1350             : 
+    1351        1460 :     if (state_heading_ > M_PI) {
+    1352           0 :       state_heading_ -= 2 * M_PI;
+    1353        1460 :     } else if (state_heading_ < -M_PI) {
+    1354           0 :       state_heading_ += 2 * M_PI;
+    1355             :     }
+    1356             : 
+    1357        1460 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1358        1460 :       state_heading_ = goal_heading_;
+    1359             :     }
+    1360             :   }
+    1361             : 
+    1362             :   // --------------------------------------------------------------
+    1363             :   // |                      landing setpoint                      |
+    1364             :   // --------------------------------------------------------------
+    1365             : 
+    1366        1460 :   if (landing_) {
+    1367             :     {
+    1368         950 :       std::scoped_lock lock(mutex_goal_);
+    1369             : 
+    1370         950 :       goal_z_ = uav_z + _landing_reference_;
+    1371             :     }
+    1372             :   }
+    1373             : }
+    1374             : 
+    1375             : //}
+    1376             : 
+    1377             : // | ------------------------ callbacks ----------------------- |
+    1378             : 
+    1379             : /* //{ callbackTakeoff() */
+    1380             : 
+    1381           1 : bool LandoffTracker::callbackTakeoff(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    1382             : 
+    1383           2 :   std::stringstream ss;
+    1384             : 
+    1385             :   // copy member variables
+    1386           2 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1387             : 
+    1388           1 :   double uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1389             : 
+    1390             :   double uav_x, uav_y, uav_z;
+    1391           1 :   uav_x = uav_state.pose.position.x;
+    1392           1 :   uav_y = uav_state.pose.position.y;
+    1393           1 :   uav_z = uav_state.pose.position.z;
+    1394             : 
+    1395           1 :   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           1 :   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           1 :   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           2 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+    1422             : 
+    1423           1 :     state_x_ = uav_x;
+    1424           1 :     goal_x_  = uav_x;
+    1425             : 
+    1426           1 :     state_y_ = uav_y;
+    1427           1 :     goal_y_  = uav_y;
+    1428             : 
+    1429           1 :     state_z_ = uav_z;
+    1430           1 :     goal_z_  = uav_z + req.goal;
+    1431             : 
+    1432           1 :     state_heading_ = uav_heading;
+    1433           1 :     goal_heading_  = uav_heading;
+    1434             : 
+    1435           1 :     speed_x_                = 0;
+    1436           1 :     speed_y_                = 0;
+    1437           1 :     current_vertical_speed_ = 0;
+    1438             : 
+    1439           1 :     have_goal_ = true;
+    1440             :   }
+    1441             : 
+    1442           1 :   ROS_INFO("[LandoffTracker]: taking off");
+    1443             : 
+    1444           1 :   taking_off_ = true;
+    1445           1 :   landing_    = false;
+    1446           1 :   elanding_   = false;
+    1447             : 
+    1448           1 :   res.success = true;
+    1449           1 :   res.message = "taking off";
+    1450             : 
+    1451           1 :   changeState(STOP_MOTION_STATE);
+    1452             : 
+    1453           1 :   return true;
+    1454             : }
+    1455             : 
+    1456             : //}
+    1457             : 
+    1458             : /* //{ callbackLand() */
+    1459             : 
+    1460           1 : bool LandoffTracker::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1461             : 
+    1462           2 :   std::scoped_lock lock(mutex_main_timer_);
+    1463             : 
+    1464           2 :   std::stringstream ss;
+    1465             : 
+    1466             :   // copy member variables
+    1467           2 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1468             : 
+    1469           1 :   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           1 :     std::scoped_lock lock(mutex_goal_);
+    1479             : 
+    1480           1 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1481             :   }
+    1482             : 
+    1483           1 :   ROS_INFO("[LandoffTracker]: landing");
+    1484             : 
+    1485           1 :   landing_    = true;
+    1486           1 :   elanding_   = false;
+    1487           1 :   taking_off_ = false;
+    1488           1 :   have_goal_  = true;
+    1489             : 
+    1490           1 :   res.success = true;
+    1491           1 :   res.message = "landing";
+    1492             : 
+    1493           1 :   changeState(STOP_MOTION_STATE);
+    1494             : 
+    1495           1 :   return true;
+    1496             : }
+    1497             : 
+    1498             : //}
+    1499             : 
+    1500             : /* //{ callbackELand() */
+    1501             : 
+    1502           0 : bool LandoffTracker::callbackELand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1503             : 
+    1504           0 :   std::scoped_lock lock(mutex_main_timer_);
+    1505             : 
+    1506           0 :   std::stringstream ss;
+    1507             : 
+    1508             :   // copy member variables
+    1509           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1510             : 
+    1511           0 :   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           0 :     std::scoped_lock lock(mutex_goal_);
+    1526             : 
+    1527           0 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1528             :   }
+    1529             : 
+    1530           0 :   ROS_WARN("[LandoffTracker]: emergency landing");
+    1531             : 
+    1532           0 :   landing_    = true;
+    1533           0 :   elanding_   = true;
+    1534           0 :   taking_off_ = false;
+    1535           0 :   have_goal_  = true;
+    1536             : 
+    1537           0 :   res.success = true;
+    1538           0 :   res.message = "elanding";
+    1539             : 
+    1540           0 :   changeState(STOP_MOTION_STATE);
+    1541             : 
+    1542           0 :   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           3 : 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..0568abc72efbc6afc8d35f6964346f6546d54f4f GIT binary patch literal 4833 zcmV<75+3b|P)qz4wE;;L<#dr}lsEooyb`PJ|O zK}EC|%`seeW%{9+p2{PZ4HQ;APeq-y9n-Jw0X<{Yr!-!=*Nm#pQXf$Dq%~64-D3u^ z^vXcRG=pM)t{-guV{0ng)UpjSdz?st{p30KCHF9v!j6S$>O|9!bi>;y`~=?#+qhVR z=80Z4%oxZOj&F-2H^mOl0>6+gd}bV_2~|%xR{>SWGC`39HW(Lz!}$?5bXs87fTmHk z2mOY42QQ1jMXWs}MN7ePGbgIHVB{rdfIN4a)y6sIog*IgmIa)+zzfL$&Zdp6J>wc@ z$Vv!u>jxFga~GRRasgEw0rOuMHpf@S0nUQym|JzGm`*7JXMXSLs;7n5bBZdOdrWQN zcL5+z7E$UVNbIZ6a8_0y$m?+5Erm7Q->g?UC*Y0_Yw^r#d$ha>OZ91v9vj#_f&U83Jc6aA5=I z1W>?C7?Mg5jADSPijm%S=s@6EdX^bL-7O5@oB>w0G22QuMOJ7p0kmA@KM7Q zxAd}B;kq!8O+y=t(Hi$h&r;)R1v3-OTg?J@ibZe6A!hwoLY%k>#U>8e)0_am$OhDk z(FVV}WmG($+Y^`FzS!WO9&1pZiNdcMl2~1>VLj`LfV%_k1dv$_#SO6@)00trw~Qju z*v7P26#$tO*~JW+d2wcbGV}XXH)wzyLsdtg*Y-?E$19VB zBHgqt1ioHhs2taS$JhD(r^`zh4SfIgeC5mIRYF*xxC<8_V4C3+($t07A^-~dT3a0R zbB~Fw(!Nd>L|p*;fgMB383dzYy?qYL+cz)YTo@`ktsO z_ZvR$@5Vx-k86yxj`!yFtyaJMKm(AddJNE8fYitkm}XmbKDk;7j7TX}Q>d;v2?{aZj&x{sRV#gaI0&Fn4{*lC(^&ka?xxq%EQKS0d_Ekkd1Ys2NH6M|o0F zK)Rv}^?1+vDJ?)+(FXLjb9?rLYP+f}13aLnm=C%DAcLZjP!`B6FpN_Gaw&e$vnRY1 z@0z$5Lo7w;!*y`jRdb6SLu~_Y=9~3Z`#zy4IpCclaYHlMx@^1Hx9P#{jT8&$w6F-Y zO1meOH`)+O1DOR-9I|Pb3re#UmtFNh#qD-U4G2iNz*Hg}#*9fZH|<{!W{^t|Jo_XguQZT|mng|Gt?soAdc4< zw!#4M6lj1uv^JHiVZ}&LvHsPPnM#hq+E7y#c&owHZG2IQUc?e@uT6^0?F~E7^)|hj z5@&}&Z6ZLJKC)I7Z3%N;%g49b=Z4Q@XdFX!*cogEu zLlPzbD^@^Z^ZZN~sL z_N&ia=zWR<{yyBRvyP&1`M=V+`v92dyTB{Fczfa0Nm5krV~U6E>jBP3 z?__t!%lSgXhIADye+P}a-Z|5JF$O3?q&*GD26FrQ4vFM&p25n4<&P(%pgSD&{rN!g z*mraI6oVgdIE}CrC)66pHCsH2q89XV5R`fVb+5<3LF>ZPIZ>(kV~{5WON#FQ?VGvM zzFWa};wU*%;<=$5JA_^TwO_!wH;=%YUUyt8k&S$buJp zV?uuGnKQbLMADb8hOuAhfUbE7prnnN`|(__-eqHP>N*lglR_DfX5TEbGB@RiL=+covw9G|2GHwiU$ zvjUj1k~hGR_ZGJ2KL$tV6F@e_)l+BAOp3E(x29n1brKM zRj+(zc<#r5$9@xTq0LjfGS)WhBHiqqX0J6@%Ju}Nr`Gm_H*BPU8Pn5ZdlJfCV|(ah zZjhoi5yk5gdXE`ykr;s6j_Y7p-)&s?Ix9hq>u^lVxaMCw5!IUj-lJ!r!HupiZuTF0 ztho|4JhE6fJjS{Q_T1l>Eux4W;5~g7AXHoOdb|**TSfzUht6n<=&h&7mTITt zQE%&y-O)x^V&L}l6th6tRr)YlDcdy`*r8L5c-W_z3z6-?=T4id-iYBG zG3b&Isr*M2e|(`_w;+GUX7Ha1$ct$TdIlKM)QvYAWacY<_CvtTmkRiHXJ+^3c?(@A z?&~6uNzsbu;>^i0k&jCYoB(p=*!x)J${dDpF3b)?S!^#o4874;;z7@mA)^u{8WneHw?_fx)h!k+w7VrUq15W?i*>>wQV>;iYdC5JO% zMRotco*03$!G)?0pM-$Q4_Q=X%*vR9+Tn^lEb8wvfiM2}+H7t#Q+J%%kCZm!OCn&s z;M(yuj5YfPhWAVF0C=?|y0P4K)~#2X;e*WTSDGn*AY5rCSGnK3(!@jIbMf#8uq8TB zX-~0KKV9C8j>%jFIJ=9+BZ}AGXS2|}-pmF(+y9J(b!`O0&p;=*2z0JT!Px}bvjJyi zvk&orx4QAlRXg`tL%iW7a=BQVdjVN`#_i|_q2V+)f3cJw982MbAGl>$RN>h#SfK8* zGfsEbE#!HR=31UIJsHKHztT*#k$5Uu@2CQsmrt%0ezVKP4esY2(c2?tA5i2mGbFQM zLtKZ&{8fA#nf-c7-V$TaWqTE*x-pp%xYt}}xIIHSDRdjTOVXP;AKea!5Xm74N0Y~O z)00@tuga=dzreoM+dFzu2^3b1RvXC+vUxyJ)o((;J|%#BivCJ>MK&=jXjcJ6d*5%) z=+1_*yD2}pjZd?3Eu{>yf&4CHn&#hWAN!peincPy>(e9`ZZfMLbj<&NLjb&JqKIzM zYP#}9Q+yKwj(q~irwHzhIFmP?f6`LC>|Iucy9jD8e^IP7J_(wA#)pY}eM62WI^oln zquKH5^3dgS6WOyd&pw@EFQ&+h_vm{*I7)Gc=h|DK{R7T8b+7!wXD5DuFSvWy?f=mV za>qc|n$#I=aS(8(9fwMw3L=)cO*!Y+(60k1+QUMo(haZdKaP>&|@W$ z;fLOU(-YyW=f-Jq?`^ih3wTIjIlymG)epDBzSup;8C)c)HwPb}k^b>w;JQ61a1`?G zaI+Gin7>K;rJ>jhKg_G+u&?c`v|yhD?8TVh7mDjljN2S``>iv%NfaKHNDZ&rXd#OE z?CPKTcz;4*_9BhXMuxYPM5@K}3_Zif=TxllxOAqK)!Wi}iiwFJ0*=_fW*ic)<2wJa zi1o8vGd5ntT(f{&W_l09c+9llFfew^q$Txn?b!-KT>rRcwDHGtjeMC*VndZDRSsU| zoh=U>koNt1?Bh^(=81C)P<9UC7H)fZG*_R#;>z)XRsi<;c7HFiNg$joZ~}>hE?v+* zh+_8Hg)ZxHa!#)E!6yg=6p0i`Egr{q`R)fE*Vm6HQMkZ{svq4`hT=A6xaPKzr{pWF zJCoKR>lw~fKN0YHh+-1Ru>fqQ2jJL+&e4ue=S!Xqh!c0;KdjBc?**BF;pT@rO{5;t zqkpOw)Uyk&&*5|Q;LOFEkUYg+kKGE|m?X)kxPrrbIvBTm8xG#zCsW!gVR0euwCdY| zTm!%yg)8qmvUtUw0gg>1@3WM0cA>Bk{}F*9U>Ac!D#}4(w(&(I0+3f6OHImQH}1a| zC~70)!E)@)_(8GMOo^W=VLed0acMpt_*4n&Lpzr@Q3Mf9rsA%n5>DZo1Yav>Z}4jY z2H4#?A0Ix?B~ml}&MbN0XS0x^tc`Z@rdC)9;OElk@wu2DS8kxyuv40C3tNhvIJfI} z!0kR&R6YgEnyqkl-JAmywx!As$J|6fni;ZrSe<%Qot1K5?i6($kTNJ? zCVh#HFaK?VA3x#rN|QcJOz(7&z3>?jaRRgpp`GyQx{_yf18*%eExN4u;-uL>DHi*1 zGC7j5&;MJFF8G{s&?G_WP~eAoV)tqP`=15gvMwauL+M$mQc%$XcNXwhR;_2QURC?4 zFq2YkdbSJ{Rc+rUkFkq^rlb$X%f%Ci5hCD->$V^_dTceGBr$SfJrGSFd|&`~4sc8x z%@$cPzei9t5P27hCCyR5|MM|$$z@8AdRx(j9Zz4m0r(phJ{6w5_L2_ckkanVpM#X> zLSAr2Iw9c&9~q|kmIU55sJ#*QtX7cW7{Y%V@A=UBvH$TN?VppG^;`kKxtmCpNohP(g)`5ONvBR + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32347468.1 %
Date:2023-11-30 22:31:42Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..9ecf5dc52c --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32347468.1 %
Date:2023-11-30 22:31:42Functions:193063.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::line_tracker::LineTracker::deactivate()0
mrs_uav_trackers::line_tracker::LineTracker::resetStatic()0
mrs_uav_trackers::line_tracker::LineTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::line_tracker::LineTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_trackers::line_tracker::LineTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)1
mrs_uav_trackers::line_tracker::LineTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::line_tracker::LineTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)1
mrs_uav_trackers::line_tracker::LineTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)3
mrs_uav_trackers::line_tracker::LineTracker::changeState(mrs_uav_trackers::line_tracker::States_t)6
mrs_uav_trackers::line_tracker::LineTracker::changeStateVertical(mrs_uav_trackers::line_tracker::States_t)8
mrs_uav_trackers::line_tracker::LineTracker::changeStateHorizontal(mrs_uav_trackers::line_tracker::States_t)8
mrs_uav_trackers::line_tracker::LineTracker::stopVerticalMotion()29
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontalMotion()29
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontal()52
mrs_uav_trackers::line_tracker::LineTracker::decelerateVertical()100
mrs_uav_trackers::line_tracker::LineTracker::decelerateHorizontal()100
mrs_uav_trackers::line_tracker::LineTracker::getStatus()209
mrs_uav_trackers::line_tracker::LineTracker::accelerateVertical()354
mrs_uav_trackers::line_tracker::LineTracker::stopVertical()1496
mrs_uav_trackers::line_tracker::LineTracker::accelerateHorizontal()1798
mrs_uav_trackers::line_tracker::LineTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)1833
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2617
+
+
+ + + +
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..5cbbeea96c --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32347468.1 %
Date:2023-11-30 22:31:42Functions:193063.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4b5ba87fa1f1ac4a2c3f41a0b8736549da563101 GIT binary patch literal 3773 zcmV;u4npyXP)QIT0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vph4P2>B& zTdVS>cMzRYBt0+*&1P-O*py;~8Up6kQ6O7EQ45SwRTV~)J=uH8KK8!)@mpYX^T|@Gg*&A(r#>5x8vQRudPC+tgks|$T|+}z42@_4JM_fS?;Ig4wQ-!tFgsA7 zmaeBW`rp9_KEXeTmMm#dJCN1^6bV)PMJS;FtQejc3CA0$=y=v~L>Tbblp+K=3iM?X zXlsxqOi0mE0MdxS0}T2n3Y-wIwvL2iiP69^f}c@g#_+8QIB0=8QUM%I9rZ$M(Vjs@ zn`$UVim6^2IOa)xgH|~<86ZIGHyUmZfw-}N(g%Q6tWnguPx7(X%4g&LHZ{R_-wnQd zSh%C3n(8X+s0!jb0M+Gezcbv7_QIrfgC3Y-?S1+X#CtKp)SnlbuFa=_Ax} z6Dfw3rNB`OoYcUv)@_7&A%S3!S))4M@5>#P#r zLz&Tlc9gVsVKiLbGn9^3rW}fV)fNhTJYJw|`~UAB&-Z`l2M<$D;1hfzl-nbPu)ypt zoLm%qdj0aLz1ZNSWB@bz`t&&F=SoSf7MMX%Du{Xp@Q-x^)=M_U_xOQ_5IO)9)^V{v zZS9IFzF#a$d{pWMJlZD(RF>=15h#`lxXv+P5$lLl0Z?kEQr5EAWfWIqUuo7+UFdmP^cl@&s;-?P&I#Y_6<2B4x5w#b zll|kg$2jAseZgzbhm=CwA+xjBg}lJ+mSL{IUe`$DXQ-L+d}m@PvIk+Fb=j1IHD?NJ z9kswU;h#?6-jAmvK?6LeW_qAXfAq>Dwj+`6!_D&pQgbzSQU`+59Kc@3ml>Jni{7)`Bw@Xaq6; zjxL1%EJa~;(N`l%@eU>>IiZ5iqv(qw*Nu4FuwJ|ah7Ebd7p3JAG}+*n6>F5rgs?NLj-2Bwo*iZnG}s6$4q5A z{vEG3)0sErpz!E)$}cNhczo#BuO~M-?zw)APY02gB@^X}SzK*C+~;v-9I4=UH|W9_ zF!95_e{sNLP=hq!Ysgvw)K2X)x-e7C>=HLv<{oj+byc&mf($5Mpk_qN(3=(MBMqG0 z+w}}SKCcuvdo#ef?+fmr88a!W%SZD)*4|B5@I6oi zvW)9#4VD%-0n}5}FWPC>;!MwM;rtsHeS_z$#Rcu*c1w6Id3|(Ktk#iuVV>lLw|678 zvW`NgNm#1v0DPr#S0%!nWs77!vVab(0Un~nfVy%Eb5|R2L7>6Jj ziXqR8#+_?wJ%#?(_+^ z(4MISm_P{f6q|>lnq=fiTkj%+!kl|+P}BPqZ-y|PQY5B5a_ZVad__;$Q;g4a);9~JfW9vG&VrTYM1c7H;PvCzj9Px$-! zB2$7~iY6j`$!BQ)BGLrK(<3!}43sV+-MY+%)eZi^Ljx6iK72D4NiCkU0Xn>Gbf&ol zV*^L=crSK*m(lfOXDXPUVRf>kXEw5Z2ERi+{0d<|JEfL-pV$g;x! z>r6#QH6vPv-fUdTE_{3F+&3tNeK*oa)^t>g`h9!MF@Jp*p7x$P@Lmk$6|HYt@hPhJ zqgxOMOM^{_uiqCIx>6q>>Jyb*J95o)!FFtr@%?MZHVygLj%{Xr|9v|)+89gk*hB00 z^*akZsbGyh?`BSu!yMcuNcL*Ey1lNAHht~y!$xI*cWI07TF9E`Y`-B}w8VbTx=zM6 zBi+qRft~h|rf&Ll`nbX?TqI$Xb%gs;xc0LzF19bF)mD4Kr35#bmAe$in;2jQMY~lx z+=mZz_-D2P*KUBG4UPE4O5K2W!JB|>cQq&D*OSl!{?EFC2^>R60Q+usmpXdR-({EU z_9CLUaC^A(FXr}IxjsJrJc^J^-Q+*nzY?eRFM4?QVxeotDJj?Dss=x|NyrV3TKpmn z?)RJeMI@I@u6g2DvuT{88Ei7ZVk&G#G8t+LqRDVJBLbB+!%>9qAVr1DIbC=;oB7sp zCiBrZ>ph4jNG`=aBj-+UE1^Jj9lyb@?R}dz#$D01mwhsE3+4^m(#x0A*R6K~N;e$S zn<~&_goPW9_jfJ{xYrj@gvix2xEiQ#o^MdIvvWIyT;T13flSE7h}9wFVky$f%w2|$ z3=LmM-Hu=OK^pE?hMH+BIV<%nM=A7Os1-)rj_e{kw?3nFq?d~TzBFn!H#MLS!vSZ( z13Pc8hbd$}z|@`piAil1auzr4ioz7J9%BRFw~PwpG7SXm8GMAifs}a-KKavqqs=i6 z=0>o=9u~oOXh6O=#K5b2zJl};dpE-dh?zC7&Qz`W~`Nz*rrPwkz9j~`utklf=n))mwR`$y3|7W+rh zJqGha@g7#zQ}DoF>M5EXISKP2by9!aQ^1e*NwR%Zm`|_d0@DilTh*-HF@)g6L8a8+ zq-MPG~{hp^5l#CJ$drQ z!6i(G-!l&)x;6GYcU8UlK3roLR)}kkO4RzE*+oc6pqk>Gq?St&rjXlEWNwvjKv4(O zQcSt|e2RZkf4|hND9&k-3n*%UJs)xq^Ll@7FU?wG3Ev}idk63*btOd|P)o6&+e;l| z&7-IRc5r*r=CB2AGGNw*5n%sdwRb<*XJe&Dg*zz{P?auZD9S)pOWySJ=AQe_P|uK* zF6xTYJMDDm9v_n6bCkV%(Y~h!t6x(aiCTyv|WhRFy zR1uV#;;Esn6V)Z9@Oi)hI zq9M-ZQNp`7@Ch+@Hq5^8SIZu!_f;pvX6Pv_Yo}(eE_v4*Eg{Kp6y;g-K75r-20!PF zZ=Ue1T*d)9eeq|OT~3}$%lq4()=+xVJOvF7y3V}>o4#mAOB7rC@b + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 18
<unnamed>70.7 %65 / 9244.4 %8 / 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..0b09ab441d --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 18
<unnamed>70.7 %65 / 9244.4 %8 / 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..91f67991eb --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 18
<unnamed>70.7 %65 / 9244.4 %8 / 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..632b38d53e --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 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..aa81a95587 --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 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..d0cf538a2f --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
70.7%70.7%
+ 		70.7 %65 / 9244.4 %8 / 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..9c755abe9d --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
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::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> 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::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()1
(anonymous namespace)::ProxyExec0::ProxyExec0()3
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>)3
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)4
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()5
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)13
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
+
+
+ + + +
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..af6a0dca09 --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()3
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()5
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>)3
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&)13
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
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&)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::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)4
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()1
+
+
+ + + +
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..a906d78b9e --- /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:659270.7 %
Date:2023-11-30 22:31:42Functions:81844.4 %
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           3 : 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           3 :   this->common_handlers_  = common_handlers;
+      81           3 :   this->private_handlers_ = private_handlers;
+      82             : 
+      83           3 :   _uav_name_ = common_handlers->uav_name;
+      84             : 
+      85           3 :   nh_ = nh;
+      86             : 
+      87           3 :   ros::Time::waitForValid();
+      88             : 
+      89             :   // --------------------------------------------------------------
+      90             :   // |                     loading parameters                     |
+      91             :   // --------------------------------------------------------------
+      92             : 
+      93             :   // | ---------- loading params using the parent's nh ---------- |
+      94             : 
+      95           6 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+      96             : 
+      97           3 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+      98             : 
+      99           3 :   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           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/midair_activation_tracker.yaml");
+     107           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/midair_activation_tracker.yaml");
+     108             : 
+     109           6 :   const std::string yaml_prefix = "mrs_uav_trackers/midair_activation_tracker/";
+     110             : 
+     111           3 :   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           3 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationTracker", _profiler_enabled_);
+     119             : 
+     120             :   // | --------------------- finish the init -------------------- |
+     121             : 
+     122           3 :   is_initialized_ = true;
+     123             : 
+     124           3 :   ROS_INFO("[MidairActivationTracker]: initialized");
+     125             : 
+     126           3 :   return true;
+     127             : }
+     128             : 
+     129             : //}
+     130             : 
+     131             : /* //{ activate() */
+     132             : 
+     133           4 : std::tuple<bool, std::string> MidairActivationTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     134             : 
+     135           4 :   std::stringstream ss;
+     136             : 
+     137           4 :   is_active_ = true;
+     138             : 
+     139           4 :   ss << "activated";
+     140           4 :   ROS_INFO_STREAM("[MidairActivationTracker]: " << ss.str());
+     141             : 
+     142           8 :   return std::tuple(true, ss.str());
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* //{ deactivate() */
+     148             : 
+     149           5 : void MidairActivationTracker::deactivate(void) {
+     150             : 
+     151           5 :   is_active_ = false;
+     152             : 
+     153           5 :   ROS_INFO("[MidairActivationTracker]: deactivated");
+     154           5 : }
+     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        5071 : 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        5071 :   if (!is_active_) {
+     174        5060 :     return {};
+     175             :   }
+     176             : 
+     177          33 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     178             :   mrs_lib::ScopeTimer timer =
+     179          33 :       mrs_lib::ScopeTimer("MidairActivationTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     180             : 
+     181          22 :   mrs_msgs::TrackerCommand tracker_cmd;
+     182             : 
+     183          11 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     184          11 :   tracker_cmd.header.stamp    = ros::Time::now();
+     185             : 
+     186          11 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     187          11 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     188          11 :   tracker_cmd.position.z = uav_state.pose.position.z;
+     189             : 
+     190          11 :   tracker_cmd.velocity.x = uav_state.velocity.linear.x;
+     191          11 :   tracker_cmd.velocity.y = uav_state.velocity.linear.y;
+     192          11 :   tracker_cmd.velocity.z = uav_state.velocity.linear.z;
+     193             : 
+     194             :   try {
+     195          11 :     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          11 :   tracker_cmd.use_position_vertical   = true;
+     203          11 :   tracker_cmd.use_position_horizontal = true;
+     204             : 
+     205          11 :   tracker_cmd.use_velocity_vertical   = true;
+     206          11 :   tracker_cmd.use_velocity_horizontal = true;
+     207             : 
+     208          11 :   tracker_cmd.use_heading = true;
+     209             : 
+     210          11 :   ROS_WARN_THROTTLE(0.1, "[MidairActivationTracker]: outputting cmd");
+     211             : 
+     212          11 :   return {tracker_cmd};
+     213             : }
+     214             : 
+     215             : //}
+     216             : 
+     217             : /* //{ getStatus() */
+     218             : 
+     219           1 : const mrs_msgs::TrackerStatus MidairActivationTracker::getStatus() {
+     220             : 
+     221           1 :   mrs_msgs::TrackerStatus tracker_status;
+     222             : 
+     223           1 :   tracker_status.active            = is_active_;
+     224           1 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     225             : 
+     226           1 :   return tracker_status;
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* //{ enableCallbacks() */
+     232             : 
+     233           1 : const std_srvs::SetBoolResponse::ConstPtr MidairActivationTracker::enableCallbacks([[maybe_unused]] const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     234             : 
+     235           2 :   std_srvs::SetBoolResponse res;
+     236             : 
+     237           1 :   res.message = "callbacks are always disabled";
+     238           1 :   res.success = true;
+     239             : 
+     240           2 :   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          13 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationTracker::setConstraints([
+     298             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     299             : 
+     300          26 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     301             : 
+     302          13 :   res.success = true;
+     303          13 :   res.message = "constraints updated";
+     304             : 
+     305          26 :   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           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MidairActivationTracker::setTrajectoryReference([
+     331             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     332           0 :   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           3 : 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..3e904aa6caa3f3aac1073aba0929c9b60f4bb71a GIT binary patch literal 1156 zcmV-~1bh35P)uv-3tej>J+KOGR&9rzDzP990o!cMh)sc9iQLKMZ5*X7m3T)P!@lX7vFef+A5bl{ zV?y7EK_z%9QpT{f~mmvM`C_<6bl3O_-Lp4~9)F-+%kbEwkVFoy`B#0TxB{EbR z8^Lkq+aejkjX_WXy^wTI@U7cj(=d*u^ zvAf^OyjeD4P>=FeZ^2WWa_^FXfXV?C0b~x4xFh>hd2JFe%P0|pHs)z*m$(ouAWnKd z&b*t%>Jy9|0gx$?Jz&vD_<#4>Wxaq559 z%g^;=E*yse{@m}2V>xOF3v71b>UJF_>${61E-a1)V1uts#(`HgL1HcpvB%%$>$tp) z_&P^(aJeb%1ttmT(8Q>XU}h=j52X}{5)}bkB<`iUVS$XIWq|DQL?;E=e_GpzShpDnuC(BP6zEl9@tM>I#6-;yuIgXNSq)-CTtmW;{>zZ&*lr%ow zd#>GDM62$Q7o9wLhc42J)WfsJF{LNN#VZ+1$@zV zZ(6GV5?+%i@P~2Z%<~EVhk2ZkD!x3Cc-y+3M0$qsgmiyo zcjg~C?;|^!@Pzai68= + + + + + + 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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 49
<unnamed>51.9 %869 / 167351.0 %25 / 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..b57034ecbc --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 49
<unnamed>51.9 %869 / 167351.0 %25 / 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..2851bb727e --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 49
<unnamed>51.9 %869 / 167351.0 %25 / 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..b7a5041e90 --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 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..41f67dc49a --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 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..18daf26b56 --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
51.9%51.9%
+ 		51.9 %869 / 167351.0 %25 / 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..26964135ba --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackWiggle(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollision(double, double, double, double, double, double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)0
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintMPCResult(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)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::timerTrajectoryTracking(ros::TimerEvent 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::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()2
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)2
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)2
(anonymous namespace)::ProxyExec0::ProxyExec0()3
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>)3
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)3
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)5
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)7
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)8
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)9
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)13
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()108
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)134
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)675
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()686
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()1047
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)1047
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)1047
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()1047
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)1047
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)1047
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)1054
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
+
+
+ + + +
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..2ed18e92df --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()3
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()2
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>)3
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)5
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)8
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()1047
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)1054
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)2
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> >)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)13
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&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)7
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)1047
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)675
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)1047
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()1047
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()686
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&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)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::setSinglePointReference(double, double, double, double)9
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&)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::timerAvoidanceTrajectory(ros::TimerEvent const&)134
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)3
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&)1047
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&)5071
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)2
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)1047
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()108
+
+
+ + + +
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..848debf3f1 --- /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:869167351.9 %
Date:2023-11-30 22:31:42Functions:254951.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : #include <mrs_uav_managers/controller.h>
+       8             : 
+       9             : #include <geometry_msgs/Pose.h>
+      10             : #include <geometry_msgs/PoseArray.h>
+      11             : 
+      12             : #include <mrs_msgs/FuturePoint.h>
+      13             : #include <mrs_msgs/FutureTrajectory.h>
+      14             : #include <mrs_msgs/MpcTrackerDiagnostics.h>
+      15             : #include <mrs_msgs/MpcPredictionFullState.h>
+      16             : #include <mrs_msgs/EstimationDiagnostics.h>
+      17             : #include <mrs_msgs/VelocityReference.h>
+      18             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      19             : 
+      20             : #include <std_msgs/String.h>
+      21             : 
+      22             : #include <mrs_lib/profiler.h>
+      23             : #include <mrs_lib/utils.h>
+      24             : #include <mrs_lib/mutex.h>
+      25             : #include <mrs_lib/attitude_converter.h>
+      26             : #include <mrs_lib/subscribe_handler.h>
+      27             : #include <mrs_lib/publisher_handler.h>
+      28             : #include <mrs_lib/geometry/cyclic.h>
+      29             : #include <mrs_lib/geometry/misc.h>
+      30             : #include <mrs_lib/scope_timer.h>
+      31             : 
+      32             : #include <mpc_tracker.h>
+      33             : 
+      34             : #include <dynamic_reconfigure/server.h>
+      35             : #include <mrs_uav_trackers/mpc_trackerConfig.h>
+      36             : 
+      37             : #include <visualization_msgs/Marker.h>
+      38             : #include <visualization_msgs/MarkerArray.h>
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* defines //{ */
+      43             : 
+      44             : 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           3 : 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           3 :   nh_ = nh;
+     412             : 
+     413           3 :   common_handlers_  = common_handlers;
+     414           3 :   private_handlers_ = private_handlers;
+     415             : 
+     416           3 :   _uav_name_ = common_handlers->uav_name;
+     417             : 
+     418           3 :   ros::Time::waitForValid();
+     419             : 
+     420             :   // --------------------------------------------------------------
+     421             :   // |                     loading parameters                     |
+     422             :   // --------------------------------------------------------------
+     423             : 
+     424             :   // | ---------- loading params using the parent's nh ---------- |
+     425             : 
+     426           6 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     427             : 
+     428           3 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     429             : 
+     430           3 :   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           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/mpc_tracker.yaml");
+     438           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/mpc_tracker.yaml");
+     439             : 
+     440           6 :   const std::string yaml_prefix = "mrs_uav_trackers/mpc_tracker/";
+     441             : 
+     442           3 :   private_handlers->param_loader->loadParam("network/robot_names", _avoidance_other_uav_names_);
+     443             : 
+     444           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/synchronous_rate_limit", _mpc_synchronous_rate_limit_);
+     445           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/asynchronous_loop_rate", _mpc_asynchronous_rate_);
+     446             : 
+     447           3 :   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           3 :   dt1_ = 1.0 / _mpc_asynchronous_rate_;
+     453             : 
+     454           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/enabled", drs_params_.braking_enabled);
+     455           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_braking", drs_params_.q_vel_braking);
+     456           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_no_braking", drs_params_.q_vel_no_braking);
+     457             : 
+     458           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/translation/n_states", _mpc_n_states_);
+     459           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/translation/n_inputs", _mpc_m_states_);
+     460           3 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/translation/A", _mat_A_, _mpc_n_states_, _mpc_n_states_);
+     461           3 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/translation/B", _mat_B_, _mpc_n_states_, _mpc_m_states_);
+     462             : 
+     463           3 :   A_ = _mat_A_;
+     464           3 :   B_ = _mat_B_;
+     465             : 
+     466           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/heading/n_states", _mpc_n_states_heading_);
+     467           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/heading/n_inputs", _mpc_n_inputs_heading_);
+     468           3 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/heading/A", _mat_A_heading_, _mpc_n_states_heading_, _mpc_n_states_heading_);
+     469           3 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/heading/B", _mat_B_heading_, _mpc_n_states_heading_, _mpc_n_inputs_heading_);
+     470             : 
+     471           3 :   A_heading_ = _mat_A_heading_;
+     472           3 :   B_heading_ = _mat_B_heading_;
+     473             : 
+     474             :   // load the MPC parameters
+     475           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/horizon_len", _mpc_horizon_len_);
+     476             : 
+     477           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/dt2", _dt2_);
+     478             : 
+     479           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_rate_);
+     480           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/position_tracking_threshold", _diag_pos_tracking_thr_);
+     481           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/orientation_tracking_threshold", _diag_heading_tracking_thr_);
+     482             : 
+     483           3 :   bool verbose_xy      = false;
+     484           3 :   bool verbose_z       = false;
+     485           3 :   bool verbose_heading = false;
+     486             : 
+     487           6 :   std::vector<double> xy_Q;
+     488           6 :   std::vector<double> z_Q;
+     489           6 :   std::vector<double> heading_Q;
+     490             : 
+     491           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/verbose", verbose_xy);
+     492           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/max_n_iterations", _max_iters_xy_);
+     493           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/Q", xy_Q);
+     494             : 
+     495           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/verbose", verbose_z);
+     496           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/max_n_iterations", _max_iters_z_);
+     497           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/Q", z_Q);
+     498             : 
+     499           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/verbose", verbose_heading);
+     500           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/max_n_iterations", _max_iters_heading_);
+     501           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/Q", heading_Q);
+     502             : 
+     503           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/enabled", drs_params_.wiggle_enabled);
+     504           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/amplitude", drs_params_.wiggle_amplitude);
+     505           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/frequency", drs_params_.wiggle_frequency);
+     506             : 
+     507           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled", collision_avoidance_enabled_);
+     508           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled_passively", collision_avoidance_enabled_passively_);
+     509           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/predicted_trajectory_publish_rate", _avoidance_trajectory_rate_);
+     510           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/correction", _avoidance_z_correction_);
+     511           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/radius", _avoidance_radius_threshold_);
+     512           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/altitude_threshold", _avoidance_z_threshold_);
+     513           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_horizontal_speed_coef", _avoidance_collision_horizontal_speed_coef_);
+     514           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_fully", _avoidance_collision_slow_down_fully_);
+     515           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_start", _avoidance_collision_slow_down_);
+     516           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_start_climbing", _avoidance_collision_start_climbing_);
+     517           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/trajectory_timeout", _collision_trajectory_timeout_);
+     518             : 
+     519           3 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     520           0 :     ROS_ERROR("[MpcTracker]: could not load all parameters!");
+     521           0 :     return false;
+     522             :   }
+     523             : 
+     524           3 :   ROS_INFO_STREAM("[MpcTracker]: initializing solvers with dt1 = " << dt1_);
+     525             : 
+     526           3 :   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           3 :   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           3 :   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           3 :       std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_hdg", verbose_heading, _max_iters_heading_, heading_Q, dt1_, _dt2_, 0);
+     531             : 
+     532           3 :   mpc_x_         = MatrixXd::Zero(_mpc_n_states_, 1);
+     533           3 :   mpc_x_heading_ = MatrixXd::Zero(_mpc_n_states_heading_, 1);
+     534             : 
+     535           3 :   mpc_u_ = VectorXd::Zero(_mpc_m_states_);
+     536             : 
+     537           3 :   coef_time = ros::Time(0);
+     538             : 
+     539           3 :   des_x_trajectory_       = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     540           3 :   des_y_trajectory_       = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     541           3 :   des_z_trajectory_       = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     542           3 :   des_z_filtered_offset_  = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     543           3 :   des_heading_trajectory_ = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     544             : 
+     545           3 :   service_server_wiggle_ = nh_.advertiseService("wiggle", &MpcTracker::callbackWiggle, this);
+     546             : 
+     547           3 :   pub_diagnostics_   = mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics>(nh_, "diagnostics", 1);
+     548           3 :   pub_status_string_ = mrs_lib::PublisherHandler<std_msgs::String>(nh_, "string", 1);
+     549             : 
+     550             :   // extract the numerical name
+     551           3 :   sscanf(_uav_name_.c_str(), "uav%d", &avoidance_this_uav_number_);
+     552           3 :   ROS_INFO("[MpcTracker]: Numerical ID of this UAV is %d", avoidance_this_uav_number_);
+     553           3 :   avoidance_this_uav_priority_ = avoidance_this_uav_number_;
+     554             : 
+     555             :   // exclude this drone from the list
+     556           3 :   std::vector<std::string>::iterator it = _avoidance_other_uav_names_.begin();
+     557           6 :   while (it != _avoidance_other_uav_names_.end()) {
+     558             : 
+     559           3 :     std::string temp_str = *it;
+     560             : 
+     561             :     int other_uav_priority;
+     562           3 :     sscanf(temp_str.c_str(), "uav%d", &other_uav_priority);
+     563             : 
+     564           3 :     if (other_uav_priority == avoidance_this_uav_number_) {
+     565             : 
+     566           3 :       _avoidance_other_uav_names_.erase(it);
+     567           3 :       continue;
+     568             :     }
+     569             : 
+     570           0 :     it++;
+     571             :   }
+     572             : 
+     573             :   // initialize velocity tracker
+     574             : 
+     575           3 :   velocity_reference_time_ = ros::Time(0);
+     576             : 
+     577             :   // create publishers for predicted trajectory
+     578             : 
+     579           3 :   ph_avoidance_trajectory_           = mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory>(nh_, "predicted_trajectory", 1);
+     580           3 :   ph_predicted_trajectory_debugging_ = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "predicted_trajectory_debugging", 1);
+     581           3 :   ph_mpc_reference_debugging_        = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "mpc_reference_debugging", 1, true);
+     582           3 :   ph_current_trajectory_point_       = mrs_lib::PublisherHandler<geometry_msgs::PoseStamped>(nh_, "current_trajectory_point", 1, true);
+     583             : 
+     584           3 :   pub_debug_processed_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_processed/poses", 1, true);
+     585           3 :   pub_debug_processed_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_processed/markers", 1, true);
+     586             : 
+     587             :   // preallocate predicted trajectory
+     588           3 :   predicted_trajectory_         = MatrixXd::Zero(_mpc_horizon_len_ * _mpc_n_states_, 1);
+     589           3 :   predicted_heading_trajectory_ = MatrixXd::Zero(_mpc_horizon_len_ * _mpc_n_states_, 1);
+     590             : 
+     591           3 :   collision_free_altitude_ = std::numeric_limits<float>::lowest();
+     592             : 
+     593             :   // collision avoidance toggle service
+     594           3 :   service_server_toggle_avoidance_ = nh_.advertiseService("collision_avoidance", &MpcTracker::callbackToggleCollisionAvoidance, this);
+     595             : 
+     596           6 :   mrs_lib::SubscribeHandlerOptions shopts;
+     597           3 :   shopts.nh                 = nh_;
+     598           3 :   shopts.node_name          = "MpcTracker";
+     599           3 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     600           3 :   shopts.threadsafe         = true;
+     601           3 :   shopts.autostart          = true;
+     602           3 :   shopts.queue_size         = 10;
+     603           3 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     604             : 
+     605             :   // create subscribers on other drones diagnostics
+     606           3 :   if (collision_avoidance_enabled_ || collision_avoidance_enabled_passively_) {
+     607             : 
+     608           3 :     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           3 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, std::string("/") + _uav_name_ + "/estimation_manager/diagnostics");
+     626             : 
+     627             :   // | --------------- dynamic reconfigure server --------------- |
+     628             : 
+     629           3 :   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, nh_));
+     630           3 :   reconfigure_server_->updateConfig(drs_params_);
+     631           3 :   ReconfigureServer::CallbackType f = boost::bind(&MpcTracker::dynamicReconfigureCallback, this, _1, _2);
+     632           3 :   reconfigure_server_->setCallback(f);
+     633             : 
+     634             :   // | ------------------------ profiler ------------------------ |
+     635             : 
+     636           3 :   profiler = mrs_lib::Profiler(common_handlers->parent_nh, "MpcTracker", _profiler_enabled_);
+     637             : 
+     638             :   // | ------------------------- timers ------------------------- |
+     639             : 
+     640           6 :   timer_avoidance_trajectory_ = nh_.createTimer(ros::Rate(_avoidance_trajectory_rate_), &MpcTracker::timerAvoidanceTrajectory, this, false,
+     641           6 :                                                 collision_avoidance_enabled_ || collision_avoidance_enabled_passively_);
+     642           3 :   timer_diagnostics_          = nh_.createTimer(ros::Rate(_diagnostics_rate_), &MpcTracker::timerDiagnostics, this);
+     643           3 :   timer_mpc_iteration_        = nh_.createTimer(ros::Rate(_mpc_asynchronous_rate_), &MpcTracker::timerMPC, this, false, false);
+     644           3 :   timer_trajectory_tracking_  = nh_.createTimer(ros::Rate(1.0), &MpcTracker::timerTrajectoryTracking, this, false, false);
+     645           3 :   timer_velocity_tracking_    = nh_.createTimer(ros::Rate(30.0), &MpcTracker::timerVelocityTracking, this, false, false);
+     646           3 :   timer_hover_                = nh_.createTimer(ros::Rate(10.0), &MpcTracker::timerHover, this, false, false);
+     647             : 
+     648             :   // | ----------------------- finish init ---------------------- |
+     649             : 
+     650           3 :   is_initialized_ = true;
+     651             : 
+     652           3 :   ROS_INFO("[MpcTracker]: initialized");
+     653             : 
+     654           3 :   return true;
+     655             : }
+     656             : 
+     657             : //}
+     658             : 
+     659             : /* //{ activate() */
+     660             : 
+     661           2 : std::tuple<bool, std::string> MpcTracker::activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     662             : 
+     663           4 :   std::stringstream ss;
+     664             : 
+     665           2 :   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           4 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     674             : 
+     675             :   double uav_state_heading;
+     676             : 
+     677             :   try {
+     678           2 :     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           4 :   MatrixXd mpc_x         = MatrixXd::Zero(_mpc_n_states_, 1);
+     687           2 :   MatrixXd mpc_x_heading = MatrixXd::Zero(_mpc_n_states_heading_, 1);
+     688             : 
+     689           2 :   if (last_tracker_cmd) {
+     690             : 
+     691             :     // set the initial condition from the last tracker's cmd
+     692             : 
+     693           2 :     if (last_tracker_cmd->use_position_horizontal) {
+     694           2 :       mpc_x(0, 0) = last_tracker_cmd->position.x;
+     695           2 :       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           2 :     if (last_tracker_cmd->use_position_vertical) {
+     702           2 :       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           2 :     if (last_tracker_cmd->use_velocity_horizontal) {
+     708           2 :       mpc_x(1, 0) = last_tracker_cmd->velocity.x;
+     709           2 :       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           2 :     if (last_tracker_cmd->use_velocity_vertical) {
+     716           2 :       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           2 :     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           2 :       mpc_x(2, 0)  = 0;
+     727           2 :       mpc_x(6, 0)  = 0;
+     728           2 :       mpc_x(10, 0) = 0;
+     729             :     }
+     730             : 
+     731             :     // the jerks
+     732           2 :     mpc_x(3, 0)  = 0;
+     733           2 :     mpc_x(7, 0)  = 0;
+     734           2 :     mpc_x(11, 0) = 0;
+     735             : 
+     736           2 :     if (last_tracker_cmd->use_heading) {
+     737           2 :       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           2 :     if (last_tracker_cmd->use_heading_rate) {
+     750           1 :       mpc_x_heading(1, 0) = last_tracker_cmd->heading_rate;
+     751             :     } else {
+     752           1 :       mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     753             :     }
+     754             : 
+     755           2 :     mpc_x_heading(2, 0) = 0;
+     756           2 :     mpc_x_heading(3, 0) = 0;
+     757             : 
+     758           2 :     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           4 :     std::scoped_lock lock(mutex_mpc_x_);
+     789             : 
+     790           2 :     mpc_x_         = mpc_x;
+     791           2 :     mpc_x_heading_ = mpc_x_heading;
+     792             :   }
+     793             : 
+     794           2 :   trajectory_tracking_in_progress_ = false;
+     795             : 
+     796           2 :   timer_trajectory_tracking_.stop();
+     797             : 
+     798           2 :   ss << "activated";
+     799           2 :   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           2 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     805             : 
+     806           2 :   toggleHover(true);
+     807             : 
+     808           2 :   model_first_iteration_ = true;
+     809             : 
+     810           2 :   A_ = _mat_A_;
+     811           2 :   B_ = _mat_B_;
+     812             : 
+     813           2 :   A_heading_ = _mat_A_heading_;
+     814           2 :   B_heading_ = _mat_B_heading_;
+     815             : 
+     816           2 :   is_active_ = true;
+     817             : 
+     818           2 :   if (!mpc_synchronous_) {
+     819           2 :     timer_mpc_iteration_.start();
+     820             :   }
+     821             : 
+     822           2 :   return std::tuple(true, ss.str());
+     823             : }
+     824             : 
+     825             : //}
+     826             : 
+     827             : /* //{ deactivate() */
+     828             : 
+     829           2 : void MpcTracker::deactivate(void) {
+     830             : 
+     831           2 :   toggleHover(false);
+     832             : 
+     833           2 :   is_active_                       = false;
+     834           2 :   trajectory_tracking_in_progress_ = false;
+     835           2 :   model_first_iteration_           = true;
+     836             : 
+     837           2 :   timer_trajectory_tracking_.stop();
+     838             : 
+     839             :   {
+     840           2 :     std::scoped_lock lock(mutex_trajectory_tracking_states_);
+     841             : 
+     842           2 :     trajectory_tracking_idx_     = 0;
+     843           2 :     trajectory_tracking_sub_idx_ = 0;
+     844             :   }
+     845             : 
+     846           2 :   ROS_INFO("[MpcTracker]: deactivated");
+     847             : 
+     848           2 :   timer_mpc_iteration_.stop();
+     849             : 
+     850           2 :   publishDiagnostics();
+     851           2 : }
+     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        5071 : 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       15213 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("update");
+     931       15213 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("MpcTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     932             : 
+     933       10142 :   auto old_uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     934             : 
+     935             :   // calculate dt
+     936        5071 :   double dt = (uav_state.header.stamp - old_uav_state.header.stamp).toSec();
+     937             : 
+     938             :   // save the uav state
+     939        5071 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     940             : 
+     941        5071 :   if (dt > 0) {
+     942             : 
+     943        5071 :     double rate = 1.0 / dt;
+     944             : 
+     945        5071 :     update_rate_ = 0.9 * update_rate_ + 0.1 * rate;
+     946             : 
+     947        5071 :     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        5071 :     } else if (!mpc_synchronous_ && (update_rate_ <= _mpc_synchronous_rate_limit_)) {
+     954           0 :       mpc_synchronous_ = true;
+     955           0 :       ROS_INFO("[MpcTracker]: detecting low update rate (%.1f Hz < %.1f Hz), switching to synchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     956           0 :       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        5071 :   if (!is_active_) {
+     962        4015 :     return {};
+     963             :   }
+     964             : 
+     965        2112 :   mrs_msgs::TrackerCommand tracker_cmd;
+     966             : 
+     967        1056 :   if (!mpc_synchronous_ && (!mpc_computed_ || mpc_result_invalid_)) {
+     968             : 
+     969           2 :     ROS_WARN_THROTTLE(0.1, "[MpcTracker]: MPC not ready, returning current odom as the command");
+     970             : 
+     971             :     // set the header
+     972           2 :     tracker_cmd.header.stamp    = uav_state.header.stamp;
+     973           2 :     tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     974             : 
+     975             :     // set positions from odom
+     976           2 :     tracker_cmd.position.x              = uav_state.pose.position.x;
+     977           2 :     tracker_cmd.position.y              = uav_state.pose.position.y;
+     978           2 :     tracker_cmd.position.z              = uav_state.pose.position.z;
+     979           2 :     tracker_cmd.use_position_vertical   = 1;
+     980           2 :     tracker_cmd.use_position_horizontal = 1;
+     981             : 
+     982             :     // set velocities from odom
+     983           2 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     984           2 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     985           2 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     986           2 :     tracker_cmd.use_velocity_vertical   = 1;
+     987           2 :     tracker_cmd.use_velocity_horizontal = 1;
+     988             : 
+     989             :     // set zero accelerations
+     990           2 :     tracker_cmd.acceleration.x   = 0;
+     991           2 :     tracker_cmd.acceleration.y   = 0;
+     992           2 :     tracker_cmd.acceleration.z   = 0;
+     993           2 :     tracker_cmd.use_acceleration = 1;
+     994             : 
+     995             :     try {
+     996           2 :       tracker_cmd.heading     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     997           2 :       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           2 :     tracker_cmd.jerk.x = 0;
+    1006           2 :     tracker_cmd.jerk.y = 0;
+    1007           2 :     tracker_cmd.jerk.z = 0;
+    1008             : 
+    1009             :     try {
+    1010           2 :       tracker_cmd.heading_rate     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeadingRate(uav_state.velocity.angular);
+    1011           2 :       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           2 :     return {tracker_cmd};
+    1019             :   }
+    1020             : 
+    1021        1054 :   ros::TimerEvent event;
+    1022             : 
+    1023        1054 :   if (mpc_synchronous_) {
+    1024           0 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in SYNCHRONOUS mode");
+    1025           0 :     timerMPC(event);
+    1026             :   } else {
+    1027        1054 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in ASYNCHRONOUS mode");
+    1028             :   }
+    1029             : 
+    1030        1054 :   if (dt > 0) {
+    1031        1054 :     iterateModel(dt);
+    1032             :   } else {
+    1033           0 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: dt !> 0, not iterating the model");
+    1034             :   }
+    1035             : 
+    1036        2108 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1037        2108 :   auto prediction_full_state  = mrs_lib::get_mutexed(mutex_prediction_full_state_, prediction_full_state_);
+    1038             : 
+    1039             :   // check whether all outputs are finite
+    1040        1054 :   bool arefinite = true;
+    1041       13702 :   for (int i = 0; i < 12; i++) {
+    1042       12648 :     if (!std::isfinite(mpc_x(i, 0))) {
+    1043           0 :       arefinite = false;
+    1044             :     }
+    1045             :   }
+    1046             : 
+    1047        1054 :   if (arefinite) {
+    1048             : 
+    1049             :     // set the desired states base on the result of the mpc
+    1050        1054 :     tracker_cmd.position.x     = mpc_x(0, 0);
+    1051        1054 :     tracker_cmd.velocity.x     = mpc_x(1, 0);
+    1052        1054 :     tracker_cmd.acceleration.x = mpc_x(2, 0);
+    1053        1054 :     tracker_cmd.jerk.x         = mpc_x(3, 0);
+    1054             : 
+    1055        1054 :     tracker_cmd.position.y     = mpc_x(4, 0);
+    1056        1054 :     tracker_cmd.velocity.y     = mpc_x(5, 0);
+    1057        1054 :     tracker_cmd.acceleration.y = mpc_x(6, 0);
+    1058        1054 :     tracker_cmd.jerk.y         = mpc_x(7, 0);
+    1059             : 
+    1060        1054 :     tracker_cmd.position.z     = mpc_x(8, 0);
+    1061        1054 :     tracker_cmd.velocity.z     = mpc_x(9, 0);
+    1062        1054 :     tracker_cmd.acceleration.z = mpc_x(10, 0);
+    1063        1054 :     tracker_cmd.jerk.z         = mpc_x(11, 0);
+    1064             : 
+    1065        1054 :     tracker_cmd.full_state_prediction = prediction_full_state;
+    1066             : 
+    1067        1054 :     tracker_cmd.use_position_vertical     = 1;
+    1068        1054 :     tracker_cmd.use_position_horizontal   = 1;
+    1069        1054 :     tracker_cmd.use_velocity_vertical     = 1;
+    1070        1054 :     tracker_cmd.use_velocity_horizontal   = 1;
+    1071        1054 :     tracker_cmd.use_acceleration          = 1;
+    1072        1054 :     tracker_cmd.use_jerk                  = 1;
+    1073        1054 :     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        1054 :   bool heading_finite = true;
+    1083        5270 :   for (int i = 0; i < _mpc_n_states_heading_; i++) {
+    1084        4216 :     if (!std::isfinite(mpc_x_heading(i, 0))) {
+    1085           0 :       heading_finite = false;
+    1086             :     }
+    1087             :   }
+    1088             : 
+    1089        1054 :   if (heading_finite) {
+    1090             : 
+    1091        1054 :     tracker_cmd.heading              = mpc_x_heading(0, 0);
+    1092        1054 :     tracker_cmd.heading_rate         = mpc_x_heading(1, 0);
+    1093        1054 :     tracker_cmd.heading_acceleration = mpc_x_heading(2, 0);
+    1094        1054 :     tracker_cmd.heading_jerk         = mpc_x_heading(3, 0);
+    1095             : 
+    1096        1054 :     tracker_cmd.use_heading              = 1;
+    1097        1054 :     tracker_cmd.use_heading_rate         = 1;
+    1098        1054 :     tracker_cmd.use_heading_acceleration = 1;
+    1099        1054 :     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        1054 :   tracker_cmd.header.stamp    = uav_state.header.stamp;
+    1110        1054 :   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        1054 :   return {tracker_cmd};
+    1115             : }  // namespace mpc_tracker
+    1116             : 
+    1117             : //}
+    1118             : 
+    1119             : /* //{ getStatus() */
+    1120             : 
+    1121         108 : const mrs_msgs::TrackerStatus MpcTracker::getStatus() {
+    1122             : 
+    1123         216 :   auto [mpc_x, mpc_x_heading]  = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1124         108 :   auto trajectory_size         = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    1125         108 :   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         108 :     std::scoped_lock lock(mutex_des_trajectory_);
+    1130             : 
+    1131         108 :     des_x       = des_x_trajectory_(0);
+    1132         108 :     des_y       = des_y_trajectory_(0);
+    1133         108 :     des_z       = des_z_trajectory_(0);
+    1134         108 :     des_heading = des_heading_trajectory_(0);
+    1135             :   }
+    1136             : 
+    1137         108 :   mrs_msgs::TrackerStatus tracker_status;
+    1138             : 
+    1139         108 :   tracker_status.active            = is_active_;
+    1140         108 :   tracker_status.callbacks_enabled = is_active_ && callbacks_enabled_ && !hovering_in_progress_;
+    1141             : 
+    1142         108 :   tracker_status.tracking_trajectory = trajectory_tracking_in_progress_;
+    1143             : 
+    1144         108 :   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         108 :   bool have_heading_error    = fabs(radians::diff(mpc_x_heading(0), des_heading)) > _diag_heading_tracking_thr_;
+    1146         108 :   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         108 :   tracker_status.have_goal = trajectory_tracking_in_progress_ || hovering_in_progress_ || have_position_error || have_heading_error || have_nonzero_velocity;
+    1149             : 
+    1150         108 :   tracker_status.trajectory_length = trajectory_size;
+    1151         108 :   tracker_status.trajectory_idx    = trajectory_tracking_idx;
+    1152             : 
+    1153         108 :   if (trajectory_tracking_in_progress_) {
+    1154             : 
+    1155           0 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1156             : 
+    1157           0 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    1158             : 
+    1159           0 :     tracker_status.trajectory_reference.header.stamp    = ros::Time::now();
+    1160           0 :     tracker_status.trajectory_reference.header.frame_id = uav_state.header.frame_id;
+    1161             : 
+    1162           0 :     tracker_status.trajectory_reference.reference.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1163           0 :     tracker_status.trajectory_reference.reference.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1164           0 :     tracker_status.trajectory_reference.reference.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1165           0 :     tracker_status.trajectory_reference.reference.heading    = (*des_heading_whole_trajectory_)(trajectory_tracking_idx);
+    1166             : 
+    1167             :     // | ---------- publish the current trajectory point ---------- |
+    1168             : 
+    1169           0 :     geometry_msgs::PoseStamped debug_trajectory_point;
+    1170           0 :     debug_trajectory_point.header.stamp    = ros::Time::now();
+    1171           0 :     debug_trajectory_point.header.frame_id = uav_state_.header.frame_id;
+    1172             : 
+    1173           0 :     debug_trajectory_point.pose.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1174           0 :     debug_trajectory_point.pose.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1175           0 :     debug_trajectory_point.pose.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1176             : 
+    1177           0 :     debug_trajectory_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(trajectory_tracking_idx));
+    1178             : 
+    1179           0 :     ph_current_trajectory_point_.publish(debug_trajectory_point);
+    1180             :   }
+    1181             : 
+    1182         216 :   return tracker_status;
+    1183             : }
+    1184             : 
+    1185             : //}
+    1186             : 
+    1187             : /* //{ enableCallbacks() */
+    1188             : 
+    1189           1 : const std_srvs::SetBoolResponse::ConstPtr MpcTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+    1190             : 
+    1191           2 :   std::stringstream ss;
+    1192             : 
+    1193           1 :   if (cmd->data != callbacks_enabled_) {
+    1194             : 
+    1195           0 :     callbacks_enabled_ = cmd->data;
+    1196           0 :     ss << "callbacks %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1197             : 
+    1198             :   } else {
+    1199             : 
+    1200           1 :     ss << "callbacks were already %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1201             :   }
+    1202             : 
+    1203           2 :   std_srvs::SetBoolResponse res;
+    1204           1 :   res.message = ss.str();
+    1205           1 :   res.success = true;
+    1206             : 
+    1207           2 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+    1208             : }
+    1209             : 
+    1210             : //}
+    1211             : 
+    1212             : /* switchOdometrySource() //{ */
+    1213             : 
+    1214           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::switchOdometrySource(const mrs_msgs::UavState& new_uav_state) {
+    1215             : 
+    1216           0 :   odometry_reset_in_progress_ = true;
+    1217           0 :   mpc_result_invalid_         = true;
+    1218             : 
+    1219           0 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1220           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1221             : 
+    1222           0 :   ROS_INFO(
+    1223             :       "[MpcTracker]: start of odmetry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1224             :       "%.2f], "
+    1225             :       "new odom [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: %.2f]",
+    1226             :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0), new_uav_state.pose.position.x, new_uav_state.pose.position.y,
+    1227             :       new_uav_state.pose.position.z, new_uav_state.velocity.linear.x, new_uav_state.velocity.linear.y, new_uav_state.velocity.linear.z,
+    1228             :       new_uav_state.acceleration.linear.x, new_uav_state.acceleration.linear.y, new_uav_state.acceleration.linear.z);
+    1229             : 
+    1230           0 :   timer_mpc_iteration_.stop();
+    1231           0 :   ROS_INFO("[MpcTracker]: mpc timer stopped");
+    1232             : 
+    1233           0 :   while (mpc_timer_running_) {
+    1234             : 
+    1235           0 :     ROS_DEBUG("[MpcTracker]: the mpc is in the middle of an iteration, waiting for it to finish");
+    1236           0 :     ros::Duration wait(0.001);
+    1237           0 :     wait.sleep();
+    1238             : 
+    1239           0 :     if (!mpc_timer_running_) {
+    1240           0 :       ROS_DEBUG("[ControlManager]: mpc timer finished");
+    1241           0 :       break;
+    1242             :     }
+    1243             :   }
+    1244             : 
+    1245             :   // | --------- recalculate the goal to new coordinates -------- |
+    1246             : 
+    1247           0 :   double old_heading  = 0;
+    1248           0 :   double new_heading  = 0;
+    1249           0 :   bool   got_headings = true;
+    1250             : 
+    1251             :   try {
+    1252           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1253             :   }
+    1254           0 :   catch (...) {
+    1255           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+    1256           0 :     got_headings = false;
+    1257             :   }
+    1258             : 
+    1259             :   try {
+    1260           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+    1261             :   }
+    1262           0 :   catch (...) {
+    1263           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+    1264           0 :     got_headings = false;
+    1265             :   }
+    1266             : 
+    1267           0 :   std_srvs::TriggerResponse res;
+    1268             : 
+    1269           0 :   if (!got_headings) {
+    1270           0 :     res.message = "could not calculate the heading difference";
+    1271           0 :     res.success = false;
+    1272             : 
+    1273           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1274             :   }
+    1275             : 
+    1276             :   // calculate the difference of position
+    1277           0 :   double dx       = new_uav_state.pose.position.x - uav_state_.pose.position.x;
+    1278           0 :   double dy       = new_uav_state.pose.position.y - uav_state_.pose.position.y;
+    1279           0 :   double dz       = new_uav_state.pose.position.z - uav_state_.pose.position.z;
+    1280           0 :   double dheading = new_heading - old_heading;
+    1281             : 
+    1282           0 :   ROS_INFO("[MpcTracker]: dx %f dy %f dz %f dheading %f", dx, dy, dz, dheading);
+    1283             : 
+    1284             :   {
+    1285           0 :     std::scoped_lock lock(mutex_mpc_x_, mutex_des_trajectory_, mutex_des_whole_trajectory_, mutex_uav_state_);
+    1286             : 
+    1287           0 :     if (trajectory_set_) {
+    1288             : 
+    1289           0 :       for (int i = 0; i < trajectory_size_ + _mpc_horizon_len_; i++) {
+    1290             : 
+    1291           0 :         Eigen::Vector2d temp_vec((*des_x_whole_trajectory_)(i)-uav_state_.pose.position.x, (*des_y_whole_trajectory_)(i)-uav_state_.pose.position.y);
+    1292           0 :         temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1293             : 
+    1294           0 :         (*des_x_whole_trajectory_)(i) = new_uav_state.pose.position.x + temp_vec[0];
+    1295           0 :         (*des_y_whole_trajectory_)(i) = new_uav_state.pose.position.y + temp_vec[1];
+    1296           0 :         (*des_z_whole_trajectory_)(i) += dz;
+    1297           0 :         (*des_heading_whole_trajectory_)(i) += dheading;
+    1298             :       }
+    1299             :     }
+    1300             : 
+    1301           0 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    1302             : 
+    1303           0 :       Eigen::Vector2d temp_vec(des_x_trajectory_(i) - uav_state_.pose.position.x, des_y_trajectory_(i) - uav_state_.pose.position.y);
+    1304           0 :       temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1305             : 
+    1306           0 :       des_x_trajectory_(i, 0) = new_uav_state.pose.position.x + temp_vec[0];
+    1307           0 :       des_y_trajectory_(i, 0) = new_uav_state.pose.position.y + temp_vec[1];
+    1308           0 :       des_z_trajectory_(i, 0) += dz;
+    1309           0 :       des_heading_trajectory_(i, 0) += dheading;
+    1310             :     }
+    1311             : 
+    1312             :     // update the position
+    1313             :     {
+    1314           0 :       Eigen::Vector2d temp_vec(mpc_x_(0, 0) - uav_state_.pose.position.x, mpc_x_(4, 0) - uav_state_.pose.position.y);
+    1315           0 :       temp_vec     = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1316           0 :       mpc_x_(0, 0) = new_uav_state.pose.position.x + temp_vec[0];
+    1317           0 :       mpc_x_(4, 0) = new_uav_state.pose.position.y + temp_vec[1];
+    1318           0 :       mpc_x_(8, 0) += dz;
+    1319             :     }
+    1320             : 
+    1321             :     // update the velocity
+    1322             :     {
+    1323           0 :       mpc_x_(1, 0) = new_uav_state.velocity.linear.x;
+    1324           0 :       mpc_x_(5, 0) = new_uav_state.velocity.linear.y;
+    1325             :       // we leave the z velocity as it was in the original frame
+    1326             :     }
+    1327             : 
+    1328             :     // update the acceleration
+    1329             :     {
+    1330           0 :       mpc_x_(2, 0)  = 0;
+    1331           0 :       mpc_x_(6, 0)  = 0;
+    1332           0 :       mpc_x_(10, 0) = 0;
+    1333             :     }
+    1334             : 
+    1335             :     // update the heading and its derivative
+    1336           0 :     mpc_x_heading_(0, 0) += dheading;
+    1337           0 :     mpc_x_heading_(1, 0) = new_uav_state.velocity.angular.x;
+    1338             :   }
+    1339             : 
+    1340           0 :   ROS_INFO(
+    1341             :       "[MpcTracker]: start of odmetry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1342             :       "%.2f]",
+    1343             :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0));
+    1344             : 
+    1345           0 :   ROS_INFO("[MpcTracker]: starting the MPC timer");
+    1346             : 
+    1347           0 :   if (!mpc_synchronous_) {
+    1348           0 :     timer_mpc_iteration_.start();
+    1349             :   }
+    1350             : 
+    1351           0 :   odometry_reset_in_progress_ = false;
+    1352             : 
+    1353           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1354             : }
+    1355             : 
+    1356             : //}
+    1357             : 
+    1358             : /* //{ hover() */
+    1359             : 
+    1360           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1361             : 
+    1362           0 :   toggleHover(true);
+    1363             : 
+    1364           0 :   std::stringstream ss;
+    1365           0 :   ss << "initiating hover";
+    1366             : 
+    1367           0 :   std_srvs::TriggerResponse res;
+    1368           0 :   res.success = true;
+    1369           0 :   res.message = ss.str();
+    1370             : 
+    1371           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1372             : }
+    1373             : 
+    1374             : //}
+    1375             : 
+    1376             : /* //{ startTrajectoryTracking() */
+    1377             : 
+    1378           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1379           0 :   std::stringstream ss;
+    1380             : 
+    1381           0 :   auto [success, message] = startTrajectoryTrackingImpl();
+    1382             : 
+    1383           0 :   std_srvs::TriggerResponse res;
+    1384           0 :   res.success = success;
+    1385           0 :   res.message = message;
+    1386             : 
+    1387           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1388             : }
+    1389             : 
+    1390             : //}
+    1391             : 
+    1392             : /* //{ stopTrajectoryTracking() */
+    1393             : 
+    1394           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1395             : 
+    1396           0 :   auto [success, message] = stopTrajectoryTrackingImpl();
+    1397             : 
+    1398           0 :   std_srvs::TriggerResponse res;
+    1399           0 :   res.success = success;
+    1400           0 :   res.message = message;
+    1401             : 
+    1402           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1403             : }
+    1404             : 
+    1405             : //}
+    1406             : 
+    1407             : /* //{ resumeTrajectoryTracking() */
+    1408             : 
+    1409           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1410             : 
+    1411           0 :   auto [success, message] = resumeTrajectoryTrackingImpl();
+    1412             : 
+    1413           0 :   std_srvs::TriggerResponse res;
+    1414           0 :   res.success = success;
+    1415           0 :   res.message = message;
+    1416             : 
+    1417           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1418             : }
+    1419             : 
+    1420             : //}
+    1421             : 
+    1422             : /* //{ gotoTrajectoryStart() */
+    1423             : 
+    1424           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1425             : 
+    1426           0 :   auto [success, message] = gotoTrajectoryStartImpl();
+    1427             : 
+    1428           0 :   std_srvs::TriggerResponse res;
+    1429           0 :   res.success = success;
+    1430           0 :   res.message = message;
+    1431             : 
+    1432           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1433             : }
+    1434             : 
+    1435             : //}
+    1436             : 
+    1437             : /* //{ setConstraints() */
+    1438             : 
+    1439          13 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+    1440             : 
+    1441          13 :   if (!is_initialized_) {
+    1442           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+    1443             :   }
+    1444             : 
+    1445          13 :   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          26 :     std::scoped_lock lock(mutex_constraints_filtered_);
+    1451             : 
+    1452             :     // important! this needs to be done to initialize the full struct
+    1453          13 :     if (!got_constraints_) {
+    1454             : 
+    1455           3 :       constraints_filtered_ = constraints->constraints;
+    1456             : 
+    1457             :     } else {
+    1458             : 
+    1459          10 :       constraints_filtered_.horizontal_speed          = constraints->constraints.horizontal_speed;
+    1460          10 :       constraints_filtered_.vertical_ascending_speed  = constraints->constraints.vertical_ascending_speed;
+    1461          10 :       constraints_filtered_.vertical_descending_speed = constraints->constraints.vertical_descending_speed;
+    1462          10 :       constraints_filtered_.heading_speed             = constraints->constraints.heading_speed;
+    1463             :     }
+    1464             :   }
+    1465             : 
+    1466          13 :   got_constraints_ = true;
+    1467             : 
+    1468          13 :   all_constraints_set_ = false;
+    1469             : 
+    1470          13 :   ROS_INFO("[MpcTracker]: updating constraints");
+    1471             : 
+    1472          26 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+    1473          13 :   res.success = true;
+    1474          13 :   res.message = "constraints updated";
+    1475             : 
+    1476          13 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+    1477             : }
+    1478             : 
+    1479             : //}
+    1480             : 
+    1481             : /* //{ setReference() */
+    1482             : 
+    1483           2 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MpcTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+    1484             : 
+    1485           2 :   toggleHover(false);
+    1486             : 
+    1487           2 :   setGoal(cmd->reference.position.x, cmd->reference.position.y, cmd->reference.position.z, cmd->reference.heading, true);
+    1488             : 
+    1489           4 :   mrs_msgs::ReferenceSrvResponse res;
+    1490           2 :   res.success = true;
+    1491           2 :   res.message = "reference set";
+    1492             : 
+    1493           4 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+    1494             : }
+    1495             : 
+    1496             : //}
+    1497             : 
+    1498             : /* //{ setVelocityReference() */
+    1499             : 
+    1500           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MpcTracker::setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+    1501             : 
+    1502           0 :   if (!is_initialized_) {
+    1503           0 :     return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+    1504             :   }
+    1505             : 
+    1506             :   {
+    1507           0 :     std::scoped_lock lock(mutex_velocity_reference_);
+    1508             : 
+    1509           0 :     velocity_reference_time_ = ros::Time::now();
+    1510             : 
+    1511           0 :     velocity_reference_ = cmd->reference;
+    1512             :   }
+    1513             : 
+    1514           0 :   if (!velocity_tracking_active_) {
+    1515             : 
+    1516           0 :     ROS_INFO("[MpcTracker]: starting velocity tracking timer");
+    1517             : 
+    1518           0 :     timer_velocity_tracking_.stop();
+    1519           0 :     timer_velocity_tracking_.start();
+    1520             : 
+    1521           0 :     velocity_tracking_active_ = true;
+    1522             :   }
+    1523             : 
+    1524           0 :   mrs_msgs::VelocityReferenceSrvResponse response;
+    1525           0 :   response.success = true;
+    1526           0 :   response.message = "reference set";
+    1527             : 
+    1528           0 :   return mrs_msgs::VelocityReferenceSrvResponse::ConstPtr(new mrs_msgs::VelocityReferenceSrvResponse(response));
+    1529             : }
+    1530             : 
+    1531             : //}
+    1532             : 
+    1533             : /* //{ setTrajectoryReference() */
+    1534             : 
+    1535           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MpcTracker::setTrajectoryReference([
+    1536             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+    1537             : 
+    1538           0 :   std::stringstream ss;
+    1539             : 
+    1540           0 :   auto [success, message, modified] = loadTrajectory(cmd->trajectory);
+    1541             : 
+    1542           0 :   mrs_msgs::TrajectoryReferenceSrvResponse response;
+    1543           0 :   response.success  = success;
+    1544           0 :   response.message  = message;
+    1545           0 :   response.modified = modified;
+    1546             : 
+    1547           0 :   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           3 : void MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, [[maybe_unused]] uint32_t level) {
+    1691             : 
+    1692           6 :   std::scoped_lock lock(mutex_drs_params_);
+    1693             : 
+    1694           3 :   drs_params_ = config;
+    1695             : 
+    1696           3 :   ROS_INFO("[MpcTracker]: DRS updated");
+    1697           3 : }
+    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        1047 : double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
+    1743             : 
+    1744        1047 :   std::scoped_lock lock(mutex_predicted_trajectory_, mutex_des_trajectory_, mutex_other_uav_avoidance_trajectories_);
+    1745             : 
+    1746        1047 :   first_collision_index = INT_MAX;
+    1747        1047 :   avoiding_collision_   = false;
+    1748             : 
+    1749        1047 :   std::map<std::string, mrs_msgs::FutureTrajectory>::iterator u = other_uav_avoidance_trajectories_.begin();
+    1750             : 
+    1751        1047 :   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        1047 :   if (!avoiding_collision_) {
+    1800             : 
+    1801        1047 :     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        1047 :     collision_free_altitude_ -= 2.0 / (1.0 / dt1);
+    1805             : 
+    1806        1047 :     double safety_area_min_z = common_handlers_->safety_area.getMinZ("");
+    1807             : 
+    1808        1047 :     if (collision_free_altitude_ < safety_area_min_z) {
+    1809             : 
+    1810        1047 :       collision_free_altitude_ = safety_area_min_z;
+    1811             :     }
+    1812             :   }
+    1813             : 
+    1814        2094 :   return collision_free_altitude_;
+    1815             : }
+    1816             : 
+    1817             : //}
+    1818             : 
+    1819             : // | ------------------ trajectory filtering ------------------ |
+    1820             : 
+    1821             : /* //{ filterReferenceXY() */
+    1822             : 
+    1823        1047 : 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        1047 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1827             : 
+    1828        2094 :   auto mpc_x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1829        1047 :   auto trajectory_dt = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_dt_);
+    1830             : 
+    1831        2094 :   MatrixXd filtered_x_trajectory = MatrixXd::Zero(_mpc_horizon_len_, 1);
+    1832        2094 :   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       42927 :   for (int i = 0; i < _mpc_horizon_len_; i++) {
+    1840             : 
+    1841       41880 :     if (i == 0) {
+    1842        1047 :       max_sample_x = max_speed_x * dt1;
+    1843        1047 :       max_sample_y = max_speed_y * dt1;
+    1844        1047 :       difference_x = des_x_trajectory(i, 0) - mpc_x(0, 0);
+    1845        1047 :       difference_y = des_y_trajectory(i, 0) - mpc_x(4, 0);
+    1846             :     } else {
+    1847       40833 :       max_sample_x = max_speed_x * _dt2_;
+    1848       40833 :       max_sample_y = max_speed_y * _dt2_;
+    1849       40833 :       difference_x = des_x_trajectory(i, 0) - filtered_x_trajectory(i - 1, 0);
+    1850       40833 :       difference_y = des_y_trajectory(i, 0) - filtered_y_trajectory(i - 1, 0);
+    1851             :     }
+    1852             : 
+    1853       41880 :     if (!trajectory_tracking_in_progress_) {
+    1854             : 
+    1855       41880 :       double direction_angle  = atan2(difference_y, difference_x);
+    1856       41880 :       double max_dir_sample_x = abs(max_sample_x * cos(direction_angle));
+    1857       41880 :       double max_dir_sample_y = abs(max_sample_y * sin(direction_angle));
+    1858             : 
+    1859       41880 :       if (max_sample_x > max_dir_sample_x) {
+    1860       10552 :         max_sample_x = max_dir_sample_x;
+    1861             :       }
+    1862       41880 :       if (max_sample_y > max_dir_sample_y) {
+    1863       41880 :         max_sample_y = max_dir_sample_y;
+    1864             :       }
+    1865             : 
+    1866             :       // saturate the difference
+    1867       41880 :       if (difference_x > max_sample_x)
+    1868           0 :         difference_x = max_sample_x;
+    1869       41880 :       else if (difference_x < -max_sample_x)
+    1870        9698 :         difference_x = -max_sample_x;
+    1871             : 
+    1872       41880 :       if (difference_y > max_sample_y)
+    1873           0 :         difference_y = max_sample_y;
+    1874       41880 :       else if (difference_y < -max_sample_y)
+    1875        9698 :         difference_y = -max_sample_y;
+    1876             :     }
+    1877             : 
+    1878       41880 :     if (i == 0) {
+    1879        1047 :       filtered_x_trajectory(i, 0) = mpc_x(0, 0) + difference_x;
+    1880        1047 :       filtered_y_trajectory(i, 0) = mpc_x(4, 0) + difference_y;
+    1881             :     } else {
+    1882       40833 :       filtered_x_trajectory(i, 0) = filtered_x_trajectory(i - 1, 0) + difference_x;
+    1883       40833 :       filtered_y_trajectory(i, 0) = filtered_y_trajectory(i - 1, 0) + difference_y;
+    1884             :     }
+    1885             :   }
+    1886             : 
+    1887             :   // | ----------------------- add wiggle ----------------------- |
+    1888             : 
+    1889        1047 :   auto [wiggle_enabled, wiggle_amplitude, wiggle_frequency_] =
+    1890        1047 :       mrs_lib::get_mutexed(mutex_drs_params_, drs_params_.wiggle_enabled, drs_params_.wiggle_amplitude, drs_params_.wiggle_frequency);
+    1891             : 
+    1892        1047 :   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        2094 :   return std::make_tuple(filtered_x_trajectory, filtered_y_trajectory);
+    1907             : }
+    1908             : 
+    1909             : //}
+    1910             : 
+    1911             : /* //{ filterReferenceZ() */
+    1912             : 
+    1913        1047 : MatrixXd MpcTracker::filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed) {
+    1914             : 
+    1915        2094 :   auto mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1916             : 
+    1917        1047 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1918             : 
+    1919             :   double difference_z;
+    1920             :   double max_sample_z;
+    1921             : 
+    1922        1047 :   MatrixXd filtered_trajectory = MatrixXd::Zero(_mpc_horizon_len_, 1);
+    1923             : 
+    1924        1047 :   double current_z = mpc_x(8, 0);
+    1925             : 
+    1926       42927 :   for (int i = 0; i < _mpc_horizon_len_; i++) {
+    1927             : 
+    1928       41880 :     if (i == 0) {
+    1929             : 
+    1930        1047 :       difference_z = des_z_trajectory(i, 0) - current_z;
+    1931             : 
+    1932        1047 :       if (difference_z > 0) {
+    1933         522 :         max_sample_z = max_ascending_speed * dt1;
+    1934             :       } else {
+    1935         525 :         max_sample_z = max_descending_speed * dt1;
+    1936             :       }
+    1937             : 
+    1938             :     } else {
+    1939             : 
+    1940       40833 :       difference_z = des_z_trajectory(i, 0) - filtered_trajectory(i - 1, 0);
+    1941             : 
+    1942       40833 :       if (difference_z > 0) {
+    1943        2839 :         max_sample_z = max_ascending_speed * _dt2_;
+    1944             :       } else {
+    1945       37994 :         max_sample_z = max_descending_speed * _dt2_;
+    1946             :       }
+    1947             :     }
+    1948             : 
+    1949       41880 :     if (!trajectory_tracking_in_progress_) {
+    1950             : 
+    1951             :       // saturate the difference
+    1952       41880 :       if (difference_z > max_sample_z)
+    1953        2839 :         difference_z = max_sample_z;
+    1954       39041 :       else if (difference_z < -max_sample_z)
+    1955         234 :         difference_z = -max_sample_z;
+    1956             :     }
+    1957             : 
+    1958       41880 :     if (i == 0) {
+    1959        1047 :       filtered_trajectory(i, 0) = current_z + difference_z;
+    1960             :     } else {
+    1961       40833 :       filtered_trajectory(i, 0) = filtered_trajectory(i - 1, 0) + difference_z;
+    1962             :     }
+    1963             :   }
+    1964             : 
+    1965        2094 :   return filtered_trajectory;
+    1966             : }
+    1967             : 
+    1968             : //}
+    1969             : 
+    1970             : /* //{ manageConstraints() */
+    1971             : 
+    1972        1047 : void MpcTracker::manageConstraints() {
+    1973             : 
+    1974        1047 :   if (!got_constraints_) {
+    1975        1043 :     return;
+    1976             :   }
+    1977             : 
+    1978        1047 :   if (all_constraints_set_) {
+    1979        1043 :     return;
+    1980             :   }
+    1981             : 
+    1982           4 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1983           8 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1984             : 
+    1985           8 :   bool can_change = (fabs(mpc_x(1, 0)) < constraints.horizontal_speed) && (fabs(mpc_x(2, 0)) < constraints.horizontal_acceleration) &&
+    1986           4 :                     (fabs(mpc_x(3, 0)) < constraints.horizontal_jerk) && (fabs(mpc_x(5, 0)) < constraints.horizontal_speed) &&
+    1987           4 :                     (fabs(mpc_x(6, 0)) < constraints.horizontal_acceleration) && (fabs(mpc_x(7, 0)) < constraints.horizontal_jerk) &&
+    1988           4 :                     (mpc_x(9, 0) < constraints.vertical_ascending_speed) && (mpc_x(9, 0) > -constraints.vertical_descending_speed) &&
+    1989           4 :                     (mpc_x(10, 0) < constraints.vertical_ascending_acceleration) && (mpc_x(10, 0) > -constraints.vertical_descending_acceleration) &&
+    1990           4 :                     (mpc_x(11, 0) < constraints.vertical_ascending_jerk) && (mpc_x(11, 0) > -constraints.vertical_descending_jerk) &&
+    1991          12 :                     (fabs(mpc_x_heading(1, 0)) < constraints.heading_speed) && (fabs(mpc_x_heading(2, 0)) < constraints.heading_acceleration) &&
+    1992           4 :                     (fabs(mpc_x_heading(3, 0)) < constraints.heading_jerk);
+    1993             : 
+    1994           4 :   if (can_change) {
+    1995             : 
+    1996             :     {
+    1997           4 :       std::scoped_lock lock(mutex_constraints_filtered_);
+    1998             : 
+    1999           4 :       constraints_filtered_.horizontal_acceleration = constraints.horizontal_acceleration;
+    2000           4 :       constraints_filtered_.horizontal_jerk         = constraints.horizontal_jerk;
+    2001           4 :       constraints_filtered_.horizontal_snap         = constraints.horizontal_snap;
+    2002             : 
+    2003           4 :       constraints_filtered_.vertical_ascending_acceleration = constraints.vertical_ascending_acceleration;
+    2004           4 :       constraints_filtered_.vertical_ascending_jerk         = constraints.vertical_ascending_jerk;
+    2005           4 :       constraints_filtered_.vertical_ascending_snap         = constraints.vertical_ascending_snap;
+    2006             : 
+    2007           4 :       constraints_filtered_.vertical_descending_acceleration = constraints.vertical_descending_acceleration;
+    2008           4 :       constraints_filtered_.vertical_descending_jerk         = constraints.vertical_descending_jerk;
+    2009           4 :       constraints_filtered_.vertical_descending_snap         = constraints.vertical_descending_snap;
+    2010             : 
+    2011           4 :       constraints_filtered_.heading_acceleration = constraints.heading_acceleration;
+    2012           4 :       constraints_filtered_.heading_jerk         = constraints.heading_jerk;
+    2013           4 :       constraints_filtered_.heading_snap         = constraints.heading_snap;
+    2014             :     }
+    2015             : 
+    2016           4 :     ROS_INFO_THROTTLE(1.0, "[MpcTracker]: all constraints succesfully applied");
+    2017           4 :     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        1047 : void MpcTracker::calculateMPC() {
+    2029             : 
+    2030        1047 :   std::scoped_lock lock(mutex_mpc_calculation_);
+    2031             : 
+    2032        1047 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2033             : 
+    2034        1047 :   ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: MPC calculation dt = " << dt1);
+    2035             : 
+    2036        1047 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_filtered_, constraints_filtered_);
+    2037        1047 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2038        1047 :   auto uav_state              = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2039        1047 :   auto drs_params             = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    2040             : 
+    2041        1047 :   MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    2042             :   {
+    2043        2094 :     std::scoped_lock lock(mutex_des_trajectory_);
+    2044             : 
+    2045        1047 :     des_x_trajectory       = des_x_trajectory_;
+    2046        1047 :     des_y_trajectory       = des_y_trajectory_;
+    2047        1047 :     des_z_trajectory       = des_z_trajectory_;
+    2048        1047 :     des_heading_trajectory = des_heading_trajectory_;
+    2049             :   }
+    2050             : 
+    2051        1047 :   int    first_collision_index = INT_MAX;
+    2052        1047 :   double lowest_z              = std::numeric_limits<double>::max();
+    2053             : 
+    2054        1047 :   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       42927 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2058       41880 :       if (des_z_trajectory_(i, 0) < lowest_z) {
+    2059        1047 :         lowest_z = des_z_trajectory_(i, 0);
+    2060             :       }
+    2061             :     }
+    2062             : 
+    2063             :     // check other drone trajectories for collisions
+    2064        1047 :     minimum_collison_free_altitude_ = checkTrajectoryForCollisions(first_collision_index);
+    2065             : 
+    2066             :   } else {
+    2067             : 
+    2068           0 :     minimum_collison_free_altitude_ = common_handlers_->safety_area.getMinZ("");
+    2069             :   }
+    2070             : 
+    2071        1047 :   double max_speed_x = constraints.horizontal_speed;
+    2072        1047 :   double max_speed_y = constraints.horizontal_speed;
+    2073        1047 :   double max_speed_z = constraints.vertical_ascending_speed;
+    2074        1047 :   double min_speed_z = constraints.vertical_descending_speed;
+    2075             : 
+    2076        1047 :   double max_acc_x = constraints.horizontal_acceleration;
+    2077        1047 :   double max_acc_y = constraints.horizontal_acceleration;
+    2078        1047 :   double max_acc_z = constraints.vertical_ascending_acceleration;
+    2079        1047 :   double min_acc_z = constraints.vertical_descending_acceleration;
+    2080             : 
+    2081        1047 :   double max_snap_x = constraints.horizontal_snap;
+    2082        1047 :   double max_snap_y = constraints.horizontal_snap;
+    2083        1047 :   double max_snap_z = constraints.vertical_ascending_snap;
+    2084        1047 :   double min_snap_z = constraints.vertical_descending_snap;
+    2085             : 
+    2086        1047 :   double max_jerk_x = constraints.horizontal_jerk;
+    2087        1047 :   double max_jerk_y = constraints.horizontal_jerk;
+    2088        1047 :   double max_jerk_z = constraints.vertical_ascending_jerk;
+    2089        1047 :   double min_jerk_z = constraints.vertical_descending_jerk;
+    2090             : 
+    2091        1047 :   collision_avoidance_affecting_me_ = false;
+    2092             : 
+    2093        1047 :   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        1047 :   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        2094 :   VectorXd mpc_u         = VectorXd::Zero(_mpc_m_states_);
+    2139        1047 :   double   mpc_u_heading = 0;
+    2140             : 
+    2141        1047 :   double iters_z       = 0;
+    2142        1047 :   double iters_x       = 0;
+    2143        1047 :   double iters_y       = 0;
+    2144        1047 :   double iters_heading = 0;
+    2145             : 
+    2146        1047 :   ros::Time time_begin = ros::Time::now();
+    2147             : 
+    2148        2094 :   MatrixXd des_z_filtered = filterReferenceZ(des_z_trajectory, max_speed_z, min_speed_z);
+    2149             : 
+    2150       42927 :   for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2151       41880 :     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       41880 :       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        1047 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2163         575 :     mpc_solver_z_->setVelQ(drs_params.q_vel_braking);
+    2164             :   } else {
+    2165         472 :     mpc_solver_z_->setVelQ(drs_params.q_vel_no_braking);
+    2166             :   }
+    2167             : 
+    2168        2094 :   MatrixXd initial_z = MatrixXd::Zero(4, 1);
+    2169             : 
+    2170        1047 :   initial_z(0, 0) = mpc_x(8, 0);
+    2171        1047 :   initial_z(1, 0) = mpc_x(9, 0);
+    2172        1047 :   initial_z(2, 0) = mpc_x(10, 0);
+    2173        1047 :   initial_z(3, 0) = mpc_x(11, 0);
+    2174             : 
+    2175        1047 :   mpc_solver_z_->setDt(dt1);
+    2176        1047 :   mpc_solver_z_->setInitialState(initial_z);
+    2177        1047 :   mpc_solver_z_->loadReference(des_z_filtered_offset_);
+    2178        1047 :   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        1047 :   iters_z += mpc_solver_z_->solveMPC();
+    2180             : 
+    2181             :   {
+    2182        2094 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2183             : 
+    2184        1047 :     mpc_solver_z_->getStates(predicted_trajectory_);
+    2185             :   }
+    2186             : 
+    2187        1047 :   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        1047 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2193             : 
+    2194        1047 :     ascend = (predicted_trajectory_(10, 0) / max_speed_z);
+    2195             :   }
+    2196             : 
+    2197        1047 :   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        3141 :   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        1047 :   des_heading_trajectory(0, 0) = sradians::unwrap(des_heading_trajectory(0, 0), mpc_x_heading(0));
+    2207             : 
+    2208       41880 :   for (int i = 1; i < _mpc_horizon_len_; i++) {
+    2209       40833 :     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        1047 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2215         575 :     mpc_solver_x_->setVelQ(drs_params.q_vel_braking);
+    2216             :   } else {
+    2217         472 :     mpc_solver_x_->setVelQ(drs_params.q_vel_no_braking);
+    2218             :   }
+    2219             : 
+    2220        2094 :   MatrixXd initial_x = MatrixXd::Zero(4, 1);
+    2221             : 
+    2222        1047 :   initial_x(0, 0) = mpc_x(0, 0);
+    2223        1047 :   initial_x(1, 0) = mpc_x(1, 0);
+    2224        1047 :   initial_x(2, 0) = mpc_x(2, 0);
+    2225        1047 :   initial_x(3, 0) = mpc_x(3, 0);
+    2226             : 
+    2227        1047 :   mpc_solver_x_->setDt(dt1);
+    2228        1047 :   mpc_solver_x_->setInitialState(initial_x);
+    2229        1047 :   mpc_solver_x_->loadReference(des_x_filtered);
+    2230        1047 :   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        1047 :   iters_x += mpc_solver_x_->solveMPC();
+    2232             : 
+    2233             :   {
+    2234        2094 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2235             : 
+    2236        1047 :     mpc_solver_x_->getStates(predicted_trajectory_);
+    2237             :   }
+    2238             : 
+    2239        1047 :   mpc_u(0) = mpc_solver_x_->getFirstControlInput();
+    2240             : 
+    2241             :   // | -------------------- MPC solver y-axis ------------------- |
+    2242             : 
+    2243        1047 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2244         575 :     mpc_solver_y_->setVelQ(drs_params.q_vel_braking);
+    2245             :   } else {
+    2246         472 :     mpc_solver_y_->setVelQ(drs_params.q_vel_no_braking);
+    2247             :   }
+    2248             : 
+    2249        2094 :   MatrixXd initial_y = MatrixXd::Zero(4, 1);
+    2250             : 
+    2251        1047 :   initial_y(0, 0) = mpc_x(4, 0);
+    2252        1047 :   initial_y(1, 0) = mpc_x(5, 0);
+    2253        1047 :   initial_y(2, 0) = mpc_x(6, 0);
+    2254        1047 :   initial_y(3, 0) = mpc_x(7, 0);
+    2255             : 
+    2256        1047 :   mpc_solver_y_->setDt(dt1);
+    2257        1047 :   mpc_solver_y_->setInitialState(initial_y);
+    2258        1047 :   mpc_solver_y_->loadReference(des_y_filtered);
+    2259        1047 :   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        1047 :   iters_y += mpc_solver_y_->solveMPC();
+    2261             :   {
+    2262        2094 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2263             : 
+    2264        1047 :     mpc_solver_y_->getStates(predicted_trajectory_);
+    2265             :   }
+    2266        1047 :   mpc_u(1) = mpc_solver_y_->getFirstControlInput();
+    2267             : 
+    2268             :   // | ------------------- MPC solver heading ------------------- |
+    2269             : 
+    2270        1047 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2271         575 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_braking);
+    2272             :   } else {
+    2273         472 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_no_braking);
+    2274             :   }
+    2275             : 
+    2276        1047 :   mpc_solver_heading_->setDt(dt1);
+    2277        1047 :   mpc_solver_heading_->setInitialState(mpc_x_heading);
+    2278        1047 :   mpc_solver_heading_->loadReference(des_heading_trajectory);
+    2279        1047 :   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        1047 :   iters_heading += mpc_solver_heading_->solveMPC();
+    2282             :   {
+    2283        2094 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2284             : 
+    2285        1047 :     mpc_solver_heading_->getStates(predicted_heading_trajectory_);
+    2286             :   }
+    2287        1047 :   mpc_u_heading = mpc_solver_heading_->getFirstControlInput();
+    2288             : 
+    2289             :   {
+    2290        1047 :     bool saturating = false;
+    2291             : 
+    2292        1047 :     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        1047 :     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        1047 :     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        1047 :     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        1047 :     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        1047 :     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        1047 :     if (saturating) {
+    2324           0 :       debugPrintState(0.1);
+    2325           0 :       debugPrintMPCResult(0.1);
+    2326             :     }
+    2327             :   }
+    2328             : 
+    2329             :   {
+    2330        1047 :     std::scoped_lock lock(mutex_mpc_u_);
+    2331             : 
+    2332        1047 :     mpc_u_         = mpc_u;
+    2333        1047 :     mpc_u_heading_ = mpc_u_heading;
+    2334             :   }
+    2335             : 
+    2336        1047 :   double mpc_solver_time = (ros::Time::now() - time_begin).toSec();
+    2337        1047 :   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        1047 :   future_was_predicted_ = true;
+    2345             : 
+    2346             :   // | ------------- breaking for the next iteration ------------ |
+    2347             : 
+    2348        3141 :   if (drs_params.braking_enabled &&
+    2349        1047 :       (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         577 :       (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        2670 :       (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         576 :       (fabs(radians::diff(des_heading_trajectory(10), des_heading_trajectory(_mpc_horizon_len_ - 1))) <= 0.1 &&
+    2353         576 :        fabs(radians::diff(des_heading_trajectory(30), des_heading_trajectory(_mpc_horizon_len_ - 1))) <= 0.1)) {
+    2354         576 :     brake_ = true;
+    2355         576 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: braking");
+    2356             :   } else {
+    2357         471 :     brake_ = false;
+    2358             :   }
+    2359             : 
+    2360             :   /* publish mpc reference //{ */
+    2361             : 
+    2362             :   {
+    2363        2094 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2364        1047 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2365        1047 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    2366             : 
+    2367             :     {
+    2368        2094 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    2369             : 
+    2370       42927 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2371             : 
+    2372       41880 :         geometry_msgs::Pose new_pose;
+    2373             : 
+    2374       41880 :         new_pose.position.x = des_x_filtered(i, 0);
+    2375       41880 :         new_pose.position.y = des_y_filtered(i, 0);
+    2376       41880 :         new_pose.position.z = des_z_filtered(i, 0);
+    2377             : 
+    2378       41880 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(i));
+    2379             : 
+    2380       41880 :         debug_trajectory_out.poses.push_back(new_pose);
+    2381             :       }
+    2382             :     }
+    2383             : 
+    2384        1047 :     ph_mpc_reference_debugging_.publish(debug_trajectory_out);
+    2385             :   }
+    2386             : 
+    2387             :   //}
+    2388             : }
+    2389             : 
+    2390             : //}
+    2391             : 
+    2392             : /* iterateModel() //{ */
+    2393             : 
+    2394        1054 : void MpcTracker::iterateModel(const double& dt) {
+    2395             : 
+    2396        1054 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2397             : 
+    2398        1054 :   if (model_first_iteration_) {
+    2399             : 
+    2400           2 :     model_iteration_last_time_ = ros::Time::now();
+    2401           2 :     model_first_iteration_     = false;
+    2402             : 
+    2403             :   } else {
+    2404             : 
+    2405        1052 :     dt1 = 0.9 * dt1 + 0.1 * dt;
+    2406             : 
+    2407        1052 :     mrs_lib::set_mutexed(mutex_dt1_, dt1, dt1_);
+    2408        1052 :     timer_mpc_iteration_.setPeriod(ros::Duration(dt1), false);
+    2409             : 
+    2410             :     // clang-format off
+    2411        1052 :     A_ << 1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,           0, 0,   0,           0,
+    2412        1052 :           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,           0, 0,   0,           0,
+    2413        1052 :           0, 0,   1,           dt1,         0, 0,   0,           0,           0, 0,   0,           0,
+    2414        1052 :           0, 0,   0,           1,           0, 0,   0,           0,           0, 0,   0,           0,
+    2415        1052 :           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,
+    2416        1052 :           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,
+    2417        1052 :           0, 0,   0,           0,           0, 0,   1,           dt1,         0, 0,   0,           0,
+    2418        1052 :           0, 0,   0,           0,           0, 0,   0,           1,           0, 0,   0,           0,
+    2419        1052 :           0, 0,   0,           0,           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,
+    2420        1052 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1,
+    2421        1052 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   1,           dt1,
+    2422        1052 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   0,           1;
+    2423             : 
+    2424        1052 :       B_ << 0,   0,   0,
+    2425        1052 :             0,   0,   0,
+    2426        1052 :             0,   0,   0,
+    2427        1052 :             dt1, 0,   0,
+    2428        1052 :             0,   0,   0,
+    2429        1052 :             0,   0,   0,
+    2430        1052 :             0,   0,   0,
+    2431        1052 :             0,   dt1, 0,
+    2432        1052 :             0,   0,   0,
+    2433        1052 :             0,   0,   0,
+    2434        1052 :             0,   0,   0,
+    2435        1052 :             0,   0,   dt1;
+    2436             : 
+    2437        1052 :       A_heading_ << 1, dt1, 0.5*dt1*dt1, 0,
+    2438        1052 :                     0, 1,   dt1,         0.5*dt1*dt1,
+    2439        1052 :                     0, 0,   1,           dt1,
+    2440        1052 :                     0, 0,   0,           1;
+    2441             : 
+    2442        1052 :       B_heading_ << 0,
+    2443        1052 :                     0,
+    2444        2104 :                     0,
+    2445        1052 :                     dt1;
+    2446             : 
+    2447        1052 :     model_iteration_last_time_ = ros::Time::now();
+    2448             :   }
+    2449             : 
+    2450             :   {
+    2451        2108 :     auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2452        2108 :     auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    2453             : 
+    2454        2108 :     MatrixXd new_mpc_x         = A_ * mpc_x + B_ * mpc_u;
+    2455        2108 :     MatrixXd new_mpc_x_heading = A_heading_ * mpc_x_heading + B_heading_ * mpc_u_heading;
+    2456             : 
+    2457             :     // | --------------- check the state difference --------------- |
+    2458             :     {
+    2459        1054 :       auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    2460             : 
+    2461        1054 :       bool problem = false;
+    2462             : 
+    2463             :       // position
+    2464             : 
+    2465        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       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        1054 :       if (problem) {
+    2574           0 :         debugPrintState(0.001);
+    2575           0 :         debugPrintMPCResult(0.001);
+    2576             :       }
+    2577             :     }
+    2578             : 
+    2579             :     {
+    2580        1054 :       std::scoped_lock lock(mutex_mpc_x_);
+    2581             : 
+    2582        1054 :       mpc_x_         = new_mpc_x;
+    2583        1054 :       mpc_x_heading_ = new_mpc_x_heading;
+    2584             : 
+    2585        1054 :       mpc_x_heading_(0) = sradians::wrap(mpc_x_heading_(0));
+    2586             :     }
+    2587             :   }
+    2588        1054 : }
+    2589             : 
+    2590             : //}
+    2591             : 
+    2592             : // | -------------------- referece setting -------------------- |
+    2593             : 
+    2594             : /* //{ loadTrajectory() */
+    2595             : 
+    2596             : // method for setting desired trajectory
+    2597           0 : std::tuple<bool, std::string, bool> MpcTracker::loadTrajectory(const mrs_msgs::TrajectoryReference msg) {
+    2598             : 
+    2599           0 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2600             : 
+    2601             :   // copy the member variables
+    2602           0 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    2603           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2604             : 
+    2605           0 :   std::stringstream ss;
+    2606             : 
+    2607             :   /* check the trajectory dt //{ */
+    2608             : 
+    2609             :   double trajectory_dt;
+    2610           0 :   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           0 :   } 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           0 :     trajectory_dt = msg.dt;
+    2620             :   }
+    2621             : 
+    2622             :   //}
+    2623             : 
+    2624           0 :   int trajectory_size = msg.points.size();
+    2625             : 
+    2626             :   /* sanitize the time-ness of the trajectory //{ */
+    2627             : 
+    2628           0 :   int    trajectory_sample_offset    = 0;  // how many samples in past is the trajectory
+    2629           0 :   int    trajectory_subsample_offset = 0;  // how many simulation inner loops ahead of the first valid sample
+    2630           0 :   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           0 :   if (msg.fly_now) {
+    2634             : 
+    2635           0 :     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           0 :     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           0 :       trajectory_time_offset = (ros::Time::now() - trajectory_time).toSec();
+    2647             : 
+    2648             :       // when the time offset is negative, thus in the future
+    2649             :       // just say it, but use it like its from the current time
+    2650           0 :       if (trajectory_time_offset < 0.0) {
+    2651             : 
+    2652           0 :         ROS_WARN_THROTTLE(1.0, "[MpcTracker]: received trajectory with timestamp in the future by %.3f s", -trajectory_time_offset);
+    2653             : 
+    2654           0 :         trajectory_time_offset = 0.0;
+    2655             :       }
+    2656             :     }
+    2657             : 
+    2658             :     // if the time offset is set, check if we need to "move the first idx"
+    2659           0 :     if (trajectory_time_offset > 0.0) {
+    2660             : 
+    2661             :       // calculate the offset in samples
+    2662           0 :       trajectory_sample_offset = int(floor(trajectory_time_offset / trajectory_dt));
+    2663             : 
+    2664             :       // and get the subsample offset, which will be used to initialize the interpolator
+    2665           0 :       trajectory_subsample_offset = int(floor(fmod(trajectory_time_offset, trajectory_dt) / dt1));
+    2666             : 
+    2667           0 :       ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: received trajectory with timestamp in the past by %.3f s",
+    2668             :                          trajectory_dt * trajectory_sample_offset + dt1 * trajectory_subsample_offset);
+    2669             : 
+    2670             :       // if the offset is larger than the number of points in the trajectory
+    2671             :       // the trajectory can not be used
+    2672           0 :       if (trajectory_sample_offset >= trajectory_size) {
+    2673             : 
+    2674           0 :         ss << "trajectory timestamp is too old (time difference = " << trajectory_time_offset << ")";
+    2675           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2676           0 :         return std::tuple(false, ss.str(), false);
+    2677             : 
+    2678             :       } else {
+    2679             : 
+    2680             :         // if the offset is larger than one trajectory sample,
+    2681             :         // offset the start
+    2682           0 :         if (trajectory_time_offset >= trajectory_dt) {
+    2683             : 
+    2684             :           // decrease the trajectory size
+    2685           0 :           trajectory_size -= trajectory_sample_offset;
+    2686             : 
+    2687           0 :           ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: offsetting trajectory by %d samples", trajectory_sample_offset);
+    2688             : 
+    2689             :         } else {
+    2690             : 
+    2691           0 :           trajectory_sample_offset = 0;
+    2692             :         }
+    2693             :       }
+    2694             :     }
+    2695             : 
+    2696             :   } else { // not fly_now
+    2697             : 
+    2698           0 :       trajectory_tracking_in_progress_ = false;
+    2699             : 
+    2700           0 :       timer_trajectory_tracking_.stop();
+    2701             :   }
+    2702             : 
+    2703             :   //}
+    2704             : 
+    2705           0 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory sample offset: %d", trajectory_sample_offset);
+    2706           0 :   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           0 :   MatrixXd des_x_whole_trajectory       = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2718           0 :   MatrixXd des_y_whole_trajectory       = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2719           0 :   MatrixXd des_z_whole_trajectory       = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2720           0 :   MatrixXd des_heading_whole_trajectory = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2721             : 
+    2722           0 :   for (int i = 0; i < trajectory_size; i++) {
+    2723             : 
+    2724           0 :     des_x_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.x;
+    2725           0 :     des_y_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.y;
+    2726           0 :     des_z_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.z;
+    2727           0 :     des_heading_whole_trajectory(i) = msg.points[trajectory_sample_offset + i].heading;
+    2728             :   }
+    2729             : 
+    2730             :   //}
+    2731             : 
+    2732             :   /* set looping //{ */
+    2733             : 
+    2734           0 :   bool loop = false;
+    2735             : 
+    2736           0 :   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           0 :     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           0 :   if (!loop) {
+    2773             : 
+    2774             :     // extend it so it has smooth ending
+    2775           0 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2776             : 
+    2777           0 :       des_x_whole_trajectory(i + trajectory_size)       = des_x_whole_trajectory(i + trajectory_size - 1);
+    2778           0 :       des_y_whole_trajectory(i + trajectory_size)       = des_y_whole_trajectory(i + trajectory_size - 1);
+    2779           0 :       des_z_whole_trajectory(i + trajectory_size)       = des_z_whole_trajectory(i + trajectory_size - 1);
+    2780           0 :       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           0 :     std::scoped_lock lock(mutex_des_whole_trajectory_, mutex_des_trajectory_, mutex_trajectory_tracking_states_);
+    2797             : 
+    2798           0 :     des_whole_trajectory_id_ = msg.input_id;
+    2799             : 
+    2800           0 :     auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2801             : 
+    2802           0 :     trajectory_tracking_in_progress_ = msg.fly_now;
+    2803           0 :     trajectory_track_heading_        = msg.use_heading;
+    2804             : 
+    2805             :     // allocate the vectors
+    2806           0 :     des_x_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2807           0 :     des_y_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2808           0 :     des_z_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2809           0 :     des_heading_whole_trajectory_ = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2810             : 
+    2811           0 :     for (int i = 0; i < trajectory_size + _mpc_horizon_len_; i++) {
+    2812             : 
+    2813           0 :       (*des_x_whole_trajectory_)(i) = des_x_whole_trajectory(i);
+    2814           0 :       (*des_y_whole_trajectory_)(i) = des_y_whole_trajectory(i);
+    2815           0 :       (*des_z_whole_trajectory_)(i) = des_z_whole_trajectory(i);
+    2816             : 
+    2817           0 :       if (trajectory_track_heading_) {
+    2818           0 :         (*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           0 :     if (trajectory_tracking_in_progress_) {
+    2826             : 
+    2827           0 :       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           0 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2832             : 
+    2833           0 :         double first_time = dt1 + i * _dt2_ + trajectory_subsample_offset * dt1;
+    2834             : 
+    2835           0 :         int first_idx  = floor(first_time / trajectory_dt);
+    2836           0 :         int second_idx = first_idx + 1;
+    2837             : 
+    2838           0 :         double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    2839             : 
+    2840           0 :         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           0 :           if (second_idx >= trajectory_size) {
+    2852           0 :             second_idx = trajectory_size - 1;
+    2853             :           }
+    2854             : 
+    2855           0 :           if (first_idx >= trajectory_size) {
+    2856           0 :             first_idx = trajectory_size - 1;
+    2857             :           }
+    2858             :         }
+    2859             : 
+    2860           0 :         des_x_trajectory_(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    2861           0 :         des_y_trajectory_(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    2862           0 :         des_z_trajectory_(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    2863             : 
+    2864           0 :         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           0 :     trajectory_size_             = trajectory_size;
+    2871           0 :     trajectory_tracking_idx_     = 0;
+    2872           0 :     trajectory_tracking_sub_idx_ = trajectory_subsample_offset;
+    2873           0 :     trajectory_set_              = true;
+    2874           0 :     trajectory_tracking_loop_    = loop;
+    2875           0 :     trajectory_dt_               = trajectory_dt;
+    2876           0 :     trajectory_count_++;
+    2877             : 
+    2878           0 :     timer_trajectory_tracking_.setPeriod(ros::Duration(trajectory_dt));
+    2879             :   }
+    2880             : 
+    2881             :   //}
+    2882             : 
+    2883           0 :   if (trajectory_tracking_in_progress_) {
+    2884           0 :     timer_trajectory_tracking_.start();
+    2885             :   }
+    2886             : 
+    2887           0 :   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           0 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2894           0 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2895           0 :     debug_trajectory_out.header.frame_id = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2896             : 
+    2897             :     {
+    2898           0 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2899             : 
+    2900           0 :       for (int i = 0; i < trajectory_size; i++) {
+    2901             : 
+    2902           0 :         geometry_msgs::Pose new_pose;
+    2903             : 
+    2904           0 :         new_pose.position.x = (*des_x_whole_trajectory_)(i);
+    2905           0 :         new_pose.position.y = (*des_y_whole_trajectory_)(i);
+    2906           0 :         new_pose.position.z = (*des_z_whole_trajectory_)(i);
+    2907             : 
+    2908           0 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(i));
+    2909             : 
+    2910           0 :         debug_trajectory_out.poses.push_back(new_pose);
+    2911             :       }
+    2912             :     }
+    2913             : 
+    2914           0 :     pub_debug_processed_trajectory_poses_.publish(debug_trajectory_out);
+    2915             : 
+    2916           0 :     visualization_msgs::MarkerArray msg_out;
+    2917             : 
+    2918           0 :     visualization_msgs::Marker marker;
+    2919             : 
+    2920           0 :     marker.header.stamp     = ros::Time::now();
+    2921           0 :     marker.header.frame_id  = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2922           0 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    2923           0 :     marker.color.a          = 1;
+    2924           0 :     marker.scale.x          = 0.05;
+    2925           0 :     marker.color.r          = 1;
+    2926           0 :     marker.color.g          = 0;
+    2927           0 :     marker.color.b          = 0;
+    2928           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2929             : 
+    2930             :     {
+    2931           0 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2932             : 
+    2933           0 :       for (int i = 0; i < trajectory_size - 1; i++) {
+    2934             : 
+    2935           0 :         geometry_msgs::Point point1;
+    2936             : 
+    2937           0 :         point1.x = des_x_whole_trajectory(i);
+    2938           0 :         point1.y = des_y_whole_trajectory(i);
+    2939           0 :         point1.z = des_z_whole_trajectory(i);
+    2940             : 
+    2941           0 :         marker.points.push_back(point1);
+    2942             : 
+    2943           0 :         geometry_msgs::Point point2;
+    2944             : 
+    2945           0 :         point2.x = des_x_whole_trajectory(i + 1);
+    2946           0 :         point2.y = des_y_whole_trajectory(i + 1);
+    2947           0 :         point2.z = des_z_whole_trajectory(i + 1);
+    2948             : 
+    2949           0 :         marker.points.push_back(point2);
+    2950             :       }
+    2951             :     }
+    2952             : 
+    2953           0 :     msg_out.markers.push_back(marker);
+    2954             : 
+    2955           0 :     pub_debug_processed_trajectory_markers_.publish(msg_out);
+    2956             :   }
+    2957             : 
+    2958             :   //}
+    2959             : 
+    2960           0 :   publishDiagnostics();
+    2961             : 
+    2962           0 :   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           9 : void MpcTracker::setSinglePointReference(const double x, const double y, const double z, const double heading) {
+    2971             : 
+    2972          18 :   std::scoped_lock lock(mutex_des_trajectory_);
+    2973             : 
+    2974           9 :   des_x_trajectory_.fill(x);
+    2975           9 :   des_y_trajectory_.fill(y);
+    2976           9 :   des_z_trajectory_.fill(z);
+    2977           9 :   des_heading_trajectory_.fill(heading);
+    2978           9 : }
+    2979             : 
+    2980             : //}
+    2981             : 
+    2982             : /* //{ setGoal() */
+    2983             : 
+    2984             : // set absolute goal
+    2985           2 : void MpcTracker::setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    2986             : 
+    2987           2 :   double desired_heading = sradians::wrap(heading);
+    2988             : 
+    2989           4 :   auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2990             : 
+    2991           2 :   if (!use_heading) {
+    2992           0 :     desired_heading = mpc_x_heading(0, 0);
+    2993             :   }
+    2994             : 
+    2995           2 :   trajectory_tracking_in_progress_ = false;
+    2996           2 :   timer_trajectory_tracking_.stop();
+    2997             : 
+    2998           2 :   setSinglePointReference(pos_x, pos_y, pos_z, desired_heading);
+    2999             : 
+    3000           2 :   publishDiagnostics();
+    3001           2 : }
+    3002             : 
+    3003             : //}
+    3004             : 
+    3005             : /* //{ setRelativeGoal() */
+    3006             : 
+    3007           7 : void MpcTracker::setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    3008             : 
+    3009          14 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3010             : 
+    3011           7 :   double abs_x = mpc_x(0, 0) + pos_x;
+    3012           7 :   double abs_y = mpc_x(4, 0) + pos_y;
+    3013           7 :   double abs_z = mpc_x(8, 0) + pos_z;
+    3014             : 
+    3015           7 :   double abs_heading = mpc_x_heading(0, 0);
+    3016             : 
+    3017           7 :   if (use_heading) {
+    3018           0 :     abs_heading += heading;
+    3019             :   }
+    3020             : 
+    3021           7 :   trajectory_tracking_in_progress_ = false;
+    3022           7 :   timer_trajectory_tracking_.stop();
+    3023             : 
+    3024           7 :   setSinglePointReference(abs_x, abs_y, abs_z, abs_heading);
+    3025             : 
+    3026           7 :   publishDiagnostics();
+    3027           7 : }
+    3028             : 
+    3029             : //}
+    3030             : 
+    3031             : /* toggleHover() //{ */
+    3032             : 
+    3033           8 : void MpcTracker::toggleHover(bool in) {
+    3034             : 
+    3035             :   // DON'T PUT MUTEX LOCK IN THIS FUNCTION
+    3036             :   // it is called under mutex elsewhere
+    3037             : 
+    3038           8 :   if (in == false && hovering_in_progress_) {
+    3039             : 
+    3040           2 :     ROS_DEBUG("[MpcTracker]: stoppping the hover timer");
+    3041             : 
+    3042           2 :     timer_hover_.stop();
+    3043             : 
+    3044           2 :     hovering_in_progress_ = false;
+    3045             : 
+    3046           6 :   } else if (in == true && !hovering_in_progress_) {
+    3047             : 
+    3048           2 :     ROS_DEBUG("[MpcTracker]: starting the hover timer");
+    3049             : 
+    3050           2 :     hovering_in_progress_ = true;
+    3051             : 
+    3052           2 :     timer_hover_.start();
+    3053             :   }
+    3054           8 : }
+    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         686 : void MpcTracker::publishDiagnostics(void) {
+    3220             : 
+    3221        1372 :   auto des_x_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_x_trajectory_);
+    3222        1372 :   auto des_y_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_y_trajectory_);
+    3223        1372 :   auto des_z_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_z_trajectory_);
+    3224        1372 :   auto des_heading_trajectory = mrs_lib::get_mutexed(mutex_des_trajectory_, des_heading_trajectory_);
+    3225             : 
+    3226        1372 :   mrs_msgs::MpcTrackerDiagnostics diagnostics;
+    3227             : 
+    3228         686 :   diagnostics.header.stamp    = ros::Time::now();
+    3229         686 :   diagnostics.header.frame_id = uav_state_.header.frame_id;
+    3230             : 
+    3231         686 :   diagnostics.active = is_active_;
+    3232             : 
+    3233         686 :   diagnostics.uav_name = _uav_name_;
+    3234             : 
+    3235         686 :   diagnostics.collision_avoidance_active = collision_avoidance_enabled_;
+    3236         686 :   diagnostics.avoiding_collision         = collision_avoidance_affecting_me_;
+    3237             : 
+    3238         686 :   diagnostics.setpoint.position.x = des_x_trajectory(0, 0);
+    3239         686 :   diagnostics.setpoint.position.y = des_y_trajectory(0, 0);
+    3240         686 :   diagnostics.setpoint.position.z = des_z_trajectory(0, 0);
+    3241             : 
+    3242         686 :   diagnostics.setpoint.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(0, 0));
+    3243             : 
+    3244        1372 :   std::stringstream ss;
+    3245             : 
+    3246             :   {
+    3247        1372 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    3248             : 
+    3249             :     // fill in if other UAVs are sending their trajectories
+    3250         686 :     std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>::iterator u = other_uav_diagnostics_.begin();
+    3251             : 
+    3252         686 :     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        1372 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3268             : 
+    3269         686 :   if (ss.str().length() > 0) {
+    3270           0 :     ROS_DEBUG_STREAM_THROTTLE(5.0, "[MpcTracker]: getting avoidance trajectories: " << ss.str());
+    3271        1491 :   } else if (collision_avoidance_enabled_ &&
+    3272         805 :       (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    3273         567 :     ROS_DEBUG_THROTTLE(10.0, "[MpcTracker]: missing avoidance trajectories!");
+    3274             :   }
+    3275             : 
+    3276         686 :   pub_diagnostics_.publish(diagnostics);
+    3277             : 
+    3278        1372 :   std_msgs::String string_msg;
+    3279             : 
+    3280         686 :   if (diagnostics.avoidance_active_uavs.empty()) {
+    3281             : 
+    3282         686 :     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         686 :   if (diagnostics.avoidance_active_uavs.size() <= 3) {
+    3290             : 
+    3291         686 :     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         686 :   pub_status_string_.publish(string_msg);
+    3308         686 : }
+    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         675 : void MpcTracker::timerDiagnostics(const ros::TimerEvent& event) {
+    3348             : 
+    3349         675 :   if (!is_initialized_)
+    3350           0 :     return;
+    3351             : 
+    3352        2025 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.1, event);
+    3353        2025 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3354             : 
+    3355         675 :   publishDiagnostics();
+    3356             : }
+    3357             : 
+    3358             : //}
+    3359             : 
+    3360             : /* //{ timerMPC() */
+    3361             : 
+    3362        1047 : void MpcTracker::timerMPC(const ros::TimerEvent& event) {
+    3363             : 
+    3364        1047 :   if (odometry_reset_in_progress_) {
+    3365           0 :     ROS_ERROR("[MpcTracker]: mpc iteration tried run while reseting odometry");
+    3366           0 :     return;
+    3367             :   }
+    3368             : 
+    3369        1047 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    3370             : 
+    3371        1047 :   mrs_lib::AtomicScopeFlag unset_running(mpc_timer_running_);
+    3372             : 
+    3373        1047 :   bool started_with_invalid = mpc_result_invalid_;
+    3374             : 
+    3375        1047 :   if (!is_active_) {
+    3376           0 :     return;
+    3377             :   }
+    3378             : 
+    3379        1047 :   if (!is_initialized_) {
+    3380           0 :     return;
+    3381             :   }
+    3382             : 
+    3383        3141 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerMPC", 1.0 / dt1, 0.01, event);
+    3384        3141 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerMPC", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3385             : 
+    3386        1047 :   ros::Time     begin = ros::Time::now();
+    3387        1047 :   ros::Time     end;
+    3388        1047 :   ros::Duration interval;
+    3389             :   int           trajectory_id;
+    3390             : 
+    3391             :   // if we are tracking trajectory, copy the setpoint
+    3392        1047 :   if (trajectory_tracking_in_progress_) {
+    3393             : 
+    3394           0 :     MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    3395           0 :     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           0 :       std::scoped_lock lock(mutex_des_trajectory_, mutex_des_whole_trajectory_);
+    3400             : 
+    3401           0 :       des_x_trajectory       = des_x_trajectory_;
+    3402           0 :       des_y_trajectory       = des_y_trajectory_;
+    3403           0 :       des_z_trajectory       = des_z_trajectory_;
+    3404           0 :       des_heading_trajectory = des_heading_trajectory_;
+    3405             : 
+    3406           0 :       des_x_whole_trajectory       = *des_x_whole_trajectory_;
+    3407           0 :       des_y_whole_trajectory       = *des_y_whole_trajectory_;
+    3408           0 :       des_z_whole_trajectory       = *des_z_whole_trajectory_;
+    3409           0 :       des_heading_whole_trajectory = *des_heading_whole_trajectory_;
+    3410             : 
+    3411           0 :       trajectory_size = trajectory_size_;
+    3412           0 :       trajectory_dt   = trajectory_dt_;
+    3413             : 
+    3414           0 :       trajectory_id = des_whole_trajectory_id_;
+    3415             :     }
+    3416             : 
+    3417             :     /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    3418             : 
+    3419           0 :     int trajectory_tracking_sub_idx = trajectory_tracking_sub_idx_;
+    3420           0 :     int trajectory_tracking_idx     = trajectory_tracking_idx_;
+    3421             : 
+    3422           0 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3423             : 
+    3424           0 :       double first_time = dt1 + i * _dt2_ + trajectory_tracking_sub_idx * dt1;
+    3425             : 
+    3426           0 :       int first_idx  = trajectory_tracking_idx + int(floor(first_time / trajectory_dt));
+    3427           0 :       int second_idx = first_idx + 1;
+    3428             : 
+    3429           0 :       double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    3430             : 
+    3431           0 :       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           0 :         if (second_idx >= trajectory_size) {
+    3444           0 :           second_idx = trajectory_size - 1;
+    3445             :         }
+    3446             : 
+    3447           0 :         if (first_idx >= trajectory_size) {
+    3448           0 :           first_idx = trajectory_size - 1;
+    3449             :         }
+    3450             :       }
+    3451             : 
+    3452           0 :       des_x_trajectory(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory[first_idx] + interp_coeff * des_x_whole_trajectory[second_idx];
+    3453           0 :       des_y_trajectory(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory[first_idx] + interp_coeff * des_y_whole_trajectory[second_idx];
+    3454           0 :       des_z_trajectory(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory[first_idx] + interp_coeff * des_z_whole_trajectory[second_idx];
+    3455             : 
+    3456           0 :       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           0 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3461             : 
+    3462           0 :       des_x_trajectory_       = des_x_trajectory;
+    3463           0 :       des_y_trajectory_       = des_y_trajectory;
+    3464           0 :       des_z_trajectory_       = des_z_trajectory;
+    3465           0 :       des_heading_trajectory_ = des_heading_trajectory;
+    3466             :     }
+    3467             : 
+    3468             :     //}
+    3469             : 
+    3470             :     // increase the trajectory subsampling counter
+    3471             :     {
+    3472           0 :       std::scoped_lock lock(mutex_trajectory_tracking_states_);
+    3473             : 
+    3474           0 :       trajectory_tracking_sub_idx_++;
+    3475             :     }
+    3476             :   } else {
+    3477             : 
+    3478        1047 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3479             : 
+    3480        1047 :     trajectory_id = des_whole_trajectory_id_;
+    3481             :   }
+    3482             : 
+    3483        1047 :   manageConstraints();
+    3484             : 
+    3485        1047 :   calculateMPC();
+    3486             : 
+    3487        1047 :   end      = ros::Time::now();
+    3488        1047 :   interval = end - begin;
+    3489             : 
+    3490             :   // | ------------------ calculate the MPC RTF ----------------- |
+    3491             : 
+    3492        1047 :   mpc_rtf_ = 0.99 * mpc_rtf_ + 0.01 * (interval.toSec()/dt1);
+    3493             : 
+    3494        1047 :   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        2094 :     geometry_msgs::PoseArray debug_trajectory_out;
+    3502        1047 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    3503        1047 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    3504             : 
+    3505             :     {
+    3506        2094 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3507             : 
+    3508       42927 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3509             : 
+    3510       41880 :         geometry_msgs::Pose newPose;
+    3511             : 
+    3512       41880 :         newPose.position.x = predicted_trajectory_(i * _mpc_n_states_);
+    3513       41880 :         newPose.position.y = predicted_trajectory_(i * _mpc_n_states_ + 4);
+    3514       41880 :         newPose.position.z = predicted_trajectory_(i * _mpc_n_states_ + 8);
+    3515             : 
+    3516             :         try {
+    3517       41880 :           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       41880 :         debug_trajectory_out.poses.push_back(newPose);
+    3523             :       }
+    3524             :     }
+    3525             : 
+    3526        1047 :     ph_predicted_trajectory_debugging_.publish(debug_trajectory_out);
+    3527             :   }
+    3528             : 
+    3529             :   //}
+    3530             : 
+    3531             :   /* publish full state prediction //{ */
+    3532             : 
+    3533             :   {
+    3534        2094 :     mrs_msgs::MpcPredictionFullState prediction_fs_out;
+    3535        1047 :     prediction_fs_out.header.stamp    = ros::Time::now();
+    3536        1047 :     prediction_fs_out.header.frame_id = uav_state_.header.frame_id;
+    3537             : 
+    3538        1047 :     ros::Time stamp = prediction_fs_out.header.stamp;
+    3539             : 
+    3540        1047 :     prediction_fs_out.input_id = trajectory_id;
+    3541             : 
+    3542             :     {
+    3543        2094 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3544             : 
+    3545       42927 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3546             : 
+    3547       41880 :         if (i == 0) {
+    3548        1047 :           stamp += ros::Duration(0.01);
+    3549             :         } else {
+    3550       40833 :           stamp += ros::Duration(0.2);
+    3551             :         }
+    3552             : 
+    3553       41880 :         prediction_fs_out.stamps.push_back(stamp);
+    3554             : 
+    3555             :         {  // position
+    3556       41880 :           geometry_msgs::Point point;
+    3557             : 
+    3558       41880 :           point.x = predicted_trajectory_(i * _mpc_n_states_);
+    3559       41880 :           point.y = predicted_trajectory_(i * _mpc_n_states_ + 4);
+    3560       41880 :           point.z = predicted_trajectory_(i * _mpc_n_states_ + 8);
+    3561             : 
+    3562       41880 :           prediction_fs_out.position.push_back(point);
+    3563             :         }
+    3564             : 
+    3565             :         {  // velocity
+    3566       41880 :           geometry_msgs::Vector3 vector;
+    3567             : 
+    3568       41880 :           vector.x = predicted_trajectory_(i * _mpc_n_states_ + 1);
+    3569       41880 :           vector.y = predicted_trajectory_(i * _mpc_n_states_ + 5);
+    3570       41880 :           vector.z = predicted_trajectory_(i * _mpc_n_states_ + 9);
+    3571             : 
+    3572       41880 :           prediction_fs_out.velocity.push_back(vector);
+    3573             :         }
+    3574             : 
+    3575             :         {  // acceleration
+    3576       41880 :           geometry_msgs::Vector3 vector3;
+    3577             : 
+    3578       41880 :           vector3.x = predicted_trajectory_(i * _mpc_n_states_ + 2);
+    3579       41880 :           vector3.y = predicted_trajectory_(i * _mpc_n_states_ + 6);
+    3580       41880 :           vector3.z = predicted_trajectory_(i * _mpc_n_states_ + 10);
+    3581             : 
+    3582       41880 :           prediction_fs_out.acceleration.push_back(vector3);
+    3583             :         }
+    3584             : 
+    3585             :         {  // jerk
+    3586       41880 :           geometry_msgs::Vector3 vector3;
+    3587             : 
+    3588       41880 :           vector3.x = predicted_trajectory_(i * _mpc_n_states_ + 3);
+    3589       41880 :           vector3.y = predicted_trajectory_(i * _mpc_n_states_ + 7);
+    3590       41880 :           vector3.z = predicted_trajectory_(i * _mpc_n_states_ + 11);
+    3591             : 
+    3592       41880 :           prediction_fs_out.jerk.push_back(vector3);
+    3593             :         }
+    3594             : 
+    3595             :         {
+    3596             :           // heading
+    3597             : 
+    3598       41880 :           prediction_fs_out.heading.push_back(predicted_heading_trajectory_(i * _mpc_n_states_));
+    3599       41880 :           prediction_fs_out.heading_rate.push_back(predicted_heading_trajectory_(i * _mpc_n_states_ + 1));
+    3600       41880 :           prediction_fs_out.heading_acceleration.push_back(predicted_heading_trajectory_(i * _mpc_n_states_ + 2));
+    3601       41880 :           prediction_fs_out.heading_jerk.push_back(predicted_heading_trajectory_(i * _mpc_n_states_ + 3));
+    3602             :         }
+    3603             :       }
+    3604             :     }
+    3605             : 
+    3606             :     {
+    3607        2094 :       std::scoped_lock lock(mutex_prediction_full_state_);
+    3608        1047 :       prediction_full_state_ = prediction_fs_out;
+    3609             :     }
+    3610             :   }
+    3611             : 
+    3612             :   //}
+    3613             : 
+    3614        1047 :   mpc_computed_ = true;
+    3615             : 
+    3616        1047 :   if (started_with_invalid) {
+    3617             : 
+    3618           0 :     mpc_result_invalid_ = false;
+    3619             : 
+    3620           0 :     ROS_INFO("[MpcTracker]: calculated the first MPC result after invalidation");
+    3621             :   }
+    3622             : }
+    3623             : 
+    3624             : //}
+    3625             : 
+    3626             : /* timerTrajectoryTracking() //{ */
+    3627             : 
+    3628           0 : void MpcTracker::timerTrajectoryTracking(const ros::TimerEvent& event) {
+    3629             : 
+    3630           0 :   auto trajectory_size = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    3631           0 :   auto trajectory_dt   = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_dt_);
+    3632             : 
+    3633           0 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerTrajectoryTracking", int(1.0 / trajectory_dt), 0.01, event);
+    3634             :   mrs_lib::ScopeTimer timer =
+    3635           0 :       mrs_lib::ScopeTimer("MpcTracker::timerTrajectoryTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3636             : 
+    3637             :   {
+    3638           0 :     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           0 :     trajectory_tracking_sub_idx_ = 0;
+    3644             : 
+    3645             :     // INCREMENT THE TRACKING IDX
+    3646           0 :     trajectory_tracking_idx_++;
+    3647             : 
+    3648             :     // if the tracking idx hits the end of the trajectory
+    3649           0 :     if (trajectory_tracking_idx_ == trajectory_size) {
+    3650             : 
+    3651           0 :       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           0 :         trajectory_tracking_in_progress_ = false;
+    3661             : 
+    3662             :         // set the idx to the last idx of the trajectory
+    3663           0 :         trajectory_tracking_idx_ = trajectory_size - 1;
+    3664             : 
+    3665           0 :         timer_trajectory_tracking_.stop();
+    3666             : 
+    3667           0 :         ROS_INFO("[MpcTracker]: done tracking trajectory");
+    3668             :       }
+    3669             :     }
+    3670             :   }
+    3671             : 
+    3672           0 :   publishDiagnostics();
+    3673           0 : }
+    3674             : 
+    3675             : //}
+    3676             : 
+    3677             : /* timerVelocityTracking() //{ */
+    3678             : 
+    3679           0 : void MpcTracker::timerVelocityTracking(const ros::TimerEvent& event) {
+    3680             : 
+    3681           0 :   if (!is_initialized_) {
+    3682           0 :     return;
+    3683             :   }
+    3684             : 
+    3685           0 :   if (!velocity_tracking_active_) {
+    3686           0 :     return;
+    3687             :   }
+    3688             : 
+    3689           0 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerVelocityTracking", int(30.0), 0.01, event);
+    3690             :   mrs_lib::ScopeTimer timer =
+    3691           0 :       mrs_lib::ScopeTimer("MpcTracker::timerVelocityTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3692             : 
+    3693             :   // stop the timer when timeout
+    3694           0 :   if ((ros::Time::now() - velocity_reference_time_).toSec() > 0.5) {
+    3695             : 
+    3696           0 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: velocity reference timeouted, hovering");
+    3697           0 :     timer_velocity_tracking_.stop();
+    3698             : 
+    3699           0 :     toggleHover(true);
+    3700             : 
+    3701           0 :     velocity_tracking_active_ = false;
+    3702             : 
+    3703           0 :     return;
+    3704             :   }
+    3705             : 
+    3706           0 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3707           0 :   auto velocity_reference     = mrs_lib::get_mutexed(mutex_velocity_reference_, velocity_reference_);
+    3708             : 
+    3709           0 :   mrs_msgs::TrajectoryReference trajectory;
+    3710             : 
+    3711           0 :   trajectory.fly_now         = true;
+    3712           0 :   trajectory.use_heading     = true;
+    3713           0 :   trajectory.dt              = 0.2;
+    3714           0 :   trajectory.header.stamp    = ros::Time::now();
+    3715           0 :   trajectory.header.frame_id = "";
+    3716             : 
+    3717           0 :   double x       = mpc_x(0, 0);
+    3718           0 :   double y       = mpc_x(4, 0);
+    3719           0 :   double z       = mpc_x(8, 0);
+    3720           0 :   double heading = mpc_x_heading(0, 0);
+    3721             : 
+    3722           0 :   for (int i = 0; i < 50; i++) {
+    3723             : 
+    3724           0 :     mrs_msgs::Reference reference;
+    3725           0 :     reference.position.x = x;
+    3726           0 :     reference.position.y = y;
+    3727           0 :     reference.position.z = z;
+    3728           0 :     reference.heading    = heading;
+    3729             : 
+    3730           0 :     trajectory.points.push_back(reference);
+    3731             : 
+    3732           0 :     x += velocity_reference.velocity.x * trajectory.dt;
+    3733           0 :     y += velocity_reference.velocity.y * trajectory.dt;
+    3734           0 :     z += velocity_reference.velocity.z * trajectory.dt;
+    3735             : 
+    3736           0 :     if (velocity_reference.use_altitude) {
+    3737           0 :       z = velocity_reference.altitude;
+    3738             :     }
+    3739             : 
+    3740           0 :     if (velocity_reference.use_heading_rate) {
+    3741           0 :       heading += velocity_reference.heading_rate * trajectory.dt;
+    3742           0 :     } else if (velocity_reference.use_heading) {
+    3743           0 :       heading = velocity_reference.heading;
+    3744             :     }
+    3745             :   }
+    3746             : 
+    3747           0 :   auto [success, message, modified] = loadTrajectory(trajectory);
+    3748             : }
+    3749             : 
+    3750             : //}
+    3751             : 
+    3752             : /* //{ timerAvoidanceTrajectory() */
+    3753             : 
+    3754         134 : void MpcTracker::timerAvoidanceTrajectory(const ros::TimerEvent& event) {
+    3755             : 
+    3756         134 :   if (!is_active_) {
+    3757         112 :     return;
+    3758             :   }
+    3759             : 
+    3760          22 :   if (!is_initialized_) {
+    3761           0 :     return;
+    3762             :   }
+    3763             : 
+    3764          22 :   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          22 :     auto                     estimation_diag      = sh_estimation_diag_.getMsg();
+    3770          22 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    3771             : 
+    3772          22 :     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          22 :     bool got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    3774             : 
+    3775          22 :     if (!got_gps_est && !got_rtk_est) {
+    3776           0 :       return;
+    3777             :     }
+    3778             :   }
+    3779             : 
+    3780          44 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerAvoidanceTrajectory", _avoidance_trajectory_rate_, 0.1, event);
+    3781             :   mrs_lib::ScopeTimer timer =
+    3782          44 :       mrs_lib::ScopeTimer("MpcTracker::timerAvoidanceTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3783             : 
+    3784          22 :   auto uav_state            = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3785          22 :   auto predicted_trajectory = mrs_lib::get_mutexed(mutex_predicted_trajectory_, predicted_trajectory_);
+    3786             : 
+    3787          22 :   if (future_was_predicted_) {
+    3788             : 
+    3789          22 :     mrs_msgs::FutureTrajectory avoidance_trajectory;
+    3790             : 
+    3791             :     // fill last trajectory with initial data
+    3792          22 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3793          22 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3794          22 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3795          22 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk");
+    3796          22 :     avoidance_trajectory.points.clear();
+    3797          22 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3798          22 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3799          22 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3800          22 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_;
+    3801             : 
+    3802          44 :     auto res = common_handlers_->transformer->getTransform(uav_state.header.frame_id, "utm_origin", ros::Time::now());
+    3803             : 
+    3804          22 :     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          44 :       geometry_msgs::TransformStamped tf = res.value();
+    3814             : 
+    3815         902 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3816             : 
+    3817             :         // original point
+    3818        1760 :         geometry_msgs::PoseStamped original_point;
+    3819             : 
+    3820         880 :         original_point.header.stamp    = ros::Time::now();
+    3821         880 :         original_point.header.frame_id = uav_state.header.frame_id;
+    3822             : 
+    3823         880 :         original_point.pose.position.x = predicted_trajectory(i * _mpc_n_states_);
+    3824         880 :         original_point.pose.position.y = predicted_trajectory(i * _mpc_n_states_ + 4);
+    3825         880 :         original_point.pose.position.z = predicted_trajectory(i * _mpc_n_states_ + 8);
+    3826             : 
+    3827         880 :         original_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    3828             : 
+    3829        1760 :         auto res = common_handlers_->transformer->transform(original_point, tf);
+    3830             : 
+    3831         880 :         if (res) {
+    3832             : 
+    3833         880 :           mrs_msgs::FuturePoint new_point;
+    3834             : 
+    3835         880 :           new_point.x = res.value().pose.position.x;
+    3836         880 :           new_point.y = res.value().pose.position.y;
+    3837         880 :           new_point.z = res.value().pose.position.z;
+    3838             : 
+    3839         880 :           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          22 :     ph_avoidance_trajectory_.publish(avoidance_trajectory);
+    3851             :   }
+    3852             : }
+    3853             : 
+    3854             : //}
+    3855             : 
+    3856             : /* timerHover() //{ */
+    3857             : 
+    3858           5 : void MpcTracker::timerHover(const ros::TimerEvent& event) {
+    3859             : 
+    3860          10 :   MatrixXd mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    3861             : 
+    3862          15 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerHover", 10, 0.01, event);
+    3863          15 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerHover", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3864             : 
+    3865           5 :   setRelativeGoal(0, 0, 0, 0, false);
+    3866             : 
+    3867           5 :   if (fabs(mpc_x(1, 0)) < 0.1 && fabs(mpc_x(5, 0)) < 0.1 && fabs(mpc_x(9, 0)) < 0.1) {
+    3868             : 
+    3869           2 :     toggleHover(false);
+    3870             : 
+    3871           2 :     ROS_INFO("[MpcTracker]: timerHover: speed is low, stopping hover timer");
+    3872             :   }
+    3873           5 : }
+    3874             : 
+    3875             : //}
+    3876             : 
+    3877             : }  // namespace mpc_tracker
+    3878             : 
+    3879             : }  // namespace mrs_uav_trackers
+    3880             : 
+    3881             : #include <pluginlib/class_list_macros.h>
+    3882           3 : 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..3193eb789f91d643f522985239946248105636a7 GIT binary patch literal 12690 zcmV;DF>TI?P)ox~sVQ80RXde3;@n=>DZf{2KKxBGOz-{+tI zZoS$Mz7y%ahB5(GLCsx_RUbO2ISGQfE>~*7R$*pmvN}VoHST8pL=V8^&m5xD4FdwHokJ$3GY^eVWBM zor9eEAwKrEACw)J_86+W678uzzjVbo)i+o*pt0+Ds;U)Fb;rI3%(b7Esw{Tz8FXF2 zJ)rB!$@n>fJ{UbI$7Ijhbk7)2JBdj>Mubz*z#)H<02N(_{$Tfvg@drlGA;mKU_L|P ze8U5s?rCZ=Nct@XoMAV(>mtT50uwP5W55upnFV43Fqoer;5`GpG{%|Iu3?Yg2^4^n z7Vv;n0M34lS7#;;&ZD&;)b^i-Y#6TwW1;!s)d6QSAMtEV(>1~Xx=uTUo6xC_vB(*C zB;r};u{0Th@`NCax-m9&UGu2KKdVlMx~{{loEvNv@a}iwtNk7rq5XDjz;nhwyuWKO zhBa`0bUN{|rm-FH1Wtg{grHjhLDzGfelyWIA3X!1ld6G_S(^c?I6&re?c%|9jk+mz z&B5Bb239*S>dfdau8Dlc%6`R;2QC(4!H=UGqrL07*ZI{=ngW-bw02oxF?Q060OLJ< zbNA<;$-7R9fRpp~pI=$Pg&W6M224at7;ix{*D!iO z38S8vDVRubFE}?vI!17CDYIuJU;!2^uFJFRF-G>ByqE z;=W)b2HjIdk@4|fAPNJDz>wiJq)K8?6Sq738=aYN=^BufW4;bp-tZJ-45!xXM+;{L zcD!OECw6qTarwl~@$rLzcO&P{Pjt27a(|BoP>r$2@EpboxNhQ3IfdV}3B{?L`}w({ z;7<9M)-k4=unHinMVZg>aTl5$sxZdcSu7P4_o&k5^6h6@9j#+xFk-SNh7T5uT6YFS zTioenKxzT-fK1o1m(m4pe%5Ij;{}iA6#q*H*D0J{0b*tfn>&qNM+4IxhLWQqBZ%qx zBH3)$^F5r0+`lIvcMI5+>Nz7mcxz^^>r4ISU3=|~`wV~#)z|DoY*%Xmdw6F8<7#N* z-v|`a&(*^j zoj3yMzu~u~D_wIf^EYk^cp31PQGJc#6JthozWOgv9lt5YQ@~H$<<(E9l_ZwI zc-7%MMn6Uz)X1E*fLe@H+wg!_*%Pj6vsr3j&vb1_WsIe}K%{s|pw_OZU;(n<@nd9w z89O{X)5m{JV?;tRXUH3-0)b<7mAt&DBcgt%madEE6a%V=QR#_N8?sP+-0y5SgLRB! zbaW!u@42_wg5 zhVgfH1H5zI837REq=V0YD+7W6IZBL7V)~7NLAo{~2XX?eOJBloo34<;#?fhv&*$=l zZ4KWn`5$hNtUbl1@A~mv*XR1Ewp$$}0ER(4A{iCHH3G5&v39*=JmXL{5F^uJ#CWTz zo@5LI{Sy@*5LW=G#Hen5(fGYO-6YL^5N3>kkGZI$-?z>T#*?Prf{`EEf&pKzH)z}O zzwN#M{O8&$Gza+e_x)9Fw^t1!0Gg-po;Jc6f2jgM+s9u%#sO=lKOe_v3RTx?1+abzAAbS+ z0P5#jI@=yC9V5Tu0EyY-NL$_i4+DKUzHd;?^z??>IKUIOJjRk&Ew#l)_3_PuWCMHM z;utX?2DWugunAP_F-CS5EdUe7fQ`&!qI)FZ6Z$GEfK=8Xpqc6?ouP|+ArZ8^fv!cs z)z)<^uISuLvSTSbXE?hAGkj8LxlAk)zpbk<_^VS~#D&Yj2ayF{{c-DB1GuU&767&EVdQ(IJD6bO za$3rMeN~+l(Dk`^NUxp=c#L#-5wM9p9Iqpwk?Qb84%H>~siXea=Fy`WvOBqxPExx_-8N1EH zj>EFrF#c?<+a61&`wzjWN>OiAfb%vABA!L3N#!PI`#1FmI#7XV*Q&bB>2WO0J42GkxZuJVzD*?yEg ze~RiakRDE(sR950((^H@8BLkS2wXPYFwKDbyAF8RQ3|T#%q)xQGF}b1KI?$JFLHt< zu9-p&Ee|VTmFU*HIDTD4j1d4WupH0{1$OyS+v*gvy20hm>wtwUMbHjF)iQJvuCVvS zt~o$m*JmLrO`4&y=a1^1f>7YQVWq|`VO(>--s?K=xj05OpswY2>^}VAaQ(+0pH_@o zDYGk-*nG4u-JyU zD`?sCsLpa5M(`VP`@oxm1~6jmFvcgwdjaDmn}S6;u1+$SeE1a?T<#6*qE`n4JKA ziWq+>O#J3C$})^m+x6$}*b~DGF4yq^>7jh=klji4>e~O4m{Rj2&)AY~V> zY-+RF;p?7-(J>s)FLG7eTy{rBRUJ@`(RtX;LVKO6AUxts`^k35G4{4lj^O4elenaa zA$fy|N=mqG@?Xzevcph30p0Ffw(hfd3TdwnfVy}J7g+$RG6l+!qX<)793!RLh5YW` z&e(nS6{1DP_^~vtn|pRF-qWY&$mVo#I7;b9;i*Jvgr%U6$FWyze$fbHRCW!ckW=k6 zSm3}M*Zd%3Jc~iB;m5^kl9ssUigaL)W~R->F%l(_0oums52FP;;6c37IK7D*V}C%; z6fS&+0erG?J}vB-EA>*U#KbI(p@1J@&s=qLp{~_F`~HB1TL1&qI#=8S^t~1n5%V}Q zF*Z#fJg1D4B%7vqz(;@~CnmXa&wqZt4hU;ZVpe5%b(#Yo`vdA)oAf;0MVCf;Z+7@kAvMs*q9cxS7iLNRXH3jHBoELs+{ls6=nAi96vHNT}&CXEE)m5Ppk%b z{9FQ5QvFMenHbI|z!D<>wh(i+Oi}g0_~1@H?7>F~*c;>d+Ztf}t+H$O#PG?K6T$e{ zk`dy5G8bS_01hj_P^*}7<12#EeThrcVs|fKH5dK8f5pyKF-Y6iwa1tOjZtzMURo-^ zqg}^nCkBvHSIti~pd;rRMkO(SR8wrOY@4gA@fDgv>+dbqlR}9N*#yezyC#F(-^9qEE(X+tt}6U7FPf~qX*KR_uBkW4e2?t^8z1* z)Gkt)Gbg+$ougiE--z+04CijjL?-wUpcx}>pqB4m1kA&`dvu-i><!)B!k-Peh`1e z5(A4rKV+PWcQ>P>!{o;hf>1aUtbCIj0efcoa;GBvyKhkdseW{q=+f~ebRv&Grx z`-T*7n(O}pprPyYo||reyI-gPFs*);dVANZ$A+M1*k9?pK-U;kvAaK<)2+3FVqmFz zOk)l?7PB{}=8!7jU)ze$Zn59^u3?+d&nbW7P8xFjq2G;rmM})fPtxXm4l#S4>di^=J_<1XVVCyAHj8xk z`J@}jxeo<8ET=fFgj4?`q_@Fu2KVcdj9W0i&h)Bn+qq}UV(_R5<12kyZV`~P!(;!u z@eMJY?#2q-B|!F;ngR5%Z)zZ^D;FEqwo)F+R3EDVkH;oGfz%Q^17K~cTi=VL1w^Cb zXa*Fc5r{FoG4t#JhvZb~>W?U;hnqsor7$y&$jT(fz+E!iTfV)v0PYnl<$D;;l>5)e zZUfw$b+<=(-1bme<_)MGah3wLAxeN&_EhSim;tMGPzFoS9NENDoz~h$ifm1N2c0&WgsSU@|~(&G0!IG_y}Do72B+|njzrWGgskQV=k;h8vflk6;|mM zM5gtyZ%rQd8lV|td2>A_JkLVw*R{u50W7=15Ma%rVHxB9O~RAA{&ElcJ0!K38D=KF zDM`F+tr)HHSaS@i0WD+vzsh5WOU)s7nxdGYQmBS0{1r1vHyI#SF^Xub7oF z>V;yknDsCt|EQQzTO6icMY?tD@WNrB~C*y>K$SavmCss06L#%5i z0OtS&K+71VRKX|M3BDyLcIvtnN_!#VSX<&(-`9)8;iV~T#>mJl1hfinrzh4V!Odsv z%LKQV@D}1NGW)9ON<;DVjbOy(E|S0NVGugP3lY9FhwKW*csAoWHu7bl%7izvC)`DY zZ!ThFSuFxy&MwGLxrKa%!IySGgh|&>6pO^*;EGhz+jxx zNAz6}zIHh=labmn3Ve|OUuEA=17PLKu-8&Zv-%}VAzumMOZbJoL+pybzPu!MW2qcv z_ozZ*#_b9(q|FJ{kRb(fXpWD-&<0J>H`6XQw|jBhmnAJGdgA;`4R`X^2?D8ES=u>umN_ahy#BLz%Z?GT4mjk!(Kdxp$1tX&I z@)$q7T%-7-AXhsvOfYxg)82Mp$gD1oU+o5aHq?DEVB;8RH1ZxwZW}OG*6Zf0#ON_@ zwX7x&2&q9s*arjdnju=&46jq5QaR0JX6I6|$0NCxUZm71hT!78+Bwzw=l zyfBWU?XjgqW4&q~5b8cWF6DaAfH-v|zfe1cA22liU>n9r2S&O!VoW5xIa||a?FIX; z^d?beksfpz*r9>Y#PO`qoxRq@iyBGx@P8=?b<~3G)MK}lT!Q-P6{J2Xj=C0 zC2-x?_02K6$njTB_UE$=J55ZP#f?K|?Yvm(n}rz-Bb)Q$F>uAJld zo5e~wo8AftZuaS(N+j3E2<{=H+tge3n#h84h@mWY?DFPv>G0)F3^C z7{2RtQV53^$!b20-0*1UD$>ocM@oWLwl5)OgaWWBdp}UGYy(+q3!qgsXBeN6ui+OK zz>gX1C{HW7)|-u7@ceD07uW(K9|>sqsikyKS%6GW{;3P!XWo-N`YfOa&3Oy8fW2!< zr{|)eyYPoNQwHIH-0p?w=fcR=muO z)lC-e0EI8dj2OEfNk4j&$~Z0^G+WA`oVetdRxKDmdEC=tl+QEm7^7`gbEeY1>2dS4 znl;u3k8!1ss1C8ihZRDTPMYqZ`6p+Fai(6K8AkVLXQubI(EogW>wY++d=$EV{D03g zei4vEO8YT|EUSq7@3}50EZ1){xq3PG@oMEKzNxrsQHb1yn1bs=o(ET7Reo@cevs~i zV%@(081xPW^nbNl%>Wm*co%eRmH<~j#wwr#Mu01O`Le7O@Y?kZ3VysPb1*9G2&&* z0_L5%-&g<#SW9fnPJgQUzhda@C%D=!>7hVd#S1Lj{WeYuIPfFq$Cg{gekOq&0-}4AJvLECt~VFkl;2Hye&q z&y_<8|C;3^=D?=cdC1-3sR0xkpQPQZVam>dhA z9oM*hKsDZsk*OcfmY}$yM5%N**wRDCKKbJ9iUL-2P|K}7iUxp#eucca^3&G=H~Z+Q6y13LK3zQ7%>$Obd#|$ zQ7esO9DuO~;{a_l6J)ATSuz!htc)`N?yo4QV&QuwF+!Fd;i_efBVM7Oy%ikZCkyp7 zru#gHddGQXC1#m|b*%*)U~*+9f-@K{U4B}WJr)_30S3VSg{Oi^A1wQ0=6OE4o&8_| zvAXiz_2=hw8!z)w&G2AToIMwgKm!3Qv{=oV704oEZZ%*mJ8{f43)JyFX>~CdL-7-M zi`(ha-@}?wRt0|KM@`r}NxslgJvPjCO1X?N(zYp=EZsJTx)>mIw)sDxp=5>NSOSeE`X)DJvE?_7-lYtfbs_fKq@{QAbiU(J`Z;S zuIS3(8r2_sJ5^o^7;w;1DEfqq#N1r^cC4*2MXfi@Pz%QB+4>+qJToNVfX1i|3yE>} zKdDg*7Ma)kE){Dr0FH2dE|Gu!U%Kg70?9$~>6vrY+nZ=nG@Ss7$dt*3V`wl zKB}ayhQwgZpVU2rRZ2{O{;*2P3F@O#AlFB!6nTn4*NXAg05krS0l_p7p~&w9AYk|` z@nvFS{>H`Sn|+LaJVLBs!x&9=PYM_)QmrNC0XMh2$wjTrqltjt)-sjh``ycM9IEZ=eI ze9f|_$h;iznXbdjcf+CQa7!xz4zbM^)O#Lk9J)^4+0|P=akTI!8vT%JFz+E;Y1_0_ zfqxd5j*v;4WpV-zNm)+(k!QQ*^br#2=d9Fd>**skDP93xol~RlXR41g%rYDhHkvjl zg!U3ZhreJt2C5k(Fk2-8n$BF6#qTjH&^?Sk`*f2wb;&!oE7Hh7*NO~xTnCXuML=@W z4}fYuel*oqbLV5im>9HDfxU_m6msIm*dNe7Mn2d5Y^@l@seXJi(dL709CeQ9mt?2L zn}R+ViGo<2d|yZ#eD=>J%`yNxSSd&z#fZfmuwW8@GV$xs%q#d`fokO#>2h6|2`szp zmS>xT?Upj}YE&P^h~i~}Bm4N*bsK(hOTTxg8ZV9|K=*scNd=&y>l_ddx`Z(sbiHdo z=<8$r>i`waWv8ysM?_HoJYf3+K0nvB0q{qZ{`xTn*)IduTQr6lbs=FSE3XLnaDWsg z4_NNH+#oSkz|eJnK+P1^n_(O7Zx&iFa>|(q%hLjAV$Tv7vMkPlP0r>JF!3?g#7kKj zwrv=^I6TEX8v5d}b)t3&{`>I6Q*5B{edem) zL#AX%|4aVI1iFQ~4tUell0IK}l|2L}p-&8!1Q<|e$pqe52O{nH+{(I*7-=X>z zp1vdvWI_3Z-u;H(2A!jO+$&gjn)gR~?(z79HDR2lYGuwJn9e?kU;WrYdVZG(nyP5YYhsbXZ&{PcA%i2wi*6iGxuR8svJP0`azxw>Xf z{-n3J;nUk+vS$8GsvFh}A^JPk%s)@0`kY}&*d1z4ea*x{=oPi_L#z-~P07v;u8Bj}g zMs{iU>K&Ozh#}v(WoacQ_h0;wXGAu}EgvPj;h3f{BRhz~J1y7q z-*$|WRDAZB;@60u7guI=cD1p5)z!OWxQgFqp}5Cncd_)=3mt}uf#{WteR0K%1s^Rj zr^Pn=lCm$htE_ECp@8^+wE3gE`*+VU1|^;CJKlBJGTEW>8T=uCLILiSv*_r1A*lf7 z(uS#4vnLu|juFAcbBi!oRMZC`7+nFdYIK>Tr+YB4Fg093x_Ce<>4F!Ynm9B0PH&UAR<|+zM4%p+- zdZ;<)IeOVHctD&{G^Kz1|C$lL;+*+ue3AO<=i2xKfN#d4*=**FyO-GZgom{Ztp~^w zDQ?ii0XgObz%NiK>G;?ymh9!`_Fa2T!y-N2s;dI3F$P;NKP3pYXFCHL$H*Thw)ap# zvHdFN)I>1&7aSmp`)xY;x z#A0**p5IfuCz=6UMz;s7JDYCI~P-OVmAcEZaWIjv;D&aOOwzp(3h$Y&47q%wQ4V=gJ%X!KHxTJ8e6C zl*T(4u*=yw-NyH-t)l&W5=XLaXICYymVJx*HY&shf*(-Q6Ln zw_$=2jN=U${@Gip@Mep*OeW?8!W}BO(`1|U>^|ZnJb?*AoEq-App`YH>rcN`7#vD3 zrQu_~s`RJvbZr`AUo9TF=vkf~S+V#;)$e-_--8e>%aIfnE`=sph;gtUxVziJOxhPK z#mvm?AfCNEg={>#$sevIMs_Ped9fMnnmfqGU11jmycr|jVul0S$4JB>nM@aUSAMEL zdeT(r@VAwZnAUtoGWN7!WD^FOG48e}C?NY7YGq)MAr%3CtL>=-L|@uL7uV}wlvY`{2-k2_638w6+?qlS-v z(e@z|qokT(WaaWp4i$wJ)yAH8(sv+AoUDnPk~;?W42#f;?RuaV2!He;IUy4Y0WuE^ z_zd*(eUK@qY?X2LF<*^NFI@9S@0rHwM~?yTDDuIy>4c)l7dvlJw%_3S}CM_I`6H(@Mt{HvC0 zza`5kdv+z7*G#QuvVC2jOOTdI9t&t3Bh(b^yA2JRqFx)v8#`Osmim`8#YK$&3r*4J zq?Q`7-lT^&)l{ciBq6<4ir{l0A3iK~?hrnAEUx2artI}(hMv~K_pYdw*P+etbJbSM z6^x}vj13sq^W6gX3fxJpz13en`H3Tz^6_}m&P4~bhvxD#8C8TAGQwsS3oeKuf zt!vj;+#WX|wvg$Wwimw*7Xo*cRIBz6+I1m~Ro``yibjm}9m!;J4a^&kC8XYQbBeKX z!x&!Qkp`GtV>kAUjc8~8-Pn6!oI3C8x{ZCa-Zpz={WtcA`>MtzB(iz(V`CL+fO?Eu ztYlPHDMMCaz}AJBp-Kv=`YkKVVqnjZAZ^7V?rS16y1K4ZO3GS$Hcj$BcKXQMA5(iD zj?ttIAqRPyVwV8pNOxFYpVaRCF2UVW&4Bpld{W*_^%=_{bJ_pm*L3ZLaYEa|J=-U) zJ>${oArCIVkGsoliIY0-!)VNhyD)x^1$?sMCK1p;4F2%ZuCXUIKn&(N>KYNVLO^@h zJ}yNBY#^qHvYp`oHQ*Wpc>IzH%)8JvVELLeygy(wdoan1>!JZ88=}-QMzwC{UbG6W zv1=FE?26LI!j^aXRB3i=PdayuU2J@OoDn1l8C9ivj4!Tg3F8Maxw&uPt&7AQJY%n{ zZ<03{f8n~OV~i~yBdn-!Jf6W?0I&Xaua)g2Mbq^MORI$~j;gwRb83h!JoS?tU`S6$ zxRj=!+z<$a#>RF1p!@3rv{-0dK$RnR%@(jB$i~8O(s({98_s|z0O~QaSsG+A8={)d zS0_(gi)VHt#xb=%qdM{>f!<6O=8#xdJ}m~X9}~k*c|MjPx3-U2I5v}Tk+ao`A;XKs zo!dtspt&U2IRM5hwMUFfh7Wm?pew?c(s6dn_BX;5#77l-&;;&MQSCnw%}tiR7u@fN zR_y7TJsRVw3m1ZWqstByC}B*l_}2!9Z~FW@#vpCsyY2lEKwp7H?c7&WJy3c&+&#L+ zNDnJNBcvw)xev1X>Hz2?W=puARhoKF2<24~BG-E|fHlnzi&b#a?c zfa?8PKi9)j8#}4n8xb>B#2g>>49A`~0T9>a3U~1_HrR*38RT(M>ffDnB(dz&rETjxo+eoN-30uPU{7{MIOD>*;$HZ&DDT2IG_Sbr3-J zXI=^cpuQ=qS#$0ORyZuaW?+&PzQKUv_YN=^aHTL?w}DI= zXdK`hmrcTeW@2#f{uh4;5Yv-jTZob05YGYkc<8xaKOFbWVjU6JtSc37suhod%t)EK zNK6K}qcbF|;j8B3nCE#mPli~f-iQ%ZsYl}L@F&1>mHNi;`9hU?Yw|_Ljl!!ET8x^a z>ERkM4L=dY0BWxR)w_bGXgV0e9=E7J&JaO2F8(=Do@oB=B=ehUikJcdo8nWn0 z0JOLbt3x?@Un~~FpR&W8{nkD|T62|bMuth$#;7HxaGY<&xWX90O4rL$C`l=jmU}d}@Xr=+^}MOE4XHgFqo}7&7jpJ!MYLq8 zP|)SfNgV@79B0NUR8#X%Dj$pPIU`t>Wm$nFF2pfq!k9S>I|LU)j2Wj>p*|(xdWOr| z099i=no1o!a{iEbyD%7M47+opBB=^)nQ4jd@ATaCd%zJOqa|--3RQcTekmBnD2y;M z-H);R?mEhbORqb3h>U4dq~}`68JOmphyv^6=QJ*xE@8E#xLb)^r{Q5UQ=S_=@%( zCdvY~Zgx*ZZcuyS(a*VPDBNzP$}g_gLK>s4Yc60QmHpo0kF>-{hZl@1sIFjqmrheg zj2{zRW8Uza#(YC|H0KTP0X4%OGsfdb_MsTswFc0S>L@Rwz_^3cR1xr05+X6m<_hV) ze9lFoM8zn;H_&wgu!WeE3Q#Ia(zIMlWrp*0Et%HC0ln#IuQ0CwReS}Sen{xi<3f+C({l>*GfQr*I_NJC20$aR567gEfg7MEiy$@ zi=@P!^3`XUbbCn$_aj#iV2NYoYd@e7w3hEERK3(=L;-&3+g6PJ$R1OCeZ(feT%V*r z8gotpC07VoASMR{9SK3P2yi{$jHh54KK^yYJgq{(R8}F)s-!AWk`LS6usba5(AwHS zrJ7Kz?L|%%qS|9i#Y%uGMXGDJ{&`vtu1M_y1j)?sNB8L2+bt&Lm95wZrUvZo&BUNe z^?ZsI7wej@i>8pl0>(Alkop6*j*&~%FjHMoDz--s9!{y$LZzDn(;UH_Fk<%o89>_@ zxfyA}c`Af<)EM1ojaFo>=8=sX~{F@P%lP^+L{3H549%Ron<~9AP={(abtPw7* zH8S++E6Xn-Y+Jv$zM=TE&n%eh<;|Kfb_uLbbsSP-0eRWSv-*7g2h2ERA-WqFY5)KL M07*qoM6N<$g7yAF00000 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..f8d2330497 --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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 +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 19
<unnamed>13.6 %56 / 41131.6 %6 / 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..f4106fad7c --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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 +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 19
<unnamed>13.6 %56 / 41131.6 %6 / 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..780e57d44c --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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 +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 19
<unnamed>13.6 %56 / 41131.6 %6 / 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..82a200d91c --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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 +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 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..1cbc2b9b08 --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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 +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 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..7193cb3f93 --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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 +
13.6%13.6%
+ 		13.6 %56 / 41131.6 %6 / 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..f454f4c71e --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> 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::deactivate()1
mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1
(anonymous namespace)::ProxyExec0::ProxyExec0()3
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>)3
mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)13
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
+
+
+ + + +
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..6a7b3b98b6 --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()3
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()1
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>)3
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&)13
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&)1
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&)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::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)5071
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..a9b30e66f2 --- /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:5641113.6 %
Date:2023-11-30 22:31:42Functions:61931.6 %
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           3 : 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           3 :   this->common_handlers_  = common_handlers;
+     129           3 :   this->private_handlers_ = private_handlers;
+     130             : 
+     131           3 :   _uav_name_ = common_handlers->uav_name;
+     132             : 
+     133           3 :   nh_ = nh;
+     134             : 
+     135           3 :   ros::Time::waitForValid();
+     136             : 
+     137             :   // --------------------------------------------------------------
+     138             :   // |                     loading parameters                     |
+     139             :   // --------------------------------------------------------------
+     140             : 
+     141             :   // | ---------------- load parent's parameters ---------------- |
+     142             : 
+     143           6 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     144             : 
+     145           3 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     146             : 
+     147           3 :   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           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/speed_tracker.yaml");
+     155           3 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/speed_tracker.yaml");
+     156             : 
+     157           6 :   const std::string yaml_prefix = "mrs_uav_trackers/speed_tracker/";
+     158             : 
+     159           3 :   private_handlers->param_loader->loadParam(yaml_prefix + "command_timeout", _external_command_timeout_);
+     160             : 
+     161           3 :   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           3 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "SpeedTracker", _profiler_enabled_);
+     169             : 
+     170             :   // | ----------------------- subscribers ---------------------- |
+     171             : 
+     172           3 :   mrs_lib::SubscribeHandlerOptions shopts;
+     173           3 :   shopts.nh              = nh_;
+     174           3 :   shopts.node_name       = "SpeedTracker";
+     175           3 :   shopts.threadsafe      = true;
+     176           3 :   shopts.autostart       = true;
+     177           3 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     178             : 
+     179           3 :   sh_command_ = mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand>(shopts, "command", &SpeedTracker::callbackCommand, this);
+     180             : 
+     181             :   // | ----------------------- publishers ----------------------- |
+     182             : 
+     183           3 :   ph_rviz_marker_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "rviz_marker", 1);
+     184             : 
+     185             :   // | --------------------- finish the init -------------------- |
+     186             : 
+     187           3 :   is_initialized_ = true;
+     188             : 
+     189           3 :   ROS_INFO("[SpeedTracker]: initialized");
+     190             : 
+     191           3 :   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           1 : void SpeedTracker::deactivate(void) {
+     236             : 
+     237           1 :   is_active_ = false;
+     238             : 
+     239           1 :   ROS_INFO("[SpeedTracker]: deactivated");
+     240           1 : }
+     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        5071 : 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       15213 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     259       15213 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     260             : 
+     261             :   {
+     262        5071 :     std::scoped_lock lock(mutex_uav_state_);
+     263             : 
+     264        5071 :     uav_state_ = uav_state;
+     265             : 
+     266        5071 :     got_uav_state_ = true;
+     267             :   }
+     268             : 
+     269             :   double uav_heading;
+     270             : 
+     271             :   try {
+     272        5071 :     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        5071 :   if (!is_active_) {
+     282        5071 :     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           1 : const std_srvs::SetBoolResponse::ConstPtr SpeedTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     381             : 
+     382           2 :   std_srvs::SetBoolResponse res;
+     383           2 :   std::stringstream         ss;
+     384             : 
+     385           1 :   if (cmd->data != callbacks_enabled_) {
+     386             : 
+     387           0 :     callbacks_enabled_ = cmd->data;
+     388             : 
+     389           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     390           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     391             : 
+     392             :   } else {
+     393             : 
+     394           1 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     395           1 :     ROS_WARN_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     396             :   }
+     397             : 
+     398           1 :   res.message = ss.str();
+     399           1 :   res.success = true;
+     400             : 
+     401           2 :   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          13 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr SpeedTracker::setConstraints([
+     459             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     460             : 
+     461             :   {
+     462          13 :     std::scoped_lock lock(mutex_constraints_);
+     463             : 
+     464          13 :     constraints_ = cmd->constraints;
+     465             :   }
+     466             : 
+     467          26 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     468             : 
+     469          13 :   res.success = true;
+     470          13 :   res.message = "constraints updated";
+     471             : 
+     472          26 :   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           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr SpeedTracker::setTrajectoryReference([
+     498             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     499           0 :   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           3 : 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 + + + + + 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..b6e59e0c3c6420af7b41f2d34abdf45c5df85907 GIT binary patch literal 3032 zcmV;}3n%o6P)Wz0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp$C1x zQk)MaXFbBZj8<|jnq#C^6?(El&+TBr1`4}g=H|HZhSBSLfMvYz{@c~`W z)*~$oDipmayS10Z-b)7L8Zid5CH^ep+YNr8+C+kj9tQb ziW6m%1i1IW0^Zby^>kyd3q=&kHh0-5VX;h%TmTnQoP(8*k4ML?3bWAMmPI=)!qbX)ID-^TPWO2aSbp}$$m$_e!v+C00)xN*)-${GKpYVUC zue5zrjo&q-@%0G56)gci0`P$VnaxqWC!&}1WE5YVVI)rSNUKFoaY92K9N5v%qM5=p z4BQm}nH1R#PMWUp4C9Y{&;UJVuAaW%Ynhx*bS4Q!oV9xnT-PUT+x~ZZn%5uOR@W@> z_1|W#w`px5Jn(mzu-XJbfvs)Caj%bpmIn$bQVmeQi+H3Q`=E4L@5v|3Gv0$34w&`$ zDGH>JrEHO+;^FsiH0C>D>_1-f7dhiZiWUfcl>k00kWH}-{LbgtlnLpWp==o2ITM~+ zUwf%rT=w$l7Z|e=kF{i_Gn#qHPgAUTR|GP$Eem9~A=_8ZwX`UL;QeamSvGV0op^zH zkKyry)o3oExCJ&F6rY=MUDsxteCFCeVQeP`0mj}-L;$MKTtMoo;8E%gPP&(sI(0*n zx+LV0+qK{6b|IM@ltAGo9|#+;g;)6g)>iYh>nQw?uR)R5u^p}^vlay`v}~}?$zqRJ zBTC8xnQ9d1aa^z;S)IWry`IWPefk`b0kG$5cGm+GvxA9Vq&fzHm1!Y^1&tqgK8 zMN6{^WX2SM``mi>lxT{~b=ntmmCKiUwTppvq)76fkV?7w&@=onf*@t16azS-g0$>s z8etAO``wq>?b5vvv2K=^Sy6wyGnSu1OikyINgD>`@aFNssQ#Ie2bd5`z3 zPJn#Y&hyib^|e`Z zaZQ52j{?cr###mpWVtZeJTV^F5QemHRTy0E@xVhE3_99zD|&)F8ZZDG!r)`y3nRBo z?79J@#=h^G(eL^L0bosrYXzw9;c3^f`}g`|C=J!OT`p9{(JPxx>E2c{4JIzvm%4g- zxUD-VT0+jxs$B;^!-s;P>kXk$059b|Y7Ugw{Cq(zf2Y69-3A`DfZ7QSe%H3N1}b)F z#ez=}guRX4yb}3i*hJcrgetyTKP@6|tTv^4NrHmLUe?iYn$TWmyFXlw^kGYKK=t?V zI_2kJ9}#4K4VFNJudAH{SkAVBEc12DE)#8AC(qj8fPio-UXl)o@o3c3eRuFqz~Ka zx>g;?>8wjJbz=tTI{nbPrO|=}AiHbV((^lrFwp6Wb3%Pmm;MTPFcHY5Xb<=?Oqkp* zsPf1bCa=0$oz{RWwILE^Hieu(|HefM|{L7`{WwqV|FjL`(o@`4=J0g{gxpG zhz_;T8y|D;?Q^rY2{Wl5C9m;l8_*+{M{Z-vvJTh?H>BXJo^qjw zO|W~z%r+tUIvQJ$+8%y4e=5@+%zce%Q;Z(|#yz9~3QvheK?&eb)WfMu(0V`tkH4lK zj@Pe;054S!cWLwa6EB_haKhpGSDH_?Y9VnI-@kjZ@{!Q>9E*Uzf4L{1nEB&!+AB}g z!_ll|MNLzh$OAbW-AwP9x(ny|NZ6!ZXF`{CisQXlr@#v5$qjD3G2_J(0sq{X(G1}~ zQaH!rsj5>1iodCF{`;i9pocH#oqTlR95Ko!-a2m_7ucdx^mvb`a85^DZr9lg5REf*JDOu zd}vbTbS}4RYDdGniE&@=(Nk7L&xIul=Td}^Ou}~!_9mf7?>|63hwwP4I^*_^mav(G z>>-iYxsn0WT}vVM#QINO5d@_?4&98e^QfIFj0h=U*D?GSt|vmE|01b*PC>!v4tN~@ z3_UUxP8_j2^dIznB2{=2&z>9t?B$W5IRwb$k$ppCZEb0~#*dFH^M=Sf3+I(MkzQFI zumdT|0IV-W?r`-mdCafwRi1FFEwv!_-jaR7?u%Gj0UVeE1pTwe{F)=74f0hv>;_2ih(Qz?4ej$wL_o*H!CTM zPICc>ZtrKn29L=zU7?i<9t{fYIsOEe04h#46$M2i$kw7NJVUK@Ne3&JsN9>;J5a1uOOi~$U38M&PN)0JC z9x?JpAg?7(Q7KJJzeB8WQw(^4Q=cJ!Y|ZW?Pb+@X!cSW0-RzSVs?htSg<3$SdajUs zyeTcT_lEU>f5M8ygjV)*{>h~0000?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
+ Top

+ Overview +