Program Listing for File mrpt_localization_core.h

↰ Return to documentation for file (/tmp/ws/src/mrpt_navigation/mrpt_localization/include/mrpt_localization/mrpt_localization_core.h)

/* +------------------------------------------------------------------------+
   |                             mrpt_navigation                            |
   |                                                                        |
   | Copyright (c) 2014-2023, Individual contributors, see commit authors   |
   | See: https://github.com/mrpt-ros-pkg/mrpt_navigation                   |
   | All rights reserved. Released under BSD 3-Clause license. See LICENSE  |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <mrpt/system/CTicTac.h>
#include <stdint.h>

#include <iostream>
using namespace mrpt::maps;
using namespace mrpt::obs;

class PFLocalizationCore
{
   public:
    enum PFStates
    {
        NA,
        INIT,
        RUN,
        IDLE
    };

    PFLocalizationCore();
    ~PFLocalizationCore();

    void init();
    void observation(
        CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry);

   protected:
    bool use_motion_model_default_options_;
    CActionRobotMovement2D::TMotionModelOptions
        motion_model_default_options_;
    CActionRobotMovement2D::TMotionModelOptions
        motion_model_options_;
    CMultiMetricMap::Ptr metric_map_ = CMultiMetricMap::Create();
    mrpt::bayes::CParticleFilter
        pf_;
    mrpt::bayes::CParticleFilter::TParticleFilterStats
        pf_stats_;
    mrpt::slam::CMonteCarloLocalization2D pdf_;
    mrpt::poses::CPosePDFGaussian
        initial_pose_;
    int initial_particle_count_;
    mrpt::system::TTimeStamp time_last_update_;
    mrpt::system::CTicTac tictac_;
    size_t update_counter_;
    PFStates state_;
    mrpt::poses::CPose2D
        odom_last_observation_;
    bool init_PDF_mode;
    float init_PDF_min_x;
    float init_PDF_max_x;
    float init_PDF_min_y;
    float init_PDF_max_y;

   private:
    void initializeFilter();

    void updateFilter(CActionCollection::Ptr _action, CSensoryFrame::Ptr _sf);
};