/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/amcl.hpp Source File

/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/amcl.hpp Source File#

Beluga ROS: /home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/amcl.hpp Source File
Beluga ROS
amcl.hpp
Go to the documentation of this file.
1 // Copyright 2024 Ekumen, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef BELUGA_ROS_AMCL_HPP
16 #define BELUGA_ROS_AMCL_HPP
17 
18 #include <optional>
19 #include <utility>
20 #include <variant>
21 
22 #include <range/v3/range/conversion.hpp>
23 #include <range/v3/view/take_exactly.hpp>
24 
25 #include <sophus/se2.hpp>
26 
28 #include <beluga/algorithm/thrun_recovery_probability_estimator.hpp>
29 #include <beluga/containers.hpp>
30 #include <beluga/motion.hpp>
31 #include <beluga/policies.hpp>
32 #include <beluga/random.hpp>
33 #include <beluga/sensor.hpp>
34 #include <beluga/views/sample.hpp>
35 
38 
44 namespace beluga_ros {
45 
47 struct AmclParams {
49  double update_min_d = 0.25;
50 
52  double update_min_a = 0.2;
53 
55  std::size_t resample_interval = 1UL;
56 
62  bool selective_resampling = false;
63 
65  std::size_t min_particles = 500UL;
66 
68  std::size_t max_particles = 2000UL;
69 
72  double alpha_slow = 0.001;
73 
76  double alpha_fast = 0.1;
77 
81  double kld_epsilon = 0.05;
82 
85  double kld_z = 3.0;
86 
88  double spatial_resolution_x = 0.5;
89 
91  double spatial_resolution_y = 0.5;
92 
94  double spatial_resolution_theta = 10 * Sophus::Constants<double>::pi() / 180;
95 };
96 
99 class Amcl {
100  public:
102  using particle_type = std::tuple<Sophus::SE2d, beluga::Weight>;
103 
105  using motion_model_variant = std::variant<
109 
111  using sensor_model_variant = std::variant<
114 
116  using execution_policy_variant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
117 
119 
128  motion_model_variant motion_model,
129  sensor_model_variant sensor_model,
130  const AmclParams& params,
131  execution_policy_variant execution_policy);
132 
134  [[nodiscard]] const auto& particles() const { return particles_; }
135 
137  template <class Distribution>
138  void initialize(Distribution distribution) {
139  particles_ = beluga::views::sample(std::move(distribution)) | //
140  ranges::views::transform(beluga::make_from_state<particle_type>) | //
141  ranges::views::take_exactly(params_.max_particles) | //
142  ranges::to<beluga::TupleVector>;
143  force_update_ = true;
144  }
145 
147 
150  void initialize(Sophus::SE2d pose, Sophus::Matrix3d covariance) {
152  }
153 
155  void initialize_from_map() { initialize(std::ref(map_distribution_)); }
156 
159 
161 
173  auto update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_scan)
174  -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>;
175 
177  void force_update() { force_update_ = true; }
178 
179  private:
181 
182  AmclParams params_;
184  motion_model_variant motion_model_;
185  sensor_model_variant sensor_model_;
186  execution_policy_variant execution_policy_;
187 
188  beluga::spatial_hash<Sophus::SE2d> spatial_hasher_;
189  beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_;
190  beluga::any_policy<Sophus::SE2d> update_policy_;
191  beluga::any_policy<decltype(particles_)> resample_policy_;
192 
193  beluga::RollingWindow<Sophus::SE2d, 2> control_action_window_;
194 
195  bool force_update_{true};
196 };
197 
198 } // namespace beluga_ros
199 
200 #endif // BELUGA_ROS_AMCL_HPP
Definition: amcl.hpp:99
void initialize_from_map()
Initialize particles using the default map distribution.
Definition: amcl.hpp:155
std::variant< beluga::DifferentialDriveModel2d, beluga::OmnidirectionalDriveModel, beluga::StationaryModel > motion_model_variant
Motion model variant type for runtime selection support.
Definition: amcl.hpp:108
std::variant< std::execution::sequenced_policy, std::execution::parallel_policy > execution_policy_variant
Execution policy variant type for runtime selection support.
Definition: amcl.hpp:116
void update_map(beluga_ros::OccupancyGrid map)
Update the map used for localization.
const auto & particles() const
Returns a reference to the current set of particles.
Definition: amcl.hpp:134
std::tuple< Sophus::SE2d, beluga::Weight > particle_type
Weighted SE(2) state particle type.
Definition: amcl.hpp:102
std::variant< beluga::LikelihoodFieldModel< beluga_ros::OccupancyGrid >, beluga::BeamSensorModel< beluga_ros::OccupancyGrid > > sensor_model_variant
Sensor model variant type for runtime selection support.
Definition: amcl.hpp:113
void initialize(Sophus::SE2d pose, Sophus::Matrix3d covariance)
Initialize particles with a given pose and covariance.
Definition: amcl.hpp:150
void initialize(Distribution distribution)
Initialize particles using a custom distribution.
Definition: amcl.hpp:138
auto update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_scan) -> std::optional< std::pair< Sophus::SE2d, Sophus::Matrix3d >>
Update particles based on motion and sensor information.
Amcl(beluga_ros::OccupancyGrid map, motion_model_variant motion_model, sensor_model_variant sensor_model, const AmclParams &params, execution_policy_variant execution_policy)
Constructor.
void force_update()
Force a manual update of the particles on the next iteration of the filter.
Definition: amcl.hpp:177
Thin wrapper type for 2D sensor_msgs/LaserScan messages.
Definition: laser_scan.hpp:34
Thin wrapper type for 2D nav_msgs/OccupancyGrid messages.
Definition: occupancy_grid.hpp:47
Implementation of sensor_msgs/LaserScan wrapper type.
The main Beluga ROS namespace.
Definition: amcl.hpp:44
constexpr detail::covariance_fn covariance
DifferentialDriveModel< Sophus::SE2d > DifferentialDriveModel2d
Implementation of nav_msgs/OccupancyGrid wrapper type.
Struct containing parameters for the Adaptive Monte Carlo Localization (AMCL) implementation.
Definition: amcl.hpp:47
std::size_t max_particles
Maximum allowed number of particles.
Definition: amcl.hpp:68
std::size_t resample_interval
Number of filter updates required before resampling.
Definition: amcl.hpp:55
double kld_epsilon
Maximum particle filter population error between the true distribution and the estimated distribution...
Definition: amcl.hpp:81
double kld_z
Upper standard normal quantile for , where is the probability that the error in the estimated distri...
Definition: amcl.hpp:85
double alpha_fast
Exponential decay rate for the fast average weight filter, used in deciding when to recover from a ba...
Definition: amcl.hpp:76
double update_min_a
Rotational movement required from last resample for resampling to happen again.
Definition: amcl.hpp:52
double alpha_slow
Exponential decay rate for the slow average weight filter, used in deciding when to recover from a ba...
Definition: amcl.hpp:72
double spatial_resolution_theta
Spatial resolution around the z-axis to create buckets for KLD resampling.
Definition: amcl.hpp:94
double spatial_resolution_x
Spatial resolution along the x-axis to create buckets for KLD resampling.
Definition: amcl.hpp:88
std::size_t min_particles
Minimum allowed number of particles.
Definition: amcl.hpp:65
double spatial_resolution_y
Spatial resolution along the y-axis to create buckets for KLD resampling.
Definition: amcl.hpp:91
bool selective_resampling
Whether to enable selective resampling to help avoid loss of diversity in the particle population....
Definition: amcl.hpp:62
double update_min_d
Translational movement required from last resample for resampling to happen again.
Definition: amcl.hpp:49