From e72e1489a74d1302ddc9ddcd5b7660d26625ea92 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 24 May 2023 15:51:46 -0300 Subject: [PATCH] Add support for map updates in laser localization filters (#189) Signed-off-by: Michel Hidalgo --- beluga/include/beluga/localization.hpp | 16 ++-- beluga/include/beluga/sensor.hpp | 14 ++++ beluga/include/beluga/sensor/beam_model.hpp | 15 +++- .../beluga/sensor/likelihood_field_model.hpp | 16 +++- beluga/test/beluga/sensor/test_beam_model.cpp | 39 ++++++++- .../sensor/test_likelihood_field_model.cpp | 37 ++++++++- .../include/beluga_amcl/private/amcl_node.hpp | 8 +- beluga_amcl/src/amcl_node.cpp | 79 +++++++++++-------- beluga_system_tests/test/test_system.cpp | 13 ++- 9 files changed, 187 insertions(+), 50 deletions(-) diff --git a/beluga/include/beluga/localization.hpp b/beluga/include/beluga/localization.hpp index 4481f0238..5f41afbd9 100644 --- a/beluga/include/beluga/localization.hpp +++ b/beluga/include/beluga/localization.hpp @@ -52,12 +52,16 @@ namespace beluga { */ /// Pure abstract class representing an interface for laser-based localization algorithms. +/** + * \tparam Map Environment representation type. + */ +template using LaserLocalizationInterface2d = beluga::mixin::compose_interfaces< BaseParticleFilterInterface, StorageInterface, EstimationInterface2d, OdometryMotionModelInterface2d, - LaserSensorModelInterface2d>; + LaserSensorModelInterface2d>; /// Commonly used resampling policies combined in a single mixin. using CombinedResamplingPolicy = ciabatta:: @@ -70,8 +74,9 @@ using CombinedResamplingPolicy = ciabatta:: * * \tparam MotionDescriptor A descriptor of a mixin that implements \ref MotionModelPage. * \tparam SensorDescriptor A descriptor of a mixin that implements \ref SensorModelPage. + * \tparam Map Environment representation type consistent with the sensor descriptor. */ -template +template using MonteCarloLocalization2d = ciabatta::mixin< BootstrapParticleFilter, ciabatta::curry::mixin, @@ -82,7 +87,7 @@ using MonteCarloLocalization2d = ciabatta::mixin< CombinedResamplingPolicy::template mixin, MotionDescriptor::template mixin, SensorDescriptor::template mixin, - ciabatta::provides::mixin>; + ciabatta::provides>::template mixin>; /// An implementation of Adaptive Monte Carlo Localization. /** @@ -91,8 +96,9 @@ using MonteCarloLocalization2d = ciabatta::mixin< * * \tparam MotionDescriptor A descriptor of a mixin that implements \ref MotionModelPage. * \tparam SensorDescriptor A descriptor of a mixin that implements \ref SensorModelPage. + * \tparam Map Environment representation type consistent with the sensor descriptor. */ -template +template using AdaptiveMonteCarloLocalization2d = ciabatta::mixin< BootstrapParticleFilter, ciabatta::curry::mixin, @@ -103,7 +109,7 @@ using AdaptiveMonteCarloLocalization2d = ciabatta::mixin< CombinedResamplingPolicy::template mixin, MotionDescriptor::template mixin, SensorDescriptor::template mixin, - ciabatta::provides::mixin>; + ciabatta::provides>::template mixin>; } // namespace beluga diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 09d05dfaf..e7bd05eb7 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -54,6 +54,10 @@ namespace beluga { /// Pure abstract class representing the laser sensor model interface. +/** + * \tparam Map Environment representation type. + */ +template struct LaserSensorModelInterface2d { /// Measurement type of the sensor: a point cloud for the range finder. using measurement_type = std::vector>; @@ -72,6 +76,16 @@ struct LaserSensorModelInterface2d { * \param points The range finder points in the reference frame of the particle. */ virtual void update_sensor(measurement_type&& points) = 0; + + /// Update the sensor model with a new map. + /** + * This method updates the sensor model with a new map, + * i.e. a representation of the environment, that it needs + * to compute the weight of each particle. + * + * \param map The range finder map. + */ + virtual void update_map(Map&& map) = 0; }; } // namespace beluga diff --git a/beluga/include/beluga/sensor/beam_model.hpp b/beluga/include/beluga/sensor/beam_model.hpp index 2067dc4e6..7802b31f3 100644 --- a/beluga/include/beluga/sensor/beam_model.hpp +++ b/beluga/include/beluga/sensor/beam_model.hpp @@ -93,9 +93,7 @@ class BeamSensorModel : public Mixin { : Mixin(std::forward(rest)...), params_{params}, grid_{std::move(grid)}, - free_states_{ - grid_.coordinates_for(grid_.free_cells(), OccupancyGrid::Frame::kGlobal) | - ranges::to>} {} + free_states_{compute_free_states(grid_)} {} // TODO(ivanpauno): is sensor model the best place for this? // Maybe the map could be provided by a different part of the mixin, @@ -165,11 +163,22 @@ class BeamSensorModel : public Mixin { /// \copydoc LaserSensorModelInterface2d::update_sensor(measurement_type&& points) void update_sensor(measurement_type&& points) final { points_ = std::move(points); } + /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map) + void update_map(OccupancyGrid&& map) final { + grid_ = std::move(map); + free_states_ = compute_free_states(grid_); + } + private: param_type params_; OccupancyGrid grid_; std::vector free_states_; std::vector> points_; + + static std::vector compute_free_states(const OccupancyGrid& grid) { + constexpr auto kFrame = OccupancyGrid::Frame::kGlobal; + return grid.coordinates_for(grid.free_cells(), kFrame) | ranges::to; + } }; } // namespace beluga diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp index e3c821624..d341970dd 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model.hpp @@ -97,9 +97,7 @@ class LikelihoodFieldModel : public Mixin { : Mixin(std::forward(rest)...), params_{params}, grid_{std::move(grid)}, - free_states_{ - grid_.coordinates_for(grid_.free_cells(), OccupancyGrid::Frame::kGlobal) | - ranges::to>}, + free_states_{compute_free_states(grid_)}, likelihood_field_{make_likelihood_field(params, grid_)} {} /// Returns the likelihood field, constructed from the provided map. @@ -152,6 +150,13 @@ class LikelihoodFieldModel : public Mixin { /// \copydoc LaserSensorModelInterface2d::update_sensor(measurement_type&& points) void update_sensor(measurement_type&& points) final { points_ = std::move(points); } + /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map) + void update_map(OccupancyGrid&& map) final { + grid_ = std::move(map); + free_states_ = compute_free_states(grid_); + likelihood_field_ = make_likelihood_field(params_, grid_); + } + private: param_type params_; OccupancyGrid grid_; @@ -159,6 +164,11 @@ class LikelihoodFieldModel : public Mixin { ValueGrid2 likelihood_field_; std::vector> points_; + static std::vector compute_free_states(const OccupancyGrid& grid) { + constexpr auto kFrame = OccupancyGrid::Frame::kGlobal; + return grid.coordinates_for(grid.free_cells(), kFrame) | ranges::to; + } + static ValueGrid2 make_likelihood_field(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid) { const auto squared_distance = [&grid, squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance]( diff --git a/beluga/test/beluga/sensor/test_beam_model.cpp b/beluga/test/beluga/sensor/test_beam_model.cpp index d8c5b4c46..cf1f62e1a 100644 --- a/beluga/test/beluga/sensor/test_beam_model.cpp +++ b/beluga/test/beluga/sensor/test_beam_model.cpp @@ -28,7 +28,7 @@ using beluga::testing::StaticOccupancyGrid; using UUT = ciabatta::mixin< ciabatta::curry>::mixin, - ciabatta::provides::mixin>; + ciabatta::provides>>::mixin>; BeamModelParam GetParams() { BeamModelParam ret; @@ -74,4 +74,41 @@ TEST(BeamSensorModel, ImportanceWeight) { mixin.update_sensor(std::vector>{{params.beam_max_range, params.beam_max_range}}); EXPECT_NEAR(0.00012500000000000003, mixin.importance_weight(grid.origin()), 1e-6); } + +TEST(BeamSensorModel, GridUpdates) { + const auto origin = Sophus::SE2d{}; + + constexpr double kResolution = 0.5; + // clang-format off + auto grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, true , false, false, + false, false, false, false, false, + false, false, false, false, false}, + kResolution, origin}; + // clang-format on + + const auto params = GetParams(); + auto mixin = UUT{params, std::move(grid)}; + + mixin.update_sensor(std::vector>{{1., 1.}}); + EXPECT_NEAR(1.0171643824743635, mixin.importance_weight(origin), 1e-6); + + // clang-format off + grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false}, + kResolution, origin}; + // clang-format on + + mixin.update_map(std::move(grid)); + + mixin.update_sensor(std::vector>{{1., 1.}}); + EXPECT_NEAR(0.0, mixin.importance_weight(origin), 1e-3); +} + } // namespace beluga diff --git a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp index fdbb11495..3818f377a 100644 --- a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp @@ -26,7 +26,7 @@ using beluga::testing::StaticOccupancyGrid; using UUT = ciabatta::mixin< ciabatta::curry>::mixin, - ciabatta::provides::mixin>; + ciabatta::provides>>::mixin>; TEST(LikelihoodFieldModel, LikelihoodField) { constexpr double kResolution = 0.5; @@ -157,4 +157,39 @@ TEST(LikelihoodFieldModel, GridWithRotationAndOffset) { ASSERT_NEAR(2.068, mixin.importance_weight(grid.origin()), 0.003); } +TEST(LikelihoodFieldModel, GridUpdates) { + const auto origin = Sophus::SE2d{}; + + constexpr double kResolution = 0.5; + // clang-format off + auto grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, true , false, false, + false, false, false, false, false, + false, false, false, false, false}, + kResolution, origin}; + // clang-format on + + const auto params = beluga::LikelihoodFieldModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + auto mixin = UUT{params, std::move(grid)}; + + mixin.update_sensor(std::vector>{{1., 1.}}); + EXPECT_NEAR(2.068577607986223, mixin.importance_weight(origin), 1e-6); + + // clang-format off + grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, true}, + kResolution, origin}; + // clang-format on + mixin.update_map(std::move(grid)); + + mixin.update_sensor(std::vector>{{1., 1.}}); + EXPECT_NEAR(1.0, mixin.importance_weight(origin), 1e-3); +} + } // namespace diff --git a/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp index c36404703..3097355a8 100644 --- a/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp @@ -33,11 +33,15 @@ #include #include +#include "beluga_amcl/occupancy_grid.hpp" #include "beluga_amcl/private/execution_policy.hpp" namespace beluga_amcl { +using LaserLocalizationInterface2d = + beluga::LaserLocalizationInterface2d; + class AmclNode : public rclcpp_lifecycle::LifecycleNode { public: @@ -54,6 +58,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override; void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr); + std::unique_ptr + make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr); void timer_callback(); template void laser_callback( @@ -66,7 +72,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr response); void reinitialize_with_pose(const Sophus::SE2d & pose, const Eigen::Matrix3d & covariance); - std::unique_ptr particle_filter_; + std::unique_ptr particle_filter_; execution::Policy execution_policy_; std::unique_ptr bond_; diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 1e75330c4..bb8d5854b 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -726,26 +726,14 @@ AmclNode::CallbackReturn AmclNode::on_shutdown(const rclcpp_lifecycle::State &) return CallbackReturn::SUCCESS; } -void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) -{ - RCLCPP_INFO(get_logger(), "A new map was received"); - - if (particle_filter_ && get_parameter("first_map_only").as_bool()) { - RCLCPP_WARN( - get_logger(), - "Ignoring new map because the particle filter has already been initialized"); - return; - } - - const auto global_frame_id = get_parameter("global_frame_id").as_string(); - if (map->header.frame_id != global_frame_id) { - RCLCPP_WARN( - get_logger(), - "Map frame \"%s\" doesn't match global frame \"%s\"", - map->header.frame_id.c_str(), - global_frame_id.c_str()); - } +template +using AdaptiveMonteCarloLocalization2d = + beluga::AdaptiveMonteCarloLocalization2d< + MotionDescriptor, SensorDescriptor, OccupancyGrid>; +std::unique_ptr +AmclNode::make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ auto sampler_params = beluga::AdaptiveSamplerParam{}; sampler_params.alpha_slow = get_parameter("recovery_alpha_slow").as_double(); sampler_params.alpha_fast = get_parameter("recovery_alpha_fast").as_double(); @@ -838,9 +826,7 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) try { using beluga::mixin::make_mixin; - using beluga::LaserLocalizationInterface2d; - using beluga::AdaptiveMonteCarloLocalization2d; - particle_filter_ = make_mixin( + return make_mixin( sampler_params, limiter_params, resample_on_motion_params, @@ -852,13 +838,44 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) ); } catch (const std::invalid_argument & error) { RCLCPP_ERROR(get_logger(), "Coudn't instantiate the particle filter: %s", error.what()); + } + return nullptr; +} + +void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + RCLCPP_INFO(get_logger(), "A new map was received"); + + if (particle_filter_ && get_parameter("first_map_only").as_bool()) { + RCLCPP_WARN( + get_logger(), + "Ignoring new map because the particle filter has already been initialized"); return; } - if (last_known_estimate_.has_value() && !get_parameter("always_reset_initial_pose").as_bool()) { - const auto & [pose, covariance] = last_known_estimate_.value(); - reinitialize_with_pose(pose, covariance); - } else if (get_parameter("set_initial_pose").as_bool()) { + const auto global_frame_id = get_parameter("global_frame_id").as_string(); + if (map->header.frame_id != global_frame_id) { + RCLCPP_WARN( + get_logger(), + "Map frame \"%s\" doesn't match global frame \"%s\"", + map->header.frame_id.c_str(), + global_frame_id.c_str()); + } + + bool should_reset_initial_pose = true; + if (!particle_filter_) { + particle_filter_ = make_particle_filter(std::move(map)); + if (last_known_estimate_.has_value() && !get_parameter("always_reset_initial_pose").as_bool()) { + const auto & [pose, covariance] = last_known_estimate_.value(); + reinitialize_with_pose(pose, covariance); + should_reset_initial_pose = false; + } + } else { + particle_filter_->update_map(OccupancyGrid{std::move(map)}); + should_reset_initial_pose = get_parameter("always_reset_initial_pose").as_bool(); + } + + if (should_reset_initial_pose && get_parameter("set_initial_pose").as_bool()) { const auto pose = Sophus::SE2d{ Sophus::SO2d{get_parameter("initial_pose.yaw").as_double()}, Eigen::Vector2d{ @@ -879,10 +896,6 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) covariance.coeffRef(2, 1) = covariance.coeffRef(1, 2); reinitialize_with_pose(pose, covariance); } - - RCLCPP_INFO( - get_logger(), "Particle filter initialized with %ld particles", - particle_filter_->particle_count()); } void AmclNode::timer_callback() @@ -1045,10 +1058,10 @@ void AmclNode::reinitialize_with_pose( return Sophus::SE2d{Sophus::SO2d{sample.z()}, Eigen::Vector2d{sample.x(), sample.y()}}; })); enable_tf_broadcast_ = true; - RCLCPP_INFO_STREAM( + RCLCPP_INFO( get_logger(), - "Particle filter initialized with initial pose x=" << - mean.x() << ", y=" << mean.y() << ", yaw=" << mean.z()); + "Particle filter initialized with %ld particles about initial pose x=%g, y=%g, yaw=%g", + particle_filter_->particle_count(), mean.x(), mean.y(), mean.z()); } catch (const std::runtime_error & error) { RCLCPP_ERROR(get_logger(), "Could not generate particles: %s", error.what()); } diff --git a/beluga_system_tests/test/test_system.cpp b/beluga_system_tests/test/test_system.cpp index 689c56ed6..f8c169a61 100644 --- a/beluga_system_tests/test/test_system.cpp +++ b/beluga_system_tests/test/test_system.cpp @@ -42,9 +42,16 @@ namespace { -using beluga::AdaptiveMonteCarloLocalization2d; -using beluga::LaserLocalizationInterface2d; -using beluga::MonteCarloLocalization2d; +using LaserLocalizationInterface2d = beluga::LaserLocalizationInterface2d; + +template +using AdaptiveMonteCarloLocalization2d = + beluga::AdaptiveMonteCarloLocalization2d; + +template +using MonteCarloLocalization2d = + beluga::MonteCarloLocalization2d; + using beluga::MultivariateNormalDistribution; struct InitialPose {