Program Listing for File amcl.hpp

↰ Return to documentation for file (include/beluga_ros/amcl.hpp)

// Copyright 2024 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_ROS_AMCL_HPP
#define BELUGA_ROS_AMCL_HPP

#include <optional>
#include <utility>
#include <variant>

#include <range/v3/range/conversion.hpp>
#include <range/v3/view/take_exactly.hpp>

#include <sophus/se2.hpp>

#include <beluga/algorithm/spatial_hash.hpp>
#include <beluga/algorithm/thrun_recovery_probability_estimator.hpp>
#include <beluga/containers.hpp>
#include <beluga/motion.hpp>
#include <beluga/policies.hpp>
#include <beluga/random.hpp>
#include <beluga/sensor.hpp>
#include <beluga/views/sample.hpp>

#include <beluga_ros/laser_scan.hpp>
#include <beluga_ros/occupancy_grid.hpp>

namespace beluga_ros {

struct AmclParams {
  double update_min_d = 0.25;

  double update_min_a = 0.2;

  std::size_t resample_interval = 1UL;

  bool selective_resampling = false;

  std::size_t min_particles = 500UL;

  std::size_t max_particles = 2000UL;

  double alpha_slow = 0.001;

  double alpha_fast = 0.1;

  double kld_epsilon = 0.05;

  double kld_z = 3.0;

  double spatial_resolution_x = 0.5;

  double spatial_resolution_y = 0.5;

  double spatial_resolution_theta = 10 * Sophus::Constants<double>::pi() / 180;
};

class Amcl {
 public:
  using particle_type = std::tuple<Sophus::SE2d, beluga::Weight>;

  using motion_model_variant = std::variant<
      beluga::DifferentialDriveModel2d,   //
      beluga::OmnidirectionalDriveModel,  //
      beluga::StationaryModel>;

  using sensor_model_variant = std::variant<
      beluga::LikelihoodFieldModel<beluga_ros::OccupancyGrid>,  //
      beluga::BeamSensorModel<beluga_ros::OccupancyGrid>>;

  using execution_policy_variant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;


  Amcl(
      beluga_ros::OccupancyGrid map,
      motion_model_variant motion_model,
      sensor_model_variant sensor_model,
      const AmclParams& params,
      execution_policy_variant execution_policy);

  [[nodiscard]] const auto& particles() const { return particles_; }

  template <class Distribution>
  void initialize(Distribution distribution) {
    particles_ = beluga::views::sample(std::move(distribution)) |                    //
                 ranges::views::transform(beluga::make_from_state<particle_type>) |  //
                 ranges::views::take_exactly(params_.max_particles) |                //
                 ranges::to<beluga::TupleVector>;
    force_update_ = true;
  }


  void initialize(Sophus::SE2d pose, Sophus::Matrix3d covariance) {
    initialize(beluga::MultivariateNormalDistribution{pose, covariance});
  }

  void initialize_from_map() { initialize(std::ref(map_distribution_)); }

  void update_map(beluga_ros::OccupancyGrid map);


  auto update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_scan)
      -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>;

  void force_update() { force_update_ = true; }

 private:
  beluga::TupleVector<particle_type> particles_;

  AmclParams params_;
  beluga::MultivariateUniformDistribution<Sophus::SE2d, beluga_ros::OccupancyGrid> map_distribution_;
  motion_model_variant motion_model_;
  sensor_model_variant sensor_model_;
  execution_policy_variant execution_policy_;

  beluga::spatial_hash<Sophus::SE2d> spatial_hasher_;
  beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_;
  beluga::any_policy<Sophus::SE2d> update_policy_;
  beluga::any_policy<decltype(particles_)> resample_policy_;

  beluga::RollingWindow<Sophus::SE2d, 2> control_action_window_;

  bool force_update_{true};
};

}  // namespace beluga_ros

#endif  // BELUGA_ROS_AMCL_HPP