do_filter_iteration.hpp

Go to the documentation of this file.

#pragma once

#include <vector>

#include <boost/shared_ptr.hpp>

#include "model/robot_state.hpp"
#include "model/ptu_tuple.hpp"

#include "filter/filter_basic.hpp"


namespace directSearchWS {

class DoFilterIteration {

private:
    std::vector<FilterBasicPtr> filtersToApply;
    const RobotStatePtrVecPtr &posesToExplorePtr;

public:
    DoFilterIteration(const RobotStatePtrVecPtr &posesToExplorePtr) : filtersToApply(), posesToExplorePtr(posesToExplorePtr) { }

    void addFilter(const FilterBasicPtr &filter) {
        filtersToApply.push_back(filter);
    }

    bool doIteration() {
        std::vector<FilterBasicPtr> filteredFiltersToApply;
        for (const FilterBasicPtr &filter_ptr : filtersToApply) {
            if (filter_ptr->needIteration()) {
                filteredFiltersToApply.push_back(filter_ptr);
            }
        }

        // Iterate over all poses and ptu_tuples and apply filters
        RobotStatePtrVec::iterator posesToExploreIter = posesToExplorePtr->begin();
        while (posesToExploreIter != posesToExplorePtr->end()) {
            PtuTuplePtrVec::iterator ptuTuplePtrIter = posesToExploreIter->get()->getPtuTuplePtrVecPtr()->begin();
            while (ptuTuplePtrIter != posesToExploreIter->get()->getPtuTuplePtrVecPtr()->end()) {
                bool shouldDelete = false;
                for (const FilterBasicPtr &filter_ptr : filteredFiltersToApply) {
                    shouldDelete |= filter_ptr->shouldPtuTupleBeDeleted(posesToExploreIter, ptuTuplePtrIter);
                    if (shouldDelete) {
                        break;
                    }
                }

                if (shouldDelete) {
                    ptuTuplePtrIter = posesToExploreIter->get()->getPtuTuplePtrVecPtr()->erase(ptuTuplePtrIter);
                } else {
                    ++ptuTuplePtrIter;
                }
            }
            if (posesToExploreIter->get()->getPtuListSize() == 0) {
                posesToExploreIter = posesToExplorePtr->erase(posesToExploreIter);
            } else {
                ++posesToExploreIter;
            }
        }

        bool wasSuccessful = true;
        for (const FilterBasicPtr &filter_ptr : filteredFiltersToApply) {
            wasSuccessful &= filter_ptr->wasIterationSuccessful();
        }
        return wasSuccessful;
    }

};

typedef boost::shared_ptr<DoFilterIteration> DoFilterIterationPtr;

}