1 #ifndef SLAM_CTOR_GMAPPING_PARTICLE_FILTER_H 2 #define SLAM_CTOR_GMAPPING_PARTICLE_FILTER_H 10 #include "../../core/states/laser_scan_grid_world.h" 11 #include "../../core/particle_filter.h" 40 for (
auto &p : _pf.particles()) {
43 _pf.heaviest_particle().mark_master();
48 handle_observation(scan);
49 notify_with_pose(pose());
50 notify_with_map(map());
54 for (
auto &world : _pf.particles()) {
55 world->update_robot_pose(delta);
57 _traversed_since_last_resample += delta.
abs();
61 return _pf.heaviest_particle();
70 for (
auto &world : _pf.particles()) {
71 world->handle_observation(obs);
75 _pf.normalize_weights();
88 if (_traversed_since_last_resample.sq_dist() <= 0.5 &&
89 std::fabs(_traversed_since_last_resample.theta <= 0.2)) {
92 bool is_resampled = _pf.try_resample();
93 if (!is_resampled) {
return false; }
94 _traversed_since_last_resample.reset();
95 ensure_master_exists();
102 auto master_pred = [](std::shared_ptr<GmappingWorld> p) {
103 return p->is_master();
105 auto prts = _pf.particles();
106 bool master_survived = prts.end() !=
107 std::find_if(prts.begin(), prts.end(), master_pred);
109 if (!master_survived) {
110 _pf.heaviest_particle().mark_master();
119 #endif // header-guard
void handle_observation(TransformedLaserScan &obs) override
const SingleStateHypothesisLSGWProperties _shw_p
RobotPoseDelta _traversed_since_last_resample
RobotPoseDelta abs() const
GmappingParticleFactory(const SingleStateHypothesisLSGWProperties &shw_p, const GMappingParams &gprms)
void update_robot_pose(const RobotPoseDelta &delta) override
const WorldT & world() const override
std::shared_ptr< GmappingWorld > create_particle() override
void ensure_master_exists()
UnboundedLazyTiledGridMap MapType
const GMappingParams _gprms
ParticleFilter< GmappingWorld > _pf
void handle_sensor_data(TransformedLaserScan &scan) override
GmappingParticleFilter(const SingleStateHypothesisLSGWProperties &shw_p, const GMappingParams &gprms, unsigned n=1)
const RobotPose & pose() const override
const GmappingWorld::MapType & map() const override