robot_state.cpp

Go to the documentation of this file.

#include "model/robot_state.hpp"

namespace directSearchWS {


RobotState::RobotState(PosePtr robotPosePtr, PtuTuplePtrVecPtr ptuTuplePtrVecPtr):
    robotPosePtr(robotPosePtr), ptuTuplePtrVecPtr(ptuTuplePtrVecPtr){
}

//Getter
PosePtr RobotState::getRobotPosePtr() const {
    return robotPosePtr;
}

PtuTuplePtr RobotState::getTopPtuTuplePtr() const {
    return ptuTuplePtrVecPtr->front();
}

PtuTuplePtrVecPtr RobotState::getPtuTuplePtrVecPtr() const {
    return ptuTuplePtrVecPtr;
}

void RobotState::eraseFrontPTUTuplePtr() {
    ptuTuplePtrVecPtr->erase(ptuTuplePtrVecPtr->begin());
}

int RobotState::getPtuListSize() const {
    return ptuTuplePtrVecPtr->size();
}


std::ostream& operator<<(std::ostream &strm, const RobotState &robot_state) {
    strm << "RobotPose:\n" << robot_state.getRobotPosePtr();
    strm << "PtuTuples:\n" << robot_state.getPtuTuplePtrVecPtr();
    return strm;
}

std::ostream& operator<<(std::ostream &strm, const RobotStatePtr &robot_state_ptr) {
    return strm << *robot_state_ptr;
}

std::ostream& operator<<(std::ostream &strm, const RobotStatePtrVec &robot_state_ptr_vec) {
    for (const RobotStatePtr &robot_state_ptr : robot_state_ptr_vec) {
        strm << robot_state_ptr;
    }
    return strm;
}

std::ostream& operator<<(std::ostream &strm, const RobotStatePtrVecPtr &robot_state_ptr_vec_ptr) {
    return strm << *robot_state_ptr_vec_ptr;
}

}