diff --git a/beluga/include/beluga/algorithm/particle_filter.hpp b/beluga/include/beluga/algorithm/particle_filter.hpp index 8b8e1474a..3a8c99a90 100644 --- a/beluga/include/beluga/algorithm/particle_filter.hpp +++ b/beluga/include/beluga/algorithm/particle_filter.hpp @@ -18,14 +18,19 @@ #include #include -#include -#include -#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include + /** * \file * \brief Implementation of particle filters. @@ -236,7 +241,10 @@ struct BootstrapParticleFilter : public Mixin { * \ref ParticleResamplingPage "ParticleResampling" named requirements. */ void resample() { - particles_ = initialize_container(this->self().generate_samples_from(particles_) | this->self().take_samples()); + const auto resampling_vote_result = this->self().do_resampling_vote(); + if (resampling_vote_result) { + particles_ = initialize_container(this->self().generate_samples_from(particles_) | this->self().take_samples()); + } } private: @@ -285,6 +293,10 @@ struct MCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, MotionModel, SensorModel> { using ciabatta::mixin< @@ -293,6 +305,10 @@ struct MCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, MotionModel, SensorModel>::mixin; }; @@ -321,6 +337,10 @@ struct AMCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, MotionModel, SensorModel> { using ciabatta::mixin< @@ -329,6 +349,10 @@ struct AMCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, + ciabatta::curry::template mixin, MotionModel, SensorModel>::mixin; }; diff --git a/beluga/include/beluga/algorithm/resampling_rate_policies/resample_interval_policy.hpp b/beluga/include/beluga/algorithm/resampling_rate_policies/resample_interval_policy.hpp new file mode 100644 index 000000000..79900bd53 --- /dev/null +++ b/beluga/include/beluga/algorithm/resampling_rate_policies/resample_interval_policy.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP +#define BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP + +#include +#include + +#include + +namespace beluga { + +struct ResampleIntervalPolicyParam { + std::size_t resample_interval_count{1}; +}; + +template +struct ResampleIntervalPolicy : public Mixin { + public: + using param_type = ResampleIntervalPolicyParam; + + template + explicit ResampleIntervalPolicy(const param_type& configuration, Args&&... rest) + : Mixin(std::forward(rest)...), configuration_{configuration} { + if (configuration_.resample_interval_count > 1) { + this->self().register_sampling_voter([this]() { return do_resampling(); }); + } + } + + private: + param_type configuration_; + std::size_t filter_update_counter_{0}; + + [[nodiscard]] bool do_resampling() { + filter_update_counter_ = (filter_update_counter_ + 1) % configuration_.resample_interval_count; + return (filter_update_counter_ == 0); + } +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/algorithm/resampling_rate_policies/resample_on_motion_policy.hpp b/beluga/include/beluga/algorithm/resampling_rate_policies/resample_on_motion_policy.hpp new file mode 100644 index 000000000..42d64375f --- /dev/null +++ b/beluga/include/beluga/algorithm/resampling_rate_policies/resample_on_motion_policy.hpp @@ -0,0 +1,78 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP +#define BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP + +#include +#include + +#include +#include + +namespace beluga { + +struct ResampleOnMotionPolicyParam { + double update_min_d{0.}; + double update_min_a{0.}; +}; + +template +struct ResampleOnMotionPolicy : public Mixin { + public: + using param_type = ResampleOnMotionPolicyParam; + using motion_event = Sophus::SE2d; + + template + explicit ResampleOnMotionPolicy(const param_type& configuration, Args&&... rest) + : Mixin(std::forward(rest)...), configuration_{configuration} { + this->self().register_sampling_voter([this]() { return do_resampling(); }); + } + + private: + param_type configuration_; + std::optional latest_resample_pose_; + + [[nodiscard]] bool do_resampling() { + // To avoid loss of diversity in the particle population, don't + // resample when the state is known to be static. + // See 'Probabilistic Robotics, Chapter 4.2.4'. + auto current_pose = this->self().latest_motion_update(); + + // default to letting other policies decide + bool must_do_resample{true}; + + if (current_pose && latest_resample_pose_) { + // calculate relative transform between previous pose and the current one + const auto delta = latest_resample_pose_->inverse() * current_pose.value(); + + // only resample if movement is above thresholds + must_do_resample = // + std::abs(delta.translation().x()) > configuration_.update_min_d || + std::abs(delta.translation().y()) > configuration_.update_min_d || + std::abs(delta.so2().log()) > configuration_.update_min_a; + } + + // we always measure the distance travelled since the last time we did resample + if (must_do_resample) { + latest_resample_pose_ = current_pose; + } + + return must_do_resample; + } +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/algorithm/resampling_rate_policies/selective_resampling_policy.hpp b/beluga/include/beluga/algorithm/resampling_rate_policies/selective_resampling_policy.hpp new file mode 100644 index 000000000..5f74fa4ce --- /dev/null +++ b/beluga/include/beluga/algorithm/resampling_rate_policies/selective_resampling_policy.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_SELECTIVE_RESAMPLING_POLICY_HPP +#define BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_SELECTIVE_RESAMPLING_POLICY_HPP + +#include + +#include +#include + +#include + +namespace beluga { + +struct SelectiveResamplingPolicyParam { + bool selective_resampling{false}; +}; + +template +struct SelectiveResamplingPolicy : public Mixin { + public: + using param_type = SelectiveResamplingPolicyParam; + + template + explicit SelectiveResamplingPolicy(const param_type& configuration, Args&&... rest) + : Mixin(std::forward(rest)...), configuration_{configuration} { + // only enable this if it's configured to be used + if (configuration.selective_resampling) { + this->self().register_sampling_voter([this]() { return do_resampling(); }); + } + } + + private: + param_type configuration_; + + [[nodiscard]] bool do_resampling() const { + const auto n_eff = + 1. / + ranges::accumulate(this->self().weights() | ranges::views::transform([](const auto w) { return w * w; }), 0.); + const auto n = static_cast(std::size(this->self().weights())); + return n_eff < n / 2.; + } +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/motion.hpp b/beluga/include/beluga/motion.hpp index a6d13da7a..0f64a39d6 100644 --- a/beluga/include/beluga/motion.hpp +++ b/beluga/include/beluga/motion.hpp @@ -44,6 +44,8 @@ * will be used in subsequent calls to the `apply_motion()` method. * - `cp.apply_motion(s)` returns a `T::state_type`, that is the result of applying the motion model * to `s` based on the updates. + * - `cp.latest_motion_update() returns a std::optional with the latest motion update + * received through motion_update(). */ #endif diff --git a/beluga/include/beluga/motion/differential_drive_model.hpp b/beluga/include/beluga/motion/differential_drive_model.hpp index 92c222770..fd2ec16c6 100644 --- a/beluga/include/beluga/motion/differential_drive_model.hpp +++ b/beluga/include/beluga/motion/differential_drive_model.hpp @@ -110,7 +110,7 @@ class DifferentialDriveModel : public Mixin { * That is done by the particle filter using the apply_motion() method * provided by this class. * - * \param pose Last odometry udpate. + * \param pose Last odometry update. */ void update_motion(const update_type& pose) { if (last_pose_) { @@ -146,11 +146,14 @@ class DifferentialDriveModel : public Mixin { last_pose_ = pose; } + /// recovers latest motion update. + [[nodiscard]] std::optional latest_motion_update() const { return last_pose_; } + private: using DistributionParam = typename std::normal_distribution::param_type; DifferentialDriveModelParam params_; - std::optional last_pose_; + std::optional last_pose_; DistributionParam first_rotation_params_{0.0, 0.0}; DistributionParam second_rotation_params_{0.0, 0.0}; diff --git a/beluga/include/beluga/motion/stationary_model.hpp b/beluga/include/beluga/motion/stationary_model.hpp index d03e9ad7b..214ed5414 100644 --- a/beluga/include/beluga/motion/stationary_model.hpp +++ b/beluga/include/beluga/motion/stationary_model.hpp @@ -69,6 +69,12 @@ class StationaryModel : public Mixin { * For the stationary model, updates are ignored. */ void update_motion(const update_type&) {} + + /// recovers latest motion update. + /** + * For the stationary model, we don't ever have motion updates. + */ + [[nodiscard]] std::optional latest_motion_update() const { return std::nullopt; } }; } // namespace beluga diff --git a/beluga/include/beluga/runtime_dispatch/runtime_dispatch.hpp b/beluga/include/beluga/runtime_dispatch/runtime_dispatch.hpp new file mode 100644 index 000000000..753a816b0 --- /dev/null +++ b/beluga/include/beluga/runtime_dispatch/runtime_dispatch.hpp @@ -0,0 +1,50 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_RUNTIME_DISPATCH_RUNTIME_DISPATCH_HPP +#define BELUGA_RUNTIME_DISPATCH_RUNTIME_DISPATCH_HPP + +#include +#include + +#include + +namespace beluga { + +template +struct RuntimeDispatch : public Mixin { + public: + using resample_voter_function = std::function; + + template + explicit RuntimeDispatch(Args&&... rest) : Mixin(std::forward(rest)...) {} + + [[nodiscard]] bool do_resampling_vote() { + // resampling voters are in cascade: if any of them votes "no", no resampling is done + return std::all_of(voters_.begin(), voters_.end(), [](const auto& predicate) { return predicate(); }); + } + + // do not split this function from the "protected" access clause. The method needs to be protected to + // prevent code that tries call register_sampling_voter() before this mixin's constructor + // has been executed from compiling. + protected: + void register_sampling_voter(resample_voter_function predicate) { voters_.push_back(std::move(predicate)); } + + private: + std::vector voters_; +}; + +} // namespace beluga + +#endif diff --git a/beluga/test/CMakeLists.txt b/beluga/test/CMakeLists.txt index eca282bf8..1f5a49aa4 100644 --- a/beluga/test/CMakeLists.txt +++ b/beluga/test/CMakeLists.txt @@ -1,2 +1,3 @@ add_subdirectory(benchmark) add_subdirectory(beluga) +add_subdirectory(build_failure_tests) diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index bc8440a6e..a65bdc6db 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -8,6 +8,10 @@ ament_add_gmock(test_beluga algorithm/test_sampling.cpp motion/test_differential_drive_model.cpp random/test_multivariate_normal_distribution.cpp + resampling_rate_policies/test_resample_interval_policy.cpp + resampling_rate_policies/test_resample_on_motion_policy.cpp + resampling_rate_policies/test_selective_resampling_policy.cpp + runtime_dispatch/test_runtime_dispatch.cpp sensor/test_likelihood_field_model.cpp test_tuple_vector.cpp test_spatial_hash.cpp) diff --git a/beluga/test/beluga/algorithm/test_particle_filter.cpp b/beluga/test/beluga/algorithm/test_particle_filter.cpp index b2b43490f..cc6904566 100644 --- a/beluga/test/beluga/algorithm/test_particle_filter.cpp +++ b/beluga/test/beluga/algorithm/test_particle_filter.cpp @@ -49,6 +49,8 @@ class MockMixin : public ciabatta::mixin, Mixin> { return range | ranges::views::all; } + MOCK_METHOD(bool, do_resampling_vote, ()); + auto take_samples() const { return ranges::views::take_exactly(max_samples()); } }; @@ -64,6 +66,7 @@ TEST(BootstrapParticleFilter, Update) { auto filter = MockMixin(); EXPECT_CALL(filter, apply_motion(1.)).WillRepeatedly(testing::Return(2.)); EXPECT_CALL(filter, importance_weight(2.)).WillRepeatedly(testing::Return(3.)); + EXPECT_CALL(filter, do_resampling_vote()).WillRepeatedly(testing::Return(true)); filter.update(); for (auto [state, weight] : filter.particles()) { ASSERT_EQ(state, 2.); @@ -74,9 +77,13 @@ TEST(BootstrapParticleFilter, Update) { template class MockMotionModel : public Mixin { public: + using update_type = Sophus::SE2d; + template explicit MockMotionModel(Args&&... args) : Mixin(std::forward(args)...) {} + [[nodiscard]] std::optional latest_motion_update() const { return std::nullopt; } + [[nodiscard]] double apply_motion(double state) { return state; } }; @@ -96,7 +103,9 @@ class MockSensorModel : public Mixin { TEST(MCL, InitializeFilter) { constexpr std::size_t kMaxSamples = 1'000; - auto filter = beluga::MCL{beluga::FixedResamplingParam{kMaxSamples}}; + auto filter = beluga::MCL{ + beluga::FixedResamplingParam{kMaxSamples}, beluga::ResampleOnMotionPolicyParam{}, + beluga::ResampleIntervalPolicyParam{}, beluga::SelectiveResamplingPolicyParam{}}; ASSERT_EQ(filter.particles().size(), kMaxSamples); } @@ -110,7 +119,9 @@ TEST(AMCL, InitializeFilter) { constexpr double kKldZ = 3.; auto filter = beluga::AMCL{ beluga::AdaptiveGenerationParam{kAlphaSlow, kAlphaFast}, - beluga::KldResamplingParam{kMinSamples, kMaxSamples, kSpatialResolution, kKldEpsilon, kKldZ}}; + beluga::KldResamplingParam{kMinSamples, kMaxSamples, kSpatialResolution, kKldEpsilon, kKldZ}, + beluga::ResampleOnMotionPolicyParam{}, beluga::ResampleIntervalPolicyParam{}, + beluga::SelectiveResamplingPolicyParam{}}; ASSERT_GE(filter.particles().size(), kMinSamples); } diff --git a/beluga/test/beluga/motion/test_differential_drive_model.cpp b/beluga/test/beluga/motion/test_differential_drive_model.cpp index eb13ff123..0576ee805 100644 --- a/beluga/test/beluga/motion/test_differential_drive_model.cpp +++ b/beluga/test/beluga/motion/test_differential_drive_model.cpp @@ -154,4 +154,37 @@ TEST(DifferentialDriveModelSamples, RotateThirdQuadrant) { ASSERT_NEAR(stddev, std::sqrt(alpha * flipped_angle * flipped_angle), 0.01); } +TEST(DifferentialDriveModelSamples, LatestMotionUpdateWorks) { + // tests that the latest motion update can be recovered + auto uut = MockMixin{beluga::DifferentialDriveModelParam{1.0, 0.0, 0.0, 0.0}}; + + auto make_motion_update = [](double x, double y, double phi) { + using Eigen::Vector2d; + using Sophus::SE2d; + using Sophus::SO2d; + return Sophus::SE2d{SO2d{phi}, Vector2d{x, y}}; + }; + + { + const auto lmu = uut.latest_motion_update(); + ASSERT_FALSE(lmu); + } + + { + const auto expected_motion_update = make_motion_update(0.1, 0.2, 0.3); + uut.update_motion(expected_motion_update); + const auto latest_motion_update = uut.latest_motion_update(); + ASSERT_TRUE(latest_motion_update); + ASSERT_THAT(expected_motion_update, testing::SE2Eq(latest_motion_update.value())); + } + + { + const auto expected_motion_update = make_motion_update(0.4, 0.5, 0.6); + uut.update_motion(expected_motion_update); + const auto latest_motion_update = uut.latest_motion_update(); + ASSERT_TRUE(latest_motion_update); + ASSERT_THAT(expected_motion_update, testing::SE2Eq(latest_motion_update.value())); + } +} + } // namespace diff --git a/beluga/test/beluga/resampling_rate_policies/test_resample_interval_policy.cpp b/beluga/test/beluga/resampling_rate_policies/test_resample_interval_policy.cpp new file mode 100644 index 000000000..bb09cc397 --- /dev/null +++ b/beluga/test/beluga/resampling_rate_policies/test_resample_interval_policy.cpp @@ -0,0 +1,66 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +namespace beluga { + +namespace { + +struct UUT : public ciabatta::mixin { + using ciabatta::mixin::mixin; +}; + +struct ResampleIntervalPolicyTests : public ::testing::Test {}; + +TEST_F(ResampleIntervalPolicyTests, Construction) { + // UUT does not blow up during contruction + [[maybe_unused]] UUT uut{ResampleIntervalPolicyParam{}}; +} + +class ResampleIntervalPolicyTestsWithParam : public testing::TestWithParam {}; + +TEST_P(ResampleIntervalPolicyTestsWithParam, ResampleEveryNthIteration) { + // uut allows resampling every N-th iteration, starting on the N-th one + const auto interval = GetParam(); + const auto periods = 10; + + ResampleIntervalPolicyParam config; + config.resample_interval_count = interval; + + UUT uut{config}; + + for (size_t i = 0; i < periods; ++i) { + // don't resample for the first N-1 iterations + for (size_t iteration = 0; iteration < interval - 1; ++iteration) { + ASSERT_FALSE(uut.do_resampling_vote()); + } + // then resample once + ASSERT_TRUE(uut.do_resampling_vote()); + } +} + +INSTANTIATE_TEST_SUITE_P( + ResampleEveryNthIterationInstance, + ResampleIntervalPolicyTestsWithParam, + testing::Values(3, 5, 10, 21)); + +} // namespace + +} // namespace beluga diff --git a/beluga/test/beluga/resampling_rate_policies/test_resample_on_motion_policy.cpp b/beluga/test/beluga/resampling_rate_policies/test_resample_on_motion_policy.cpp new file mode 100644 index 000000000..3e4087719 --- /dev/null +++ b/beluga/test/beluga/resampling_rate_policies/test_resample_on_motion_policy.cpp @@ -0,0 +1,239 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +namespace beluga { + +namespace { + +template +struct MockMotionModel : public Mixin { + using update_type = Sophus::SE2d; + + template + explicit MockMotionModel(Args&&... rest) : Mixin(std::forward(rest)...) {} + + void update_motion(const update_type& pose) { last_pose_ = pose; } + [[nodiscard]] std::optional latest_motion_update() const { return last_pose_; } + + private: + std::optional last_pose_; +}; + +struct UUT : public ciabatta::mixin { + using ciabatta::mixin::mixin; +}; + +struct ResampleOnMotionPolicyTests : public ::testing::Test { + auto make_motion_update_event(double x, double y, double phi) { + using Eigen::Vector2d; + using Sophus::SE2d; + using Sophus::SO2d; + return Sophus::SE2d{SO2d{phi}, Vector2d{x, y}}; + }; + + const double d_threshold = 0.4; + const double a_threshold = 0.2; +}; + +TEST_F(ResampleOnMotionPolicyTests, Construction) { + // tests uut does not blow up during contruction + [[maybe_unused]] UUT uut{ResampleOnMotionPolicyParam{}}; +} + +TEST_F(ResampleOnMotionPolicyTests, MotionUpdatesEnableResampling) { + // uut allows sampling only when motion values are above parameter thresholds + ResampleOnMotionPolicyParam params{}; + params.update_min_d = d_threshold; + params.update_min_a = a_threshold; + + UUT uut{params}; + + // when there's no motion data, policy should default to true + ASSERT_TRUE(uut.do_resampling_vote()); + + // first motion update, still not enough data, will default to allowing sampling + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // second motion update, enough data, but we are not moving + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // still no motion + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // still no motion (thresholds are calculated independently for each axis) + uut.update_motion(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // back to origin + uut.update_motion(make_motion_update_event(0, 0, 0)); + + // motion above threshold for axis x + uut.update_motion(make_motion_update_event(d_threshold * 1.01, 0.0, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // motion above threshold for axis y + uut.update_motion(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // motion above threshold for angular motion + uut.update_motion(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 1.01)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // no relative motion again + uut.update_motion(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 1.01)); + ASSERT_FALSE(uut.do_resampling_vote()); +} + +TEST_F(ResampleOnMotionPolicyTests, ThresholdsAreRelativeToLatestResamplingPose) { + // uut measures threshold from the latest pose where the policy agreed to do resampling + ResampleOnMotionPolicyParam params{}; + params.update_min_d = d_threshold; + params.update_min_a = a_threshold; + + UUT uut{params}; + + // when there's no motion data, policy should default to true + ASSERT_TRUE(uut.do_resampling_vote()); + + // first motion update, still not enough data, will default to allowing sampling + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // second motion update, enough data, but we are not moving + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + const auto pivot_distance = d_threshold * 10.0; + const auto lower_bound = pivot_distance - 0.95 * d_threshold; + const auto upper_bound = pivot_distance + 0.95 * d_threshold; + + // move to 10x the threshold, forcing resampling to take place + uut.update_motion(make_motion_update_event(pivot_distance, 0.0, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // move to the lower bound, then forward to the upper bound, then back to the lower bound and finally back to the + // pivot. Despite two of the steps being almost 2x the length of the threshold, we are always at a distance of less + // than a threshold from the pivot where we last resampled, so none of the steps should cause additoinal resamples + // value, because we never get farther away from the pivot than the treshold value. + uut.update_motion(make_motion_update_event(pivot_distance, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(lower_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(upper_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(lower_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(upper_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // resample when we move a full threshold from the pivot + uut.update_motion(make_motion_update_event(pivot_distance + d_threshold * 1.01, 0.0, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); +} + +TEST_F(ResampleOnMotionPolicyTests, RandomWalkingTest) { + // simulate making the uut go on a random walk, with some steps above threshold, and others below, + // to make sure it does not only work with the predictable poses (in particular rotations), that + // other more systematic tests above use + ResampleOnMotionPolicyParam params{}; + params.update_min_d = d_threshold; + params.update_min_a = a_threshold; + + UUT uut{params}; + + // when there's no motion data, policy should default to true + ASSERT_TRUE(uut.do_resampling_vote()); + + // list of pairs of relative motion from the previous pose, and the expected + // "do_resampling" value for that step + const auto relative_motions = { + std::make_tuple(make_motion_update_event(0, 0, 0), false), + // relative motions along x axis + std::make_tuple(make_motion_update_event(d_threshold * 0.99, 0.0, 0.0), false), + std::make_tuple(make_motion_update_event(d_threshold * 0.02, 0.0, 0.0), true), + std::make_tuple(make_motion_update_event(d_threshold * -0.99, 0.0, 0.0), false), + std::make_tuple(make_motion_update_event(d_threshold * -0.02, 0.0, 0.0), true), + // relative motions along y axis + std::make_tuple(make_motion_update_event(0.0, d_threshold * 0.99, 0.0), false), + std::make_tuple(make_motion_update_event(0.0, d_threshold * 0.02, 0.0), true), + std::make_tuple(make_motion_update_event(0.0, d_threshold * -0.99, 0.0), false), + std::make_tuple(make_motion_update_event(0.0, d_threshold * -0.02, 0.0), true), + // relative motions along theta axis + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * 0.99), false), + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * 0.02), true), + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * -0.99), false), + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * -0.02), true), + // random walk move and stop 1 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 0.40), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.20, d_threshold * 0.10, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 2 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.30, a_threshold * 0.10), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.50, d_threshold * 1.01, a_threshold * 0.30), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.70, d_threshold * 0.40, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 3 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 0.70), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.30, d_threshold * 0.40, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.01, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 4 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.80, a_threshold * 0.10), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.20, d_threshold * 0.50, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 5 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.30, a_threshold * 0.10), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.50, d_threshold * 1.01, a_threshold * 0.80), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.20, d_threshold * 0.40, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 6 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 0.50), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.40, d_threshold * 0.70, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + }; + + auto current_pose = make_motion_update_event(0, 0, 0); + + // warm-up the internal state of the policy + uut.update_motion(current_pose); + ASSERT_TRUE(uut.do_resampling_vote()); + uut.update_motion(current_pose); + ASSERT_FALSE(uut.do_resampling_vote()); + + for (const auto& test_tuple : relative_motions) { + const auto relative_movement = std::get<0>(test_tuple); + const auto expected_resampling_flag = std::get<1>(test_tuple); + // apply the relative motion on top of the current pose and test the result + current_pose = current_pose * relative_movement; + uut.update_motion(current_pose); + ASSERT_EQ(expected_resampling_flag, uut.do_resampling_vote()); + } +} + +} // namespace + +} // namespace beluga diff --git a/beluga/test/beluga/resampling_rate_policies/test_selective_resampling_policy.cpp b/beluga/test/beluga/resampling_rate_policies/test_selective_resampling_policy.cpp new file mode 100644 index 000000000..46728c89c --- /dev/null +++ b/beluga/test/beluga/resampling_rate_policies/test_selective_resampling_policy.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace beluga { + +namespace { + +template +struct MockMixin : public Mixin { + public: + std::vector weights_; + + using view_t = decltype(ranges::views::all(weights_)); + + [[nodiscard]] auto weights() const { return ranges::views::all(weights_) | ranges::views::const_; } + + void set_weights(std::vector v) { weights_ = std::move(v); } +}; + +struct UUT : public ciabatta::mixin { + using ciabatta::mixin::mixin; +}; + +struct SelectiveResamplingPolicyTests : public ::testing::Test {}; + +TEST_F(SelectiveResamplingPolicyTests, Construction) { + // UUT does not blow up during contruction + [[maybe_unused]] UUT uut{SelectiveResamplingPolicyParam{}}; +} + +TEST_F(SelectiveResamplingPolicyTests, SelectiveResamplingOn) { + // uut allows resampling every N-th iteration, starting on the N-th one + + SelectiveResamplingPolicyParam config; + config.selective_resampling = true; + UUT uut{config}; + + // very low n_eff, do resampling + uut.set_weights({1.0, 1.0, 1.0, 1.0, 1.0}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // very high n_eff, don't do resampling + uut.set_weights({0.1, 0.1, 0.1, 0.1, 0.1}); + ASSERT_FALSE(uut.do_resampling_vote()); + + // n_eff slightly below N/2, do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.30}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // n_eff slightly above N/2, don't do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.25}); + ASSERT_FALSE(uut.do_resampling_vote()); +} + +TEST_F(SelectiveResamplingPolicyTests, SelectiveResamplingOff) { + // uut allows resampling every N-th iteration, starting on the N-th one + + SelectiveResamplingPolicyParam config; + config.selective_resampling = false; + UUT uut{config}; + + // very low n_eff, do resampling + uut.set_weights({1.0, 1.0, 1.0, 1.0, 1.0}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // very high n_eff, don't do resampling + uut.set_weights({0.1, 0.1, 0.1, 0.1, 0.1}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // n_eff slightly below N/2, do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.30}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // n_eff slightly above N/2, don't do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.25}); + ASSERT_TRUE(uut.do_resampling_vote()); +} + +} // namespace + +} // namespace beluga diff --git a/beluga/test/beluga/runtime_dispatch/test_runtime_dispatch.cpp b/beluga/test/beluga/runtime_dispatch/test_runtime_dispatch.cpp new file mode 100644 index 000000000..ce41f125a --- /dev/null +++ b/beluga/test/beluga/runtime_dispatch/test_runtime_dispatch.cpp @@ -0,0 +1,90 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include + +#include + +namespace beluga { + +namespace { + +template +using EventSubscriberCallback = std::function; + +// this class is needed because register_sampling_voter() is a protected method +template +struct RuntimeDispatchProxy : public Mixin { + public: + template + void proxy_register_sampling_voter(T&& p) { + this->self().register_sampling_voter(std::forward(p)); + } +}; + +struct UUT : public ciabatta::mixin { + using ciabatta::mixin::mixin; +}; + +struct RuntimeDispatchTests : public ::testing::Test {}; + +TEST_F(RuntimeDispatchTests, Construction) { + // uut does not blow up during contruction + UUT uut; +} + +TEST_F(RuntimeDispatchTests, ResamplingVoteWithMultiplePolicies) { + // Tests that only when all resampling policies agree on doing resampling + // the vote is positive. This test is for the case with selective resampling on. + UUT uut; + + bool voter_1; + bool voter_2; + bool voter_3; + + uut.proxy_register_sampling_voter([&voter_1]() { return voter_1; }); + uut.proxy_register_sampling_voter([&voter_2]() { return voter_2; }); + uut.proxy_register_sampling_voter([&voter_3]() { return voter_3; }); + + const auto test_tuples = { + std::make_tuple(false, false, false, false), std::make_tuple(true, false, false, false), + std::make_tuple(false, true, false, false), std::make_tuple(true, true, false, false), + std::make_tuple(false, false, true, false), std::make_tuple(true, false, true, false), + std::make_tuple(false, true, true, false), std::make_tuple(true, true, true, true), + }; + + for (const auto& test_values : test_tuples) { + bool final_vote_value; + std::tie(voter_1, voter_2, voter_3, final_vote_value) = test_values; + ASSERT_EQ(final_vote_value, uut.do_resampling_vote()); + } +} + +TEST_F(RuntimeDispatchTests, ResamplingVoteWithNoPolicies) { + // Tests that if no policies have been registered, the vote turns + // out to be positive by default. + UUT uut{}; + + ASSERT_TRUE(uut.do_resampling_vote()); + ASSERT_TRUE(uut.do_resampling_vote()); + ASSERT_TRUE(uut.do_resampling_vote()); +} + +} // namespace + +} // namespace beluga diff --git a/beluga/test/build_failure_tests/CMakeLists.txt b/beluga/test/build_failure_tests/CMakeLists.txt new file mode 100644 index 000000000..0fad2bb26 --- /dev/null +++ b/beluga/test/build_failure_tests/CMakeLists.txt @@ -0,0 +1,5 @@ +include(cmake/beluga_add_build_failure_test.cmake) + +beluga_add_build_failure_test(test_build_failure_test_runtime_dispatch_shall_go_last + test_runtime_dispatch_shall_go_last.cpp) +target_link_libraries(test_build_failure_test_runtime_dispatch_shall_go_last ${PROJECT_NAME}) diff --git a/beluga/test/build_failure_tests/cmake/beluga_add_build_failure_test.cmake b/beluga/test/build_failure_tests/cmake/beluga_add_build_failure_test.cmake new file mode 100644 index 000000000..bea89af54 --- /dev/null +++ b/beluga/test/build_failure_tests/cmake/beluga_add_build_failure_test.cmake @@ -0,0 +1,61 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# CHANGELOG: +# - Change macro name to beluga_add_build_failure_test() +# + +# +# Register a test which tries to compile a file and expects it to fail to build. +# +# This will create two targets, one by the given target name and a test target +# which has the same name prefixed with `test_`. +# For example, if target is `should_not_compile__use_const_argument` then there +# will be an executable target called `should_not_compile__use_const_argument` +# and a test target called `test_should_not_compile__use_const_argument`. +# +# :param target: the name of the target to be created +# :type target: string +# :param ARGN: the list of source files to be used to create the test executable +# :type ARGN: list of strings +# +macro(beluga_add_build_failure_test target) + if(${ARGC} EQUAL 0) + message( + FATAL_ERROR + "beluga_add_build_failure_test() requires a target name and " + "at least one source file") + endif() + + add_executable(${target} ${ARGN}) + set_target_properties(${target} + PROPERTIES + EXCLUDE_FROM_ALL TRUE + EXCLUDE_FROM_DEFAULT_BUILD TRUE) + + add_test( + NAME test_${target} + COMMAND + ${CMAKE_COMMAND} + --build . + --target ${target} + --config $ + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) + set_tests_properties(test_${target} + PROPERTIES + WILL_FAIL TRUE + LABELS "build_failure" + ) +endmacro() diff --git a/beluga/test/build_failure_tests/test_runtime_dispatch_shall_go_last.cpp b/beluga/test/build_failure_tests/test_runtime_dispatch_shall_go_last.cpp new file mode 100644 index 000000000..001156e4a --- /dev/null +++ b/beluga/test/build_failure_tests/test_runtime_dispatch_shall_go_last.cpp @@ -0,0 +1,53 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include + +namespace beluga { + +template +struct DummyResamplingPolicy : public Mixin { + public: + template + explicit DummyResamplingPolicy(Args&&... rest) : Mixin(std::forward(rest)...) { + this->self().register_sampling_voter([this]() { return do_resampling(); }); + } + + private: + [[nodiscard]] bool do_resampling() { return false; } +}; + +// This must file will be unable to build properly because there's a mixin that uses +// register_sampling_voter() that is added to the mixin chain AFTER the RuntimeDispatch mixin. +// The register_sampling_voter() function is protected in RuntimeDispatch to cause a build error +// in this case, because otherwise that mixin ordering will cause a runtime error due to +// RuntimeDispatch not being properly initialized by the time register_sampling_voter() is called. +// +// This build failure test makes sure that this kind of error continues to be detected. + +struct UUT : public ciabatta::mixin { + using ciabatta::mixin::mixin; +}; + +} // namespace beluga + +int main() { + [[maybe_unused]] auto node = std::make_shared(); + return 0; +} diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index c5813d3c5..6fb05d450 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -172,6 +172,17 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) declare_parameter("resample_interval", rclcpp::ParameterValue(1), descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "When set to true, will reduce the resampling rate when not needed and help " + "avoid particle deprivation. The resampling will only happen if the effective " + "number of particles (N_eff = 1/(sum(k_i^2))) is lower than half the current " + "number of particles."; + descriptor.read_only = true; + declare_parameter("selective_resampling", false, descriptor); + } + { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = @@ -579,6 +590,14 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) resampling_params.kld_epsilon = get_parameter("pf_err").as_double(); resampling_params.kld_z = get_parameter("pf_z").as_double(); + auto resample_on_motion_params = beluga::ResampleOnMotionPolicyParam{}; + resample_on_motion_params.update_min_d = get_parameter("update_min_d").as_double(); + resample_on_motion_params.update_min_a = get_parameter("update_min_a").as_double(); + + auto resample_interval_params = beluga::ResampleIntervalPolicyParam{}; + resample_interval_params.resample_interval_count = + static_cast(get_parameter("resample_interval").as_int()); + auto sensor_params = beluga::LikelihoodFieldModelParam{}; sensor_params.max_obstacle_distance = get_parameter("laser_likelihood_max_dist").as_double(); sensor_params.max_laser_distance = get_parameter("laser_max_range").as_double(); @@ -592,6 +611,9 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) motion_params.translation_noise_from_translation = get_parameter("alpha3").as_double(); motion_params.translation_noise_from_rotation = get_parameter("alpha4").as_double(); + auto policy_selection_params = beluga::SelectiveResamplingPolicyParam{}; + policy_selection_params.selective_resampling = get_parameter("selective_resampling").as_bool(); + // Only when we get the first map we should use the parameters, not later. // TODO(ivanpauno): Intialize later maps from last known pose. const bool initialize_from_params = !particle_filter_ && @@ -600,6 +622,9 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) particle_filter_ = std::make_unique( generation_params, resampling_params, + resample_on_motion_params, + resample_interval_params, + policy_selection_params, motion_params, sensor_params, OccupancyGrid{map}); @@ -722,20 +747,9 @@ void AmclNode::laser_callback( static_cast(get_parameter("laser_max_range").as_double()))); particle_filter_->importance_sample(exec_policy); const auto time3 = std::chrono::high_resolution_clock::now(); - { - const auto delta = odom_to_base_transform * last_odom_to_base_transform_.inverse(); - const bool has_moved_since_last_resample = - std::abs(delta.translation().x()) > get_parameter("update_min_d").as_double() || - std::abs(delta.translation().y()) > get_parameter("update_min_d").as_double() || - std::abs(delta.so2().log()) > get_parameter("update_min_a").as_double(); - if (has_moved_since_last_resample) { - // To avoid loss of diversity in the particle population, don't - // resample when the state is known to be static. - // See 'Probabilistic Robotics, Chapter 4.2.4'. - particle_filter_->resample(); - last_odom_to_base_transform_ = odom_to_base_transform; - } - } + + particle_filter_->resample(); + const auto time4 = std::chrono::high_resolution_clock::now(); RCLCPP_INFO_THROTTLE(