Back to home page

EIC code displayed by LXR

 
 

    


File indexing completed on 2026-05-27 07:24:22

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008 
0009 // Project include(s)
0010 #include "detray/navigation/caching_navigator.hpp"
0011 
0012 #include "detray/definitions/indexing.hpp"
0013 #include "detray/propagator/line_stepper.hpp"
0014 #include "detray/tracks/tracks.hpp"
0015 
0016 // Detray test include(s)
0017 #include "detray/test/common/build_toy_detector.hpp"
0018 #include "detray/test/common/build_wire_chamber.hpp"
0019 #include "detray/test/utils/inspectors.hpp"
0020 
0021 // Test include(s)
0022 #include "detray/test/framework/types.hpp"
0023 
0024 // VecMem include(s).
0025 #include <vecmem/memory/host_memory_resource.hpp>
0026 
0027 // GoogleTest include(s)
0028 #include <gtest/gtest.h>
0029 
0030 // System include(s)
0031 #include <map>
0032 
0033 namespace detray {
0034 
0035 namespace {
0036 
0037 constexpr std::size_t cache_size{navigation::default_cache_size};
0038 
0039 // dummy propagator state
0040 template <typename stepping_t, typename navigation_t>
0041 struct prop_state {
0042   using context_t = typename navigation_t::detector_type::geometry_context;
0043   stepping_t m_stepping;
0044   navigation_t m_navigation;
0045   context_t m_context{};
0046 
0047   constexpr context_t &context() { return m_context; }
0048   constexpr navigation_t &navigation() { return m_navigation; }
0049   constexpr stepping_t &stepping() { return m_stepping; }
0050 };
0051 
0052 /// Checks for a correct 'towards_surface' state
0053 template <typename navigator_t, typename state_t = typename navigator_t::state>
0054 inline void check_towards_surface(state_t &state, dindex vol_id,
0055                                   std::size_t n_candidates, dindex next_id) {
0056   ASSERT_EQ(state.status(), navigation::status::e_towards_object);
0057   ASSERT_EQ(state.volume(), vol_id);
0058   ASSERT_EQ(state.n_candidates(), std::min(n_candidates, cache_size - 1));
0059   // the portal is still the next object, since we did not step
0060   ASSERT_EQ(state.next_surface().index(), next_id);
0061   ASSERT_TRUE((state.trust_level() == navigation::trust_level::e_full) ||
0062               (state.trust_level() == navigation::trust_level::e_high));
0063 }
0064 
0065 /// Checks for a correct 'on_surface' state
0066 template <typename navigator_t, typename state_t = typename navigator_t::state>
0067 inline void check_on_surface(state_t &state, dindex vol_id,
0068                              std::size_t n_candidates, dindex current_id,
0069                              dindex next_id) {
0070   // The status is: on surface/towards surface if the next candidate is
0071   // immediately updated and set in the same update call
0072   ASSERT_TRUE(state.status() == navigation::status::e_on_object ||
0073               state.status() == navigation::status::e_on_portal);
0074   // Points towards next candidate
0075   ASSERT_TRUE(std::abs(state()) >= 1.f * unit<test::scalar>::um);
0076   ASSERT_EQ(state.volume(), vol_id);
0077   ASSERT_EQ(state.n_candidates(), std::min(n_candidates, cache_size - 1));
0078   ASSERT_EQ(state.geometry_identifier().volume(), vol_id);
0079   ASSERT_EQ(state.geometry_identifier().index(), current_id);
0080   // points to the next surface now
0081   ASSERT_EQ(state.next_surface().index(), next_id);
0082   ASSERT_EQ(state.trust_level(), navigation::trust_level::e_full);
0083 }
0084 
0085 /// Checks for a correctly handled volume switch
0086 template <typename navigator_t, typename state_t = typename navigator_t::state>
0087 inline void check_volume_switch(state_t &state, dindex vol_id) {
0088   // Switched to next volume
0089   ASSERT_EQ(state.volume(), vol_id);
0090   // The status is towards first surface in new volume
0091   ASSERT_EQ(state.status(), navigation::status::e_on_portal);
0092   // Kernel is newly initialized
0093   ASSERT_FALSE(state.cache_exhausted());
0094   ASSERT_EQ(state.trust_level(), navigation::trust_level::e_full);
0095 }
0096 
0097 /// Checks an entire step onto the next surface
0098 template <typename navigator_t, typename stepper_t, typename prop_state_t,
0099           typename context_t>
0100 inline void step_and_check(navigator_t &nav, stepper_t &stepper,
0101                            prop_state_t &propagation,
0102                            const navigation::config &nav_cfg,
0103                            const stepping::config &step_cfg,
0104                            const context_t &ctx, dindex vol_id,
0105                            std::size_t n_candidates, dindex current_id,
0106                            dindex next_id) {
0107   auto &navigation = propagation.navigation();
0108   auto &stepping = propagation.stepping();
0109 
0110   // Step onto the surface in volume
0111   stepper.step(navigation(), stepping, step_cfg);
0112   navigation.set_high_trust();
0113   // Stepper reduced trust level
0114   ASSERT_TRUE(navigation.trust_level() == navigation::trust_level::e_high);
0115   nav.update(stepping(), navigation, nav_cfg, ctx);
0116   ASSERT_TRUE(navigation.is_alive());
0117   // Trust level is restored
0118   ASSERT_EQ(navigation.trust_level(), navigation::trust_level::e_full);
0119   // The status is on surface
0120   check_on_surface<navigator_t>(navigation, vol_id, n_candidates, current_id,
0121                                 next_id);
0122 }
0123 
0124 }  // anonymous namespace
0125 
0126 }  // namespace detray
0127 
0128 /// This tests the construction and general methods of the navigator
0129 GTEST_TEST(detray_navigation, navigator_toy_geometry) {
0130   using namespace detray;
0131   using namespace detray::navigation;
0132 
0133   using test_algebra = test::algebra;
0134   using scalar = test::scalar;
0135   using point3 = test::point3;
0136   using vector3 = test::vector3;
0137 
0138   vecmem::host_memory_resource host_mr;
0139 
0140   /// Tolerance for tests
0141   constexpr double tol{0.01};
0142 
0143   auto [toy_det, names] = build_toy_detector<test_algebra>(host_mr);
0144 
0145   using detector_t = decltype(toy_det);
0146   using inspector_t = navigation::print_inspector;
0147   using navigator_t = caching_navigator<detector_t, cache_size, inspector_t>;
0148   using constraint_t = constrained_step<scalar>;
0149   using stepper_t = line_stepper<test_algebra, constraint_t>;
0150 
0151   // State type in the nominal navigation (no inspectors)
0152   using nav_stat_t = caching_navigator<detector_t, cache_size>::state;
0153 
0154   // Check memory layout of intersection struct (currently 224)
0155   constexpr std::size_t offset{navigation::default_cache_size *
0156                                sizeof(typename nav_stat_t::value_type)};
0157   // Align to GPU read boundaries
0158   static_assert(offset % 32 == 0);
0159 
0160   // Private members cannot be accessed this way, but keep for debugging
0161   /*static_assert(offsetof(nav_stat_t, m_detector) == offset);
0162   static_assert(offsetof(nav_stat_t, m_status) == offset + 8);
0163   static_assert(offsetof(nav_stat_t, m_trust_level) == offset + 9);
0164   static_assert(offsetof(nav_stat_t, m_direction) == offset + 10);
0165   static_assert(offsetof(nav_stat_t, m_heartbeat) == offset + 11);
0166   static_assert((offsetof(nav_stat_t, m_external_mask_tol) == offset + 12) ||
0167                 (offsetof(nav_stat_t, m_external_mask_tol) == offset + 16));
0168   // 16-byte alignment for the candidate indices
0169   static_assert((offsetof(nav_stat_t, m_next) == offset + 16) ||
0170                 (offsetof(nav_stat_t, m_next) == offset + 24));
0171   static_assert((offsetof(nav_stat_t, m_last) == offset + 17) ||
0172                 (offsetof(nav_stat_t, m_last) == offset + 25));
0173   static_assert((offsetof(nav_stat_t, m_volume_index) == offset + 18) ||
0174                 (offsetof(nav_stat_t, m_volume_index) == offset + 26));*/
0175 
0176   // 240 bytes for single precision + padding due to overalignment = 256 bytes
0177   static_assert((sizeof(nav_stat_t) <= 256) || (sizeof(nav_stat_t) <= 384));
0178 
0179   // test track
0180   point3 pos{0.f, 0.f, 0.f};
0181   vector3 mom{1.f, 1.f, 0.f};
0182   free_track_parameters<test_algebra> traj(pos, 0.f, mom, -1.f);
0183 
0184   stepper_t stepper;
0185   navigator_t nav;
0186   navigation::config nav_cfg{};
0187   nav_cfg.search_window = {3u, 3u};
0188 
0189   stepping::config step_cfg{};
0190 
0191   prop_state<stepper_t::state, navigator_t::state> propagation{
0192       stepper_t::state{traj}, navigator_t::state(toy_det)};
0193   navigator_t::state &navigation = propagation.navigation();
0194   stepper_t::state &stepping = propagation.stepping();
0195   const auto &ctx = propagation.context();
0196 
0197   // Check that the state is uninitialized
0198   // Default volume is zero
0199   ASSERT_EQ(navigation.volume(), 0u);
0200   // No surface candidates
0201   ASSERT_EQ(navigation.n_candidates(), 0u);
0202   // You can not trust the state
0203   ASSERT_EQ(navigation.trust_level(), trust_level::e_no_trust);
0204   // The status is unknown
0205   ASSERT_EQ(navigation.status(), status::e_unknown);
0206 
0207   //
0208   // beampipe
0209   //
0210 
0211   // Initialize navigation
0212   // Test that the navigator has a heartbeat
0213   nav.init(stepping(), navigation, nav_cfg, ctx);
0214   ASSERT_TRUE(navigation.is_alive());
0215   // The status is towards beampipe
0216   // Two candidates: beampipe and portal
0217   // First candidate is the beampipe
0218   check_towards_surface<navigator_t>(navigation, 0u, 2u, 0u);
0219   // Distance to beampipe surface
0220   ASSERT_NEAR(navigation(), 19.f, tol);
0221 
0222   // Let's make half the step towards the beampipe
0223   stepping.template set_constraint<step::constraint::e_user>(navigation() *
0224                                                              0.5f);
0225   stepper.step(navigation(), stepping, step_cfg);
0226   // Navigation policy might reduce trust level to fair trust
0227   navigation.set_fair_trust();
0228   // Release user constraint again
0229   stepping.template release_step<step::constraint::e_user>();
0230   ASSERT_TRUE(navigation.trust_level() == trust_level::e_fair);
0231   // Re-navigate
0232   nav.update(stepping(), navigation, nav_cfg, ctx);
0233   ASSERT_TRUE(navigation.is_alive());
0234   // Trust level is restored
0235   ASSERT_EQ(navigation.trust_level(), trust_level::e_full);
0236   // The status remains: towards surface
0237   check_towards_surface<navigator_t>(navigation, 0u, 2u, 0u);
0238   // Distance to beampipe is now halved
0239   ASSERT_NEAR(navigation(), 9.5f, tol);
0240 
0241   // Let's immediately update, nothing should change, as there is full trust
0242   nav.update(stepping(), navigation, nav_cfg, ctx);
0243   ASSERT_TRUE(navigation.is_alive());
0244   check_towards_surface<navigator_t>(navigation, 0u, 2u, 0u);
0245   ASSERT_NEAR(navigation(), 9.5f, tol);
0246 
0247   // Now step onto the beampipe (idx 0)
0248   step_and_check(nav, stepper, propagation, nav_cfg, step_cfg, ctx, 0u, 1u, 0u,
0249                  3u);
0250   // New target: Distance to the beampipe volume cylinder portal
0251   ASSERT_NEAR(navigation(), 6.f, tol);
0252 
0253   // Step onto portal 7 in volume 0
0254   stepper.step(navigation(), stepping, step_cfg);
0255   navigation.set_high_trust();
0256   ASSERT_TRUE(navigation.trust_level() == trust_level::e_high);
0257   nav.update(stepping(), navigation, nav_cfg, ctx);
0258   ASSERT_TRUE(navigation.is_alive()) << navigation.inspector().to_string();
0259   ASSERT_EQ(navigation.trust_level(), trust_level::e_full);
0260   ASSERT_EQ(navigation.volume(), 8u);
0261 
0262   //
0263   // barrel
0264   //
0265 
0266   // Last volume before we leave world
0267   dindex last_vol_id = 15u;
0268 
0269   // maps volume id to the sequence of surfaces that the navigator encounters
0270   std::vector<std::pair<dindex, std::vector<dindex>>> sf_sequences;
0271 
0272   // gap 1
0273   sf_sequences.emplace_back(8u, std::vector<dindex>{582u, 583u});
0274   // layer 1
0275   sf_sequences.emplace_back(
0276       7u, std::vector<dindex>{578u, 475u, 459u, 476u, 460u, 579u});
0277   // gap 2
0278   sf_sequences.emplace_back(10u, std::vector<dindex>{1038u, 1039u});
0279   // layer 2
0280   sf_sequences.emplace_back(
0281       9u, std::vector<dindex>{1034u, 829u, 797u, 830u, 798u, 1035u});
0282   // gap 3
0283   sf_sequences.emplace_back(12u, std::vector<dindex>{1774u, 1775u});
0284   // layer 3
0285   sf_sequences.emplace_back(11u,
0286                             std::vector<dindex>{1770u, 1438u, 1386u, 1771u});
0287   // gap 4
0288   sf_sequences.emplace_back(14u, std::vector<dindex>{2874u, 2875u});
0289   // layer 4
0290   sf_sequences.emplace_back(13u,
0291                             std::vector<dindex>{2870u, 2372u, 2294u, 2871u});
0292   // gap 5
0293   sf_sequences.emplace_back(last_vol_id, std::vector<dindex>{2878u, 2879u});
0294 
0295   // Every iteration steps through one barrel layer
0296   for (const auto &[vol_id, sf_seq] : sf_sequences) {
0297     // Exclude the portal we are already on
0298     std::size_t n_candidates = sf_seq.size() - 1u;
0299 
0300     // Test the copy constructor of the propagation state
0301     auto propagation_cpy{propagation};
0302     navigator_t::state &navigation_cpy = propagation_cpy.navigation();
0303     stepper_t::state &stepping_cpy = propagation_cpy.stepping();
0304 
0305     // We switched to next barrel volume
0306     check_volume_switch<navigator_t>(navigation_cpy, vol_id);
0307 
0308     // The status is: on adjacent portal in volume, towards next candidate
0309     check_on_surface<navigator_t>(navigation_cpy, vol_id, n_candidates,
0310                                   sf_seq[0], sf_seq[1]);
0311 
0312     // Step through the module surfaces
0313     for (std::size_t sf = 1u; sf < sf_seq.size() - 1u; ++sf) {
0314       // Count only the currently reachable candidates
0315       step_and_check(nav, stepper, propagation_cpy, nav_cfg, step_cfg, ctx,
0316                      vol_id, n_candidates - sf, sf_seq[sf], sf_seq[sf + 1u]);
0317     }
0318 
0319     // Step onto the portal in volume
0320     stepper.step(navigation_cpy(), stepping_cpy, step_cfg);
0321     navigation_cpy.set_high_trust();
0322 
0323     // Check against last volume
0324     if (vol_id == last_vol_id) {
0325       nav.update(stepping_cpy(), navigation_cpy, nav_cfg, ctx);
0326       ASSERT_FALSE(navigation_cpy.is_alive());
0327       // The status is: exited
0328       ASSERT_EQ(navigation_cpy.status(), status::e_exit);
0329       // Keep current volume id, so that nav. direction can be reversed
0330       ASSERT_EQ(last_vol_id, navigation_cpy.volume());
0331     } else {
0332       nav.update(stepping_cpy(), navigation_cpy, nav_cfg, ctx);
0333       ASSERT_TRUE(navigation_cpy.is_alive());
0334     }
0335 
0336     // Update the propagation state with current step (test assignment op)
0337     propagation = propagation_cpy;
0338   }
0339 
0340   std::clog << detray::navigation::print_state(navigation) << std::endl;
0341   std::clog << detray::navigation::print_candidates(navigation, nav_cfg,
0342                                                     traj.pos(), traj.dir())
0343             << std::endl;
0344 
0345   // Leave for debugging
0346   // std::clog << navigation.inspector().to_string() << std::endl;
0347   ASSERT_TRUE(navigation.finished()) << navigation.inspector().to_string();
0348 }
0349 
0350 GTEST_TEST(detray_navigation, navigator_wire_chamber) {
0351   using namespace detray;
0352   using namespace detray::navigation;
0353 
0354   using test_algebra = test::algebra;
0355   using scalar = test::scalar;
0356   using point3 = test::point3;
0357   using vector3 = test::vector3;
0358 
0359   vecmem::host_memory_resource host_mr;
0360 
0361   /// Tolerance for tests
0362   constexpr double tol{0.01};
0363 
0364   constexpr std::size_t n_layers{10};
0365   wire_chamber_config<scalar> wire_cfg{};
0366   auto [wire_det, names] = build_wire_chamber<test_algebra>(host_mr, wire_cfg);
0367 
0368   using detector_t = decltype(wire_det);
0369   using inspector_t = navigation::print_inspector;
0370   using navigator_t = caching_navigator<detector_t, cache_size, inspector_t>;
0371   using constraint_t = constrained_step<scalar>;
0372   using stepper_t = line_stepper<test_algebra, constraint_t>;
0373 
0374   // test track
0375   point3 pos{0.f, 0.f, 0.f};
0376   vector3 mom{0.f, 1.f, 0.f};
0377   free_track_parameters<test_algebra> traj(pos, 0.f, mom, -1.f);
0378 
0379   stepper_t stepper;
0380   navigator_t nav;
0381   navigation::config nav_cfg{};
0382   nav_cfg.intersection.mask_tolerance_scalor = 1e-2f;
0383   nav_cfg.search_window = {3u, 3u};
0384 
0385   stepping::config step_cfg{};
0386 
0387   prop_state<stepper_t::state, navigator_t::state> propagation{
0388       stepper_t::state{traj}, navigator_t::state(wire_det)};
0389   navigator_t::state &navigation = propagation.navigation();
0390   stepper_t::state &stepping = propagation.stepping();
0391   const auto &ctx = propagation.context();
0392 
0393   // Check that the state is uninitialized
0394   // Default volume is zero
0395   ASSERT_EQ(navigation.volume(), 0u);
0396   // No surface candidates
0397   ASSERT_EQ(navigation.n_candidates(), 0u);
0398   // You can not trust the state
0399   ASSERT_EQ(navigation.trust_level(), trust_level::e_no_trust);
0400   // The status is unknown
0401   ASSERT_EQ(navigation.status(), status::e_unknown);
0402 
0403   //
0404   // Beam Collision region
0405   //
0406 
0407   // Initialize navigation
0408   // Test that the navigator has a heartbeat
0409   nav.init(stepping(), navigation, nav_cfg, ctx);
0410   ASSERT_TRUE(navigation.is_alive());
0411   // The status is towards portal
0412   // One candidates: barrel cylinder portal
0413   check_towards_surface<navigator_t>(navigation, 0u, 1u, 0u);
0414   // Distance to portal
0415   ASSERT_NEAR(navigation(), 500.f * unit<scalar>::mm, tol);
0416 
0417   // Let's make half the step towards the portal
0418   stepping.template set_constraint<step::constraint::e_user>(navigation() *
0419                                                              0.5f);
0420   stepper.step(navigation(), stepping, step_cfg);
0421   // Navigation policy might reduce trust level to fair trust
0422   navigation.set_fair_trust();
0423   // Release user constraint again
0424   stepping.template release_step<step::constraint::e_user>();
0425   ASSERT_TRUE(navigation.trust_level() == trust_level::e_fair);
0426   // Re-navigate
0427   nav.update(stepping(), navigation, nav_cfg, ctx);
0428   ASSERT_TRUE(navigation.is_alive());
0429   // Trust level is restored
0430   ASSERT_EQ(navigation.trust_level(), trust_level::e_full);
0431   // The status remains: towards surface
0432   check_towards_surface<navigator_t>(navigation, 0u, 1u, 0u);
0433   // Distance to portal is now halved
0434   ASSERT_NEAR(navigation(), 250.f * unit<scalar>::mm, tol);
0435 
0436   // Let's immediately update, nothing should change, as there is full trust
0437   nav.update(stepping(), navigation, nav_cfg, ctx);
0438   ASSERT_TRUE(navigation.is_alive());
0439   check_towards_surface<navigator_t>(navigation, 0u, 1u, 0u);
0440   ASSERT_NEAR(navigation(), 250.f * unit<scalar>::mm, tol);
0441 
0442   // Step onto portal in volume 0
0443   stepper.step(navigation(), stepping, step_cfg);
0444   navigation.set_high_trust();
0445   ASSERT_TRUE(navigation.trust_level() == trust_level::e_high);
0446   nav.update(stepping(), navigation, nav_cfg, ctx);
0447   ASSERT_TRUE(navigation.is_alive()) << navigation.inspector().to_string();
0448   ASSERT_EQ(navigation.trust_level(), trust_level::e_full);
0449 
0450   //
0451   // Wire Layer
0452   //
0453 
0454   // Last volume before we leave world
0455   dindex last_vol_id = n_layers;
0456 
0457   // maps volume id to the sequence of surfaces that the navigator encounters
0458   std::map<dindex, std::vector<dindex>> sf_sequences;
0459 
0460   // layer 1 to 10
0461   sf_sequences[1] = {3u, 47u, 4u};
0462   sf_sequences[2] = {168u, 214u, 169u};
0463   sf_sequences[3] = {339u, 386u, 340u};
0464   sf_sequences[4] = {516u, 565u, 517u};
0465   sf_sequences[5] = {700u, 750u, 701u};
0466   sf_sequences[6] = {890u, 942u, 891u};
0467   sf_sequences[7] = {1086u, 1139u, 1087u};
0468   sf_sequences[8] = {1288u, 1343u, 1289u};
0469   sf_sequences[9] = {1497u, 1554u, 1498u};
0470   sf_sequences[10] = {1712u, 1770u, 1713u};
0471 
0472   // Every iteration steps through one wire layer
0473   for (const auto &[vol_id, sf_seq] : sf_sequences) {
0474     // Exclude the portal we are already on
0475     std::size_t n_candidates = sf_seq.size() - 1u;
0476 
0477     // Test the copy constructor of the propagation state
0478     auto propagation_cpy{propagation};
0479     navigator_t::state &navigation_cpy = propagation_cpy.navigation();
0480     stepper_t::state &stepping_cpy = propagation_cpy.stepping();
0481 
0482     // We switched to next barrel volume
0483     check_volume_switch<navigator_t>(navigation_cpy, vol_id);
0484 
0485     // The status is: on adjacent portal in volume, towards next candidate
0486     check_on_surface<navigator_t>(navigation_cpy, vol_id, n_candidates,
0487                                   sf_seq[0], sf_seq[1]);
0488 
0489     // Step through the module surfaces
0490     for (std::size_t sf = 1u; sf < sf_seq.size() - 1u; ++sf) {
0491       // Count only the currently reachable candidates
0492       step_and_check(nav, stepper, propagation_cpy, nav_cfg, step_cfg, ctx,
0493                      vol_id, n_candidates - sf, sf_seq[sf], sf_seq[sf + 1u]);
0494     }
0495 
0496     // Step onto the portal in volume
0497     stepper.step(navigation_cpy(), stepping_cpy, step_cfg);
0498     navigation_cpy.set_high_trust();
0499 
0500     // Check against last volume
0501     if (vol_id == last_vol_id) {
0502       nav.update(stepping_cpy(), navigation_cpy, nav_cfg, ctx);
0503       ASSERT_FALSE(navigation_cpy.is_alive());
0504       // The status is: exited
0505       ASSERT_EQ(navigation_cpy.status(), status::e_exit);
0506       // Keep current volume id, so that nav. direction can be reversed
0507       ASSERT_EQ(last_vol_id, navigation_cpy.volume());
0508     } else {
0509       nav.update(stepping_cpy(), navigation_cpy, nav_cfg, ctx);
0510       ASSERT_TRUE(navigation_cpy.is_alive())
0511           << navigation_cpy.inspector().to_string();
0512     }
0513 
0514     // Update the propagation state with current step (test assignment op)
0515     propagation = propagation_cpy;
0516   }
0517 
0518   std::clog << detray::navigation::print_state(navigation) << std::endl;
0519   std::clog << detray::navigation::print_candidates(navigation, nav_cfg,
0520                                                     traj.pos(), traj.dir())
0521             << std::endl;
0522 
0523   // Leave for debugging
0524   // std::clog << navigation.inspector().to_string() << std::endl;
0525   ASSERT_TRUE(navigation.finished()) << navigation.inspector().to_string();
0526 }