direct_search_handler.hpp

Go to the documentation of this file.

#pragma once

#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <tuple>
#include <memory>

#include <ros/ros.h>
#include <rapidxml.hpp>
#include <math.h>
#include <boost/lexical_cast.hpp>
#include <boost/shared_ptr.hpp>
#include <eigen3/Eigen/Geometry>

#include "tf/transform_datatypes.h"
#include "asr_msgs/AsrTypeAndId.h"

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


namespace directSearchWS {

typedef std::vector<asr_msgs::AsrTypeAndId> SearchedObjectTypesAndIds;

class DirectSearchHandler {

private:

protected:
    RobotStatePtrVecPtr posesToExplorePtr;

    PosePtr nextCameraPosePtr;
    PosePtr nextRobotPosePtr;
    PtuTuplePtr nextPtuPtr;
    SearchedObjectTypesAndIds nextFilteredSearchedObjectTypesAndIds;
    double remainingPosesDistances;
    bool isSameRobotPoseAsBefore;
    bool isRobotPoseChanged;
    bool isNoPoseLeft;
    bool arePosesFromDemonstrationLeft;

    PoseHelperPtr poseHelperPtr;

    void calculateRemainingPosesDistances();

public:
    DirectSearchHandler();
    virtual ~DirectSearchHandler();

    virtual bool initHandler() = 0;
    virtual bool resetHandler();
    virtual bool getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds); // decide next robot pose and ptu config
    virtual bool backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds);

    //getter
    PosePtr getActualRobotPosePtr() const {
        return nextRobotPosePtr;
    }

    PosePtr getActualCameraPosePtr() const {
        return nextCameraPosePtr;
    }

    PtuTuplePtr getActualPtuPtr() const {
        return nextPtuPtr;
    }

    SearchedObjectTypesAndIds getActualFilteredSearchedObjectTypesAndIds() const {
        return nextFilteredSearchedObjectTypesAndIds;
    }

    bool getIsSameRobotPoseAsBefore() const {
        return isSameRobotPoseAsBefore;
    }

    bool getIsNoPoseLeft() const {
        return isNoPoseLeft;
    }

    bool getArePosesFromDemonstrationLeft() const {
        return arePosesFromDemonstrationLeft;
    }

    int getRemainingPTUPoses() const {
        if (posesToExplorePtr->empty()) {
            return 0;
        }
        return posesToExplorePtr->front()->getPtuListSize();
    }

    int getRemainingRobotPoses() const {
        return posesToExplorePtr->size();
    }

    double getRemainingPosesDistances() const {
        return remainingPosesDistances;
    }

};

typedef boost::shared_ptr<DirectSearchHandler> DirectSearchHandlerPtr;

void filterSearchedObjectTypes(SearchedObjectTypes &searched_object_types_to_filter, const SearchedObjectTypes &filter_searched_object_types);
void filterSearchedObjectTypesAndIds(SearchedObjectTypesAndIds &searched_object_types_and_ids_to_filter,
                                     const SearchedObjectTypes &filter_searched_object_types);
SearchedObjectTypes getIntersectionOfSearchObjectTypes(const SearchedObjectTypes &first_searched_object_types,
                                                       const SearchedObjectTypes &second_searched_object_types);
void checkParametersFromOtherNode();
template <typename T>
void checkParameterFromOtherNode(ros::NodeHandle &nh, const std::string &node_name, const std::string &param_name);

}