Add support for map updates in laser localization filters

Signed-off-by: Michel Hidalgo <[email protected]>

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 <class Map>
 using LaserLocalizationInterface2d = beluga::mixin::compose_interfaces<
     BaseParticleFilterInterface,
     StorageInterface<Sophus::SE2d, beluga::Weight>,
     EstimationInterface2d,
     OdometryMotionModelInterface2d,
-    LaserSensorModelInterface2d>;
+    LaserSensorModelInterface2d<Map>>;
 
 /// 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 <class MotionDescriptor, class SensorDescriptor>
+template <class MotionDescriptor, class SensorDescriptor, class Map>
 using MonteCarloLocalization2d = ciabatta::mixin<
     BootstrapParticleFilter,
     ciabatta::curry<StructureOfArrays, Sophus::SE2d, beluga::Weight>::mixin,
@@ -82,7 +87,7 @@ using MonteCarloLocalization2d = ciabatta::mixin<
     CombinedResamplingPolicy::template mixin,
     MotionDescriptor::template mixin,
     SensorDescriptor::template mixin,
-    ciabatta::provides<LaserLocalizationInterface2d>::mixin>;
+    ciabatta::provides<LaserLocalizationInterface2d<Map>>::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 <class MotionDescriptor, class SensorDescriptor>
+template <class MotionDescriptor, class SensorDescriptor, class Map>
 using AdaptiveMonteCarloLocalization2d = ciabatta::mixin<
     BootstrapParticleFilter,
     ciabatta::curry<StructureOfArrays, Sophus::SE2d, beluga::Weight, beluga::Cluster>::mixin,
@@ -103,7 +109,7 @@ using AdaptiveMonteCarloLocalization2d = ciabatta::mixin<
     CombinedResamplingPolicy::template mixin,
     MotionDescriptor::template mixin,
     SensorDescriptor::template mixin,
-    ciabatta::provides<LaserLocalizationInterface2d>::mixin>;
+    ciabatta::provides<LaserLocalizationInterface2d<Map>>::template mixin>;
 
 }  // namespace beluga
 

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 <class Map>
 struct LaserSensorModelInterface2d {
   /// Measurement type of the sensor: a point cloud for the range finder.
   using measurement_type = std::vector<std::pair<double, double>>;
@@ -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

beluga/include/beluga/sensor/beam_model.hpp

@@ -91,12 +91,7 @@ class BeamSensorModel : public Mixin {
    */
   template <class... Args>
   explicit BeamSensorModel(const param_type& params, const OccupancyGrid& grid, Args&&... rest)
-      : Mixin(std::forward<Args>(rest)...),
-        params_{params},
-        grid_{grid},
-        free_states_{
-            grid_.coordinates_for(grid_.free_cells(), OccupancyGrid::Frame::kGlobal) |
-            ranges::to<std::vector<Eigen::Vector2d>>} {}
+      : Mixin(std::forward<Args>(rest)...), params_{params}, grid_{grid}, 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 +109,7 @@ class BeamSensorModel : public Mixin {
    */
   template <class Generator>
   [[nodiscard]] state_type make_random_state(Generator& gen) const {
+    const auto lock = std::shared_lock<std::shared_mutex>{mutex_};
     auto index_distribution = std::uniform_int_distribution<std::size_t>{0, free_states_.size() - 1};
     return Sophus::SE2d{Sophus::SO2d::sampleUniform(gen), free_states_[index_distribution(gen)]};
   }
@@ -124,7 +120,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<std::shared_mutex>{points_mutex_};
+    const auto lock = std::shared_lock<std::shared_mutex>{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 +162,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<std::shared_mutex>{points_mutex_};
+    const auto lock = std::lock_guard<std::shared_mutex>{mutex_};
     points_ = std::move(points);
   }
 
+  /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map)
+  void update_map(OccupancyGrid&& grid) final {
+    const auto lock = std::lock_guard<std::shared_mutex>{mutex_};
+    grid_ = std::move(grid);
+    free_states_ = compute_free_states(grid_);
+  }
+
  private:
   param_type params_;
   OccupancyGrid grid_;
   std::vector<Eigen::Vector2d> free_states_;
   std::vector<std::pair<double, double>> points_;
-  mutable std::shared_mutex points_mutex_;
+  mutable std::shared_mutex mutex_;
+
+  static std::vector<Eigen::Vector2d> compute_free_states(const OccupancyGrid& grid) {
+    constexpr auto kFrame = OccupancyGrid::Frame::kGlobal;
+    return grid.coordinates_for(grid.free_cells(), kFrame) | ranges::to<std::vector>;
+  }
 };
 
 }  // namespace beluga

beluga/include/beluga/sensor/likelihood_field_model.hpp

@@ -98,9 +98,7 @@ class LikelihoodFieldModel : public Mixin {
       : Mixin(std::forward<Args>(rest)...),
         params_{params},
         grid_{grid},
-        free_states_{
-            grid_.coordinates_for(grid_.free_cells(), OccupancyGrid::Frame::kGlobal) |
-            ranges::to<std::vector<Eigen::Vector2d>>},
+        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 <class Generator>
   [[nodiscard]] state_type make_random_state(Generator& gen) const {
+    const auto lock = std::shared_lock<std::shared_mutex>{mutex_};
     auto index_distribution = std::uniform_int_distribution<std::size_t>{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<std::shared_mutex>{points_mutex_};
+    const auto lock = std::shared_lock<std::shared_mutex>{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<std::shared_mutex>{points_mutex_};
+    const auto lock = std::lock_guard<std::shared_mutex>{mutex_};
     points_ = std::move(points);
   }
 
+  /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map)
+  void update_map(OccupancyGrid&& grid) final {
+    const auto lock = std::lock_guard<std::shared_mutex>{mutex_};
+    grid_ = std::move(grid);
+    free_states_ = compute_free_states(grid_);
+    likelihood_field_ = make_likelihood_field(params_, grid_);
+  }
+
  private:
   param_type params_;
   OccupancyGrid grid_;
   std::vector<Eigen::Vector2d> free_states_;
   ValueGrid2<double> likelihood_field_;
   std::vector<std::pair<double, double>> points_;
-  mutable std::shared_mutex points_mutex_;
+  mutable std::shared_mutex mutex_;
+
+  static std::vector<Eigen::Vector2d> compute_free_states(const OccupancyGrid& grid) {
+    constexpr auto kFrame = OccupancyGrid::Frame::kGlobal;
+    return grid.coordinates_for(grid.free_cells(), kFrame) | ranges::to<std::vector>;
+  }
 
   static ValueGrid2<double> make_likelihood_field(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid) {
     const auto squared_distance = [&grid,

beluga/test/beluga/sensor/test_beam_model.cpp

@@ -28,7 +28,7 @@ using beluga::testing::StaticOccupancyGrid;
 
 using UUT = ciabatta::mixin<
     ciabatta::curry<beluga::BeamSensorModel, StaticOccupancyGrid<5, 5>>::mixin,
-    ciabatta::provides<beluga::LaserSensorModelInterface2d>::mixin>;
+    ciabatta::provides<beluga::LaserSensorModelInterface2d<StaticOccupancyGrid<5, 5>>>::mixin>;
 
 BeamModelParam GetParams() {
   BeamModelParam ret;

beluga/test/beluga/sensor/test_likelihood_field_model.cpp

@@ -26,7 +26,7 @@ using beluga::testing::StaticOccupancyGrid;
 
 using UUT = ciabatta::mixin<
     ciabatta::curry<beluga::LikelihoodFieldModel, StaticOccupancyGrid<5, 5>>::mixin,
-    ciabatta::provides<beluga::LaserSensorModelInterface2d>::mixin>;
+    ciabatta::provides<beluga::LaserSensorModelInterface2d<StaticOccupancyGrid<5, 5>>>::mixin>;
 
 TEST(LikelihoodFieldModel, LikelihoodField) {
   constexpr double kResolution = 0.5;

beluga_amcl/include/beluga_amcl/private/amcl_node.hpp

@@ -33,11 +33,15 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <std_srvs/srv/empty.hpp>
 
+#include "beluga_amcl/occupancy_grid.hpp"
 #include "beluga_amcl/private/execution_policy.hpp"
 
 namespace beluga_amcl
 {
 
+using LaserLocalizationInterface2d =
+  beluga::LaserLocalizationInterface2d<OccupancyGrid>;
+
 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<LaserLocalizationInterface2d>
+  make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr);
   void timer_callback();
   template<typename ExecutionPolicy>
   void laser_callback(
@@ -66,7 +72,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
   void reinitialize_with_pose(const Sophus::SE2d & pose, const Eigen::Matrix3d & covariance);
 
-  std::unique_ptr<beluga::LaserLocalizationInterface2d> particle_filter_;
+  std::unique_ptr<LaserLocalizationInterface2d> particle_filter_;
   execution::Policy execution_policy_;
 
   std::unique_ptr<bond::Bond> bond_;

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<class MotionDescriptor, class SensorDescriptor>
+using AdaptiveMonteCarloLocalization2d =
+  beluga::AdaptiveMonteCarloLocalization2d<
+  MotionDescriptor, SensorDescriptor, OccupancyGrid>;
 
+std::unique_ptr<LaserLocalizationInterface2d>
+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<LaserLocalizationInterface2d, AdaptiveMonteCarloLocalization2d>(
+    return make_mixin<LaserLocalizationInterface2d, AdaptiveMonteCarloLocalization2d>(
       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());
   }

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<beluga_amcl::OccupancyGrid>;
+
+template <class MotionDescriptor, class SensorDescriptor>
+using AdaptiveMonteCarloLocalization2d =
+    beluga::AdaptiveMonteCarloLocalization2d<MotionDescriptor, SensorDescriptor, beluga_amcl::OccupancyGrid>;
+
+template <class MotionDescriptor, class SensorDescriptor>
+using MonteCarloLocalization2d =
+    beluga::MonteCarloLocalization2d<MotionDescriptor, SensorDescriptor, beluga_amcl::OccupancyGrid>;
+
 using beluga::MultivariateNormalDistribution;
 
 struct InitialPose {