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 "ReedsShepp.hpp"
#include <trajectory_follower/SubTrajectory.hpp>
#include <unordered_map>
#include <chrono>
 
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
{
protected:
 
    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;
    std::vector<bool> nodeInCorridor;
    double corridorWidth;
    std::shared_ptr<const traversability_generator3d::TravMap3d> travMap;
 
    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::TraversabilityConfig& travConf,
                              const sbpl_spline_primitives::SplinePrimitivesConfig& splineConf,
                              const std::string& nodeName="node");
    Eigen::Vector3d robotHalfSize;
 
    EnvironmentXYZTheta(std::shared_ptr<const traversability_generator3d::TravMap3d > travMap,
                        const traversability_generator3d::TraversabilityConfig &travConf,
                        const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig,
                        const Mobility& mobilityConfig);
 
    virtual ~EnvironmentXYZTheta();
 
    void updateMap(std::shared_ptr<const traversability_generator3d::TravMap3d > travMap);
 
    void setPlanningTimeout(const std::chrono::steady_clock::time_point& start, double maxTime)
    {
        planningStartTime = start;
        planningMaxTime = maxTime;
    }
 
    virtual bool InitializeEnv(const char* sEnvFile);
    virtual bool InitializeMDPCfg(MDPConfig* MDPCfg);
 
    std::shared_ptr<trajectory_follower::SubTrajectory> findTrajectoryOutOfObstacle(const Eigen::Vector3d& start, double theta,
            const Eigen::Affine3d& ground2Body, bool setZToZero);
 
 
    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;
 
    traversability_generator3d::TravGenNode* findMatchingTraversabilityPatchAt(const Eigen::Vector3d &pos);
 
    const Motion& getMotion(const int fromStateID, const int toStateID);
 
    const std::shared_ptr<const traversability_generator3d::TravMap3d> getTraversabilityMap() 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());
 
    void getTrajectoryReedsShepp(const std::vector<int> &stateIDPath, std::vector<trajectory_follower::SubTrajectory> &result,
                       bool setZToZero, const Eigen::Vector3d &startPos, const double& startHeading,
                       const Eigen::Vector3d &goalPos, const double& goalHeading,
                       const Eigen::Affine3d &plan2Body, double stepSize, int maxShortcut = 0);
 
    const PreComputedMotions& getAvailableMotions() const;
 
    void clear();
 
    void setTravConfig(const traversability_generator3d::TraversabilityConfig& cfg);
 
    void setReedsSheppGoalShot(bool enable, double maxDistance, double stepSize);
 
    void setReedsSheppMaxCusps(int maxCusps);
 
    void setCorridorWidth(double width);
    void setGoalOrientationMargin(double margin);
    void setGoalDistanceMargin(double margin);
 
    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 validateReedsSheppSegment(traversability_generator3d::TravGenNode* startNode,
                                   const std::vector<RSSample>& samples,
                                   const traversability_generator3d::TravGenNode* expectedEndNode,
                                   std::vector<traversability_generator3d::TravGenNode*>& outNodes);
 
    bool checkOrientationAllowed(const traversability_generator3d::TravGenNode* node,
                                 const base::Orientation2D& orientation) const;
 
 
    void precomputeCost();
 
    double getAvgSlope(const std::vector<const traversability_generator3d::TravGenNode*>& path) const;
 
    double getMaxSlope(const 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;
    struct CachedTransition {
        size_t motionId;
        int cost;
    };
    std::unordered_map<uint64_t, CachedTransition> transitionCache;
    double goalOrientationMargin;
    double goalDistanceMargin;
 
    std::chrono::steady_clock::time_point planningStartTime;
    double planningMaxTime;
 
    bool useReedsSheppGoalShot = false;
    double reedsSheppGoalShotMaxDistance = 0.0;
    double reedsSheppStepSize = 0.0;
    int reedsSheppMaxCusps = 1;
};
 
}