filter_is_position_allowed.hpp

Go to the documentation of this file.

#pragma once

#include "filter/filter_basic.hpp"
#include "asr_robot_model_services/IsPositionAllowed.h"

namespace directSearchWS {

class FilterIsPositionAllowed : public FilterBasic {

private:
    int deleteCount;
    bool wasSuccessful;

    ros::ServiceClient isPositionAllowedServiceClient;

public:
    FilterIsPositionAllowed(const RobotStatePtrVecPtr &posesToExplorePtr) : FilterBasic(posesToExplorePtr), deleteCount(0), wasSuccessful(true) {
        ros::NodeHandle n(ros::this_node::getName());
        isPositionAllowedServiceClient = n.serviceClient<asr_robot_model_services::IsPositionAllowed>("/asr_robot_model_services/IsPositionAllowed");
    }

    virtual ~FilterIsPositionAllowed() { }

    bool needIteration() {
        deleteCount = 0;
        wasSuccessful = true;
        isPositionAllowedServiceClient.waitForExistence();
        return true;
    }

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

    bool shouldPtuTupleBeDeleted(const RobotStatePtrVec::iterator &posesToExploreIter, const PtuTuplePtrVec::iterator &ptuTuplePtrIter) {
        bool shouldDelete = false;
        asr_robot_model_services::IsPositionAllowed isPositionAllowedCall;
        isPositionAllowedCall.request.targetPosition = posesToExploreIter->get()->getRobotPosePtr()->position;
        if (isPositionAllowedServiceClient.call(isPositionAllowedCall)) {
            shouldDelete = !isPositionAllowedCall.response.isAllowed;
        } else {
            ROS_ERROR_STREAM("Service IsPositionAllowed to robot_model_services was not successful -> no filter of FilterIsPositionAllowed");
            wasSuccessful = false;
        }

        if (shouldDelete) {
            ++deleteCount;
        }
        return shouldDelete;
    }

};

typedef boost::shared_ptr<FilterIsPositionAllowed> FilterIsPositionAllowedPtr;

}