direct_search_manager.hpp

Go to the documentation of this file.

#pragma once

#include <direct_search_handler.hpp>
#include <asr_world_model/GetViewportList.h>
#include "asr_msgs/AsrViewport.h"

#include <sensor_msgs/JointState.h>
#include <asr_next_best_view/RobotStateMessage.h>
#include <asr_next_best_view/SetInitRobotState.h>
#include <asr_next_best_view/SetAttributedPointCloud.h>
#include <asr_next_best_view/RateViewports.h>

#include "filter/filter_basic.hpp"
#include "filter/filter_poses_depending_on_already_found_object_types.hpp"
#include "filter/do_filter_iteration.hpp"
#include "filter/concat_approx_equale_poses.hpp"

#include <ISM/utility/TableHelper.hpp>


namespace directSearchWS {

class DirectSearchManager : public DirectSearchHandler {

private:
    std::string dbfilenameParam;

    ros::ServiceClient getViewportListServiceClient;

    ros::Subscriber ptuDriverStateSubscriber;
    ros::ServiceClient setInitRobotStateServiceClient;
    ros::ServiceClient setAttributedPointCloudServiceClient;
    ros::ServiceClient rateViewportsServiceClient;

    DoFilterIterationPtr doFilterIterationPtrForPosesDependingOnAlreadyFoundObjectTypes;

    double currentPan;
    double currentTilt;
    bool needSortByNBV;

    PosePtr parsePose(const rapidxml::xml_node<> *node, const char *poseName) const;
    bool getViewportsFromWorldModel(std::vector<asr_msgs::AsrViewport> &viewports);

    bool reorderPosesByNBV(const SearchedObjectTypes &searchedObjectTypes);
    bool deleteAllRedundantPoses(const RobotStatePtrVecPtr &robotStates) const;
    bool filterPosesDependingOnAlreadyFoundObjectTypes(const RobotStatePtrVecPtr &robotStates, const SearchedObjectTypes &searchedObjectTypes) const;

    bool setInitialRobotState();
    bool setPointCloudInNBV(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds);
    void ptuDriverStateCallback(const sensor_msgs::JointState::ConstPtr &ptuState);

protected:
    SearchedObjectTypes lastSearchedObjectTypes;

    bool reorderPosesByNBVParam;

    FilterPosesDependingOnAlreadyFoundObjectTypesPtr filterPosesDependingOnAlreadyFoundObjectTypesPtr;

    bool parsePosesToExploreFromXML(const std::string &path);

public:
    DirectSearchManager();
    virtual ~DirectSearchManager();

    virtual bool initHandler() = 0;
    virtual bool resetHandler();
    virtual bool getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds);
    virtual bool backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds);

};

SearchedObjectTypes getSearchedObjectTypesFromAWithoutB(const SearchedObjectTypes &a, const SearchedObjectTypes &b);
bool checkSearchedObjectTypesAreEquale(const SearchedObjectTypes &searchecObjectTypes1, const SearchedObjectTypes &searchecObjectTypes2);

}