From 6c0a1a8c3d47fd9aeaadbdc985fff4543e69b1c4 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 16 May 2023 23:24:47 +0000 Subject: [PATCH] Add support for map updates in laser localization filters Signed-off-by: Michel Hidalgo --- beluga/include/beluga/localization.hpp | 16 ++-- beluga/include/beluga/sensor.hpp | 14 ++++ beluga/include/beluga/sensor/beam_model.hpp | 23 ++++-- .../beluga/sensor/likelihood_field_model.hpp | 24 ++++-- beluga/test/beluga/sensor/test_beam_model.cpp | 2 +- .../sensor/test_likelihood_field_model.cpp | 2 +- .../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, 125 insertions(+), 56 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 8955d8f88..d6ba72be9 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -53,6 +53,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>; @@ -71,6 +75,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 9b74ad0e5..61eba119c 100644 --- a/beluga/include/beluga/sensor/beam_model.hpp +++ b/beluga/include/beluga/sensor/beam_model.hpp @@ -94,9 +94,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, @@ -114,6 +112,7 @@ class BeamSensorModel : public Mixin { */ template [[nodiscard]] state_type make_random_state(Generator& gen) const { + const auto lock = std::shared_lock{mutex_}; auto index_distribution = std::uniform_int_distribution{0, free_states_.size() - 1}; return Sophus::SE2d{Sophus::SO2d::sampleUniform(gen), free_states_[index_distribution(gen)]}; } @@ -124,7 +123,7 @@ class BeamSensorModel : public Mixin { * \return Calculated importance weight. */ [[nodiscard]] weight_type importance_weight(const state_type& state) const { - const auto lock = std::shared_lock{points_mutex_}; + const auto lock = std::shared_lock{mutex_}; const auto beam = Ray2d{grid_, state, params_.beam_max_range}; return std::transform_reduce(points_.cbegin(), points_.cend(), 0.0, std::plus{}, [this, &beam](const auto& point) { // TODO(Ramiro): We're converting from range + bearing to cartesian points in the ROS node, but we want range + @@ -166,16 +165,28 @@ class BeamSensorModel : public Mixin { /// \copydoc LaserSensorModelInterface2d::update_sensor(measurement_type&& points) void update_sensor(measurement_type&& points) final { - const auto lock = std::lock_guard{points_mutex_}; + const auto lock = std::lock_guard{mutex_}; points_ = std::move(points); } + /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map) + void update_map(OccupancyGrid&& map) final { + const auto lock = std::lock_guard{mutex_}; + grid_ = std::move(map); + free_states_ = compute_free_states(grid_); + } + private: param_type params_; OccupancyGrid grid_; std::vector free_states_; std::vector> points_; - mutable std::shared_mutex points_mutex_; + mutable std::shared_mutex mutex_; + + 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 673d6d0b6..d354f6f72 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model.hpp @@ -98,9 +98,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. @@ -122,6 +120,7 @@ class LikelihoodFieldModel : public Mixin { */ template [[nodiscard]] state_type make_random_state(Generator& gen) const { + const auto lock = std::shared_lock{mutex_}; auto index_distribution = std::uniform_int_distribution{0, free_states_.size() - 1}; return Sophus::SE2d{Sophus::SO2d::sampleUniform(gen), free_states_[index_distribution(gen)]}; } @@ -133,7 +132,7 @@ class LikelihoodFieldModel : public Mixin { */ [[nodiscard]] weight_type importance_weight(const state_type& state) const { const auto transform = grid_.origin().inverse() * state; - const auto lock = std::shared_lock{points_mutex_}; + const auto lock = std::shared_lock{mutex_}; // TODO(glpuga): Investigate why AMCL and QuickMCL both use this formula for the weight. // See https://github.com/Ekumen-OS/beluga/issues/153 return std::transform_reduce( @@ -153,17 +152,30 @@ class LikelihoodFieldModel : public Mixin { /// \copydoc LaserSensorModelInterface2d::update_sensor(measurement_type&& points) void update_sensor(measurement_type&& points) final { - const auto lock = std::lock_guard{points_mutex_}; + const auto lock = std::lock_guard{mutex_}; points_ = std::move(points); } + /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map) + void update_map(OccupancyGrid&& map) final { + const auto lock = std::lock_guard{mutex_}; + grid_ = std::move(map); + free_states_ = compute_free_states(grid_); + likelihood_field_ = make_likelihood_field(params_, grid_); + } + private: param_type params_; OccupancyGrid grid_; std::vector free_states_; ValueGrid2 likelihood_field_; std::vector> points_; - mutable std::shared_mutex points_mutex_; + mutable std::shared_mutex mutex_; + + 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, diff --git a/beluga/test/beluga/sensor/test_beam_model.cpp b/beluga/test/beluga/sensor/test_beam_model.cpp index d8c5b4c46..4d06e1f08 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; diff --git a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp index fdbb11495..fd6f33310 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; 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 {