HRICS_rrtPlanExpansion.hpp

#ifndef HRICS_RRTPLANEXPANSION_H
#define HRICS_RRTPLANEXPANSION_H

#include "API/planningAPI.hpp"
#include "planner/Diffusion/Variants/Transition-RRT.hpp"
#include "HRI_costspace/Grid/HRICS_TwoDGrid.hpp"

class HRICS_rrtPlanExpansion : public TransitionExpansion
{
public:
    HRICS_rrtPlanExpansion(Graph* ptrGraph, Node * goal);

    void init();

    void setGrid(API::TwoDGrid* grid) { m2DGrid = dynamic_cast<HRICS::PlanGrid*>(grid); }

    void setCellPath(std::vector<API::TwoDCell*> cellPath);

    std::tr1::shared_ptr<Configuration> getExpansionDirection(
            Node* expandComp, Node* goalComp, bool samplePassive, Node*& directionNode);

    std::tr1::shared_ptr<Configuration> getConfigurationInNextCell(Node* CompcoNode, Node* goalComp);

    Node* addNode(Node* currentNode, LocalPath& path, double pathDelta,
                  Node* directionNode, unsigned& nbCreatedNodes);

    bool on2DPathAndAfter(API::TwoDCell* cell);

private:
    HRICS::PlanGrid*             m2DGrid;
    std::vector<API::TwoDCell*>  m2DCellPath;

    API::TwoDCell*               mLastForward;
    API::TwoDCell*               mLastBackward;
    API::TwoDCell*               mBiasedPlanCell;

    bool                     mForward;
    bool                     mBiasing;

    double*                  mBox;

};

#endif // HRICS_RRTEXPANSION_H