filter_poses_depending_on_already_found_object_types.hpp

Go to the documentation of this file.

#pragma once

#include "filter/filter_basic.hpp"
#include "direct_search_handler.hpp"

namespace directSearchWS {

class FilterPosesDependingOnAlreadyFoundObjectTypes : public FilterBasic {

private:
    int deleteCount;
    SearchedObjectTypes searchedObjectTypes;
    bool isNeedReorder;

public:
    FilterPosesDependingOnAlreadyFoundObjectTypes(const RobotStatePtrVecPtr &posesToExplorePtr) :
        FilterBasic(posesToExplorePtr), deleteCount(0), searchedObjectTypes(), isNeedReorder(false) { }
    virtual ~FilterPosesDependingOnAlreadyFoundObjectTypes() { }

    bool needIteration() {
        deleteCount = 0;
        isNeedReorder = false;
        return true;
    }

    bool wasIterationSuccessful() {
        ROS_INFO_STREAM("Number of deleted ptuTuple by FilterPosesDependingOnAlreadyFoundObjectTypes: " << deleteCount);
        return true;
    }

    bool shouldPtuTupleBeDeleted(const RobotStatePtrVec::iterator &posesToExploreIter, const PtuTuplePtrVec::iterator &ptuTuplePtrIter) {
        SearchedObjectTypes copySearchedObjectTypes(searchedObjectTypes);
        filterSearchedObjectTypes(copySearchedObjectTypes, ptuTuplePtrIter->get()->getAlreadySearchedObjectTypes());
        // Check if every searchedObjectTypes is present in alreadySearchedObjectTypes
        bool shouldDelete = copySearchedObjectTypes.empty();
        if (shouldDelete) {
            ++deleteCount;
            isNeedReorder = true;
        }
        return shouldDelete;
    }

    void updateSearchedObjectTypes(const SearchedObjectTypes &newSearchedObjectTypes) {
        searchedObjectTypes = newSearchedObjectTypes;
    }

    bool needReorder() const {
        return isNeedReorder;
    }

};

typedef boost::shared_ptr<FilterPosesDependingOnAlreadyFoundObjectTypes> FilterPosesDependingOnAlreadyFoundObjectTypesPtr;

}