multirobot_path_finder.h

#pragma once
 
 
#include "base/path_finder.h"
#include "base/node_grid_path_finder.h"
#include "one_direction_sync_path_finder.h"
#include "one_direction_ordered_path_finder.h"
 
class MultiRobotPathFinder : public bmpf::PathFinder {
public:
 
    MultiRobotPathFinder(
            const std::shared_ptr<bmpf::Scene> &scene, bool showTrace,
            unsigned int maxOpenSetSize, int gridSize, unsigned int maxNodeCnt, int threadCnt = 1
    ) : PathFinder(scene, showTrace, threadCnt),
        _maxOpenSetSize(maxOpenSetSize), _maxNodeCnt(maxNodeCnt), _gridSize(gridSize),
        _scaleGridSize(gridSize) {
        _ready = true;
    };
 
    virtual ~MultiRobotPathFinder() {
        _singleRobotPathFinders.clear();
        _pfsNotReadyIndexes.clear();
    }
 
    void paint(const std::vector<double> &state, bool onlyRobot) override;
 
    bool findTick(std::vector<double> &state) override;
 
    void prepare(const std::vector<double> &startState, const std::vector<double> &endState) override;
 
    void buildPath() override;
 
    std::vector<std::vector<double>> checkTask(
            const std::vector<double> &startState, const std::vector<double> &endState,
            double opacity
    );
 
 
protected:
    std::vector<std::shared_ptr<bmpf::NodeGridPathFinder>> _singleRobotPathFinders;
    unsigned int _maxNodeCnt;
    unsigned int _maxOpenSetSize;
    int _gridSize;
    int _scaleGridSize;
    std::vector<double> _prevState;
    std::unordered_set<int> _pfsNotReadyIndexes;
    std::shared_ptr<bmpf::NodeGridPathFinder> _wholeScenePathFinder;
    std::shared_ptr<bmpf::NodeGridPathFinder> _scaleWholeScenePathFinder;
};