EnvironmentXYZTheta.hpp

Go to the documentation of this file.

#pragma once
 
#include <sbpl/discrete_space_information/environment.h>
#undef DEBUG //sbpl defines DEBUG 0 but the word debug is also used in base-logging which is included from TraversabilityGenerator3d
#include <traversability_generator3d/TraversabilityGenerator3d.hpp>
#include <maps/grid/TraversabilityMap3d.hpp>
#include <base/Pose.hpp>
#include "DiscreteTheta.hpp"
#include "PreComputedMotions.hpp"
#include <trajectory_follower/SubTrajectory.hpp>
 
std::ostream& operator<< (std::ostream& stream, const DiscreteTheta& angle);
 
namespace ugv_nav4d
{
 
    class StateCreationFailed : public std::runtime_error {using std::runtime_error::runtime_error;};
    class NodeCreationFailed : public std::runtime_error {using std::runtime_error::runtime_error;};
    class ObstacleCheckFailed : public std::runtime_error {using std::runtime_error::runtime_error;};
    class OrientationNotAllowed : public std::runtime_error {using std::runtime_error::runtime_error;};
 
 
class EnvironmentXYZTheta : public DiscreteSpaceInformation
{
public:
    typedef traversability_generator3d::TraversabilityGenerator3d::MLGrid MLGrid;
protected:
    traversability_generator3d::TraversabilityGenerator3d travGen;
    std::shared_ptr<MLGrid > mlsGrid;
 
    struct EnvironmentXYZThetaException : public SBPL_Exception
    {
      EnvironmentXYZThetaException(const std::string& what) :
          msg("SBPL has encountered a fatal error: " + what){}
 
        virtual const char* what() const throw()
        {
            return msg.c_str();
        }
      const std::string msg;
    };
 
 
    struct ThetaNode
    {
            ThetaNode(const DiscreteTheta &t) :theta(t) {};
            int id;
            DiscreteTheta theta;
    };
 
    struct PlannerData
    {
        PlannerData() : travNode(nullptr) {};
 
        traversability_generator3d::TravGenNode *travNode;
 
        std::map<DiscreteTheta, ThetaNode *> thetaToNodes;
    };
 
    struct Distance
    {
        double distToStart = 0;
        double distToGoal = 0;
        Distance(double toStart, double toGoal) : distToStart(toStart), distToGoal(toGoal){}
    };
 
    typedef maps::grid::TraversabilityNode<PlannerData> XYZNode;
 
    //search space without theta
    maps::grid::TraversabilityMap3d<XYZNode *> searchGrid;
 
    struct Hash
    {
        Hash(XYZNode *node, ThetaNode *thetaNode) : node(node), thetaNode(thetaNode)
        {
        }
        XYZNode *node; 
        ThetaNode *thetaNode;
    };
 
    std::vector<Hash> idToHash;
 
    std::vector<Distance> travNodeIdToDistance;
 
    PreComputedMotions availableMotions;
 
    ThetaNode *startThetaNode;
    XYZNode *startXYZNode; //part of the start state
    ThetaNode *goalThetaNode;
    XYZNode *goalXYZNode; //part of the goal state
 
    ThetaNode *createNewState(const DiscreteTheta& curTheta, EnvironmentXYZTheta::XYZNode* curNode);
    XYZNode *createNewXYZState(traversability_generator3d::TravGenNode* travNode);
    ThetaNode *createNewStateFromPose(const std::string& name, const Eigen::Vector3d& pos, double theta, ugv_nav4d::EnvironmentXYZTheta::XYZNode** xyzBackNode);
 
    bool checkStartGoalNode(const std::string& name, traversability_generator3d::TravGenNode* node, double theta);
 
public:
 
    bool obstacleCheck(const maps::grid::Vector3d& pos, double theta, const traversability_generator3d::TraversabilityGenerator3d& travGen,
                              const traversability_generator3d::TraversabilityConfig& travConf,
                              const sbpl_spline_primitives::SplinePrimitivesConfig& splineConf,
                              const std::string& nodeName="node");
    Eigen::Vector3d robotHalfSize;
 
    EnvironmentXYZTheta(std::shared_ptr<MLGrid > mlsGrid,
                        const traversability_generator3d::TraversabilityConfig &travConf,
                        const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig,
                        const Mobility& mobilityConfig);
 
    virtual ~EnvironmentXYZTheta();
 
    void updateMap(std::shared_ptr<MLGrid > mlsGrid);
    void setInitialPatch(const Eigen::Affine3d &ground2Mls, double patchRadius);
 
    virtual bool InitializeEnv(const char* sEnvFile);
    virtual bool InitializeMDPCfg(MDPConfig* MDPCfg);
 
 
    void expandMap(const std::vector<Eigen::Vector3d>& positions);
 
    std::shared_ptr<trajectory_follower::SubTrajectory> findTrajectoryOutOfObstacle(const Eigen::Vector3d& start, double theta,
            const Eigen::Affine3d& ground2Body);
 
 
    virtual int GetFromToHeuristic(int FromStateID, int ToStateID);
    virtual int GetStartHeuristic(int stateID);
    virtual int GetGoalHeuristic(int stateID);
 
    virtual void GetPreds(int TargetStateID, std::vector< int >* PredIDV, std::vector< int >* CostV);
    virtual void GetSuccs(int SourceStateID, std::vector< int >* SuccIDV, std::vector< int >* CostV);
    virtual void GetSuccs(int SourceStateID, std::vector< int >* SuccIDV, std::vector< int >* CostV, std::vector< size_t >& motionIdV);
 
    virtual void PrintEnv_Config(FILE* fOut);
    virtual void PrintState(int stateID, bool bVerbose, FILE* fOut = 0);
 
    virtual void SetAllActionsandAllOutcomes(CMDPSTATE* state);
    virtual void SetAllPreds(CMDPSTATE* state);
    virtual int SizeofCreatedEnv();
 
    void setStart(const Eigen::Vector3d &startPos, double theta);
    void setGoal(const Eigen::Vector3d &goalPos, double theta);
 
    maps::grid::Vector3d getStatePosition(const int stateID) const;
 
 
    const Motion& getMotion(const int fromStateID, const int toStateID);
 
    const maps::grid::TraversabilityMap3d<traversability_generator3d::TravGenNode *> &getTraversabilityMap() const;
 
    traversability_generator3d::TraversabilityGenerator3d& getTravGen();
 
    const MLGrid &getMlsMap() const;
 
    std::vector<Motion> getMotions(const std::vector<int> &stateIDPath);
 
    void getTrajectory(const std::vector<int> &stateIDPath, std::vector<trajectory_follower::SubTrajectory> &result,
                       bool setZToZero, const Eigen::Vector3d &startPos, const Eigen::Vector3d &goalPos, const double& goalHeading, const Eigen::Affine3d &plan2Body = Eigen::Affine3d::Identity());
 
    const PreComputedMotions& getAvailableMotions() const;
 
    void clear();
 
    void setTravConfig(const traversability_generator3d::TraversabilityConfig& cfg);
 
    void dijkstraComputeCost(const traversability_generator3d::TravGenNode* source, std::vector<double> &outDistances,
                             const double maxDist) const;
 
    void enablePathStatistics(bool enable);
 
private:
 
    traversability_generator3d::TravGenNode* checkTraversableHeuristic(const maps::grid::Index sourceIndex, traversability_generator3d::TravGenNode* sourceNode,
                                           const ugv_nav4d::Motion& motion, const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode* >& trMap);
 
    bool checkOrientationAllowed(const traversability_generator3d::TravGenNode* node,
                                 const base::Orientation2D& orientation) const;
 
 
    void precomputeCost();
 
    double getAvgSlope(std::vector<const traversability_generator3d::TravGenNode*> path) const;
 
    double getMaxSlope(std::vector<const traversability_generator3d::TravGenNode*> path) const;
 
 
    double getHeuristicDistance(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const;
 
    traversability_generator3d::TravGenNode* movementPossible(traversability_generator3d::TravGenNode* fromTravNode, const maps::grid::Index& fromIdx, const maps::grid::Index& toIdx);
 
    bool checkExpandTreadSafe(traversability_generator3d::TravGenNode * node);
 
    bool usePathStatistics;
 
    traversability_generator3d::TraversabilityConfig travConf;
    sbpl_spline_primitives::SplinePrimitivesConfig primitiveConfig;
 
    unsigned int numAngles;
 
    Mobility mobilityConfig;
};
 
}