3 #include <sbpl/discrete_space_information/environment.h>
5 #include <traversability_generator3d/TraversabilityGenerator3d.hpp>
7 #include <maps/grid/TraversabilityMap3d.hpp>
8 #include <base/Pose.hpp>
11 #include <trajectory_follower/SubTrajectory.hpp>
27 typedef traversability_generator3d::TraversabilityGenerator3d::MLGrid
MLGrid;
29 traversability_generator3d::TraversabilityGenerator3d
travGen;
36 msg(
"SBPL has encountered a fatal error: " +
what){}
38 virtual const char*
what()
const throw()
42 const std::string
msg;
60 traversability_generator3d::TravGenNode *
travNode;
77 typedef maps::grid::TraversabilityNode<PlannerData>
XYZNode;
113 bool checkStartGoalNode(
const std::string& name, traversability_generator3d::TravGenNode* node,
double theta);
117 traversability_generator3d::TravGenNode*
findObstacleNode(
const traversability_generator3d::TravGenNode* travNode)
const;
123 const traversability_generator3d::TraversabilityConfig& travConf,
124 const sbpl_spline_primitives::SplinePrimitivesConfig& splineConf,
125 const std::string& nodeName=
"node");
131 const traversability_generator3d::TraversabilityConfig &travConf,
132 const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig,
138 void setInitialPatch(
const Eigen::Affine3d &ground2Mls,
double patchRadius);
145 void expandMap(
const std::vector<Eigen::Vector3d>& positions);
154 const Eigen::Affine3d& ground2Body);
171 virtual void GetPreds(
int TargetStateID, std::vector< int >* PredIDV, std::vector< int >* CostV);
172 virtual void GetSuccs(
int SourceStateID, std::vector< int >* SuccIDV, std::vector< int >* CostV);
173 virtual void GetSuccs(
int SourceStateID, std::vector< int >* SuccIDV, std::vector< int >* CostV, std::vector< size_t >& motionIdV);
176 virtual void PrintState(
int stateID,
bool bVerbose, FILE* fOut = 0);
182 void setStart(
const Eigen::Vector3d &startPos,
double theta);
183 void setGoal(
const Eigen::Vector3d &goalPos,
double theta);
192 const maps::grid::TraversabilityMap3d<traversability_generator3d::TravGenNode *> &
getTraversabilityMap()
const;
193 const maps::grid::TraversabilityMap3d<traversability_generator3d::TravGenNode *> &
getObstacleMap()
const;
195 traversability_generator3d::TraversabilityGenerator3d&
getTravGen();
196 traversability_generator3d::TraversabilityGenerator3d&
getObstacleGen();
200 std::vector<Motion>
getMotions(
const std::vector<int> &stateIDPath);
202 void getTrajectory(
const std::vector<int> &stateIDPath, std::vector<trajectory_follower::SubTrajectory> &result,
203 bool setZToZero,
const Eigen::Vector3d &startPos,
const Eigen::Vector3d &goalPos,
const double& goalHeading,
const Eigen::Affine3d &plan2Body = Eigen::Affine3d::Identity());
210 void setTravConfig(
const traversability_generator3d::TraversabilityConfig& cfg);
214 void dijkstraComputeCost(
const traversability_generator3d::TravGenNode* source, std::vector<double> &outDistances,
215 const double maxDist)
const;
225 traversability_generator3d::TravGenNode* checkTraversableHeuristic(
const maps::grid::Index sourceIndex, traversability_generator3d::TravGenNode* sourceNode,
226 const ugv_nav4d::Motion& motion,
const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode* >& trMap);
231 bool checkOrientationAllowed(
const traversability_generator3d::TravGenNode* node,
232 const base::Orientation2D& orientation)
const;
236 void precomputeCost();
239 double getAvgSlope(std::vector<const traversability_generator3d::TravGenNode*> path)
const;
242 double getMaxSlope(std::vector<const traversability_generator3d::TravGenNode*> path)
const;
246 double getHeuristicDistance(
const Eigen::Vector3d& a,
const Eigen::Vector3d& b)
const;
252 traversability_generator3d::TravGenNode* movementPossible(traversability_generator3d::TravGenNode* fromTravNode,
const maps::grid::Index& fromIdx,
const maps::grid::Index& toIdx);
257 bool checkExpandTreadSafe(traversability_generator3d::TravGenNode * node);
259 bool usePathStatistics;
261 traversability_generator3d::TraversabilityConfig travConf;
262 sbpl_spline_primitives::SplinePrimitivesConfig primitiveConfig;
264 unsigned int numAngles;
std::ostream & operator<<(std::ostream &stream, const DiscreteTheta &angle)
Definition: DiscreteTheta.cpp:3
Definition: DiscreteTheta.hpp:7
Definition: EnvironmentXYZTheta.hpp:25
Eigen::Vector3d robotHalfSize
Definition: EnvironmentXYZTheta.hpp:126
std::vector< Distance > travNodeIdToDistance
Definition: EnvironmentXYZTheta.hpp:97
void setTravConfig(const traversability_generator3d::TraversabilityConfig &cfg)
Definition: EnvironmentXYZTheta.cpp:1232
virtual void PrintState(int stateID, bool bVerbose, FILE *fOut=0)
Definition: EnvironmentXYZTheta.cpp:916
const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > & getTraversabilityMap() const
Definition: EnvironmentXYZTheta.cpp:1146
ThetaNode * startThetaNode
Definition: EnvironmentXYZTheta.hpp:101
std::vector< Motion > getMotions(const std::vector< int > &stateIDPath)
Definition: EnvironmentXYZTheta.cpp:931
virtual ~EnvironmentXYZTheta()
Definition: EnvironmentXYZTheta.cpp:93
virtual bool InitializeEnv(const char *sEnvFile)
Definition: EnvironmentXYZTheta.cpp:505
XYZNode * createNewXYZState(traversability_generator3d::TravGenNode *travNode)
Definition: EnvironmentXYZTheta.cpp:120
void dijkstraComputeCost(const traversability_generator3d::TravGenNode *source, std::vector< double > &outDistances, const double maxDist) const
void setStart(const Eigen::Vector3d &startPos, double theta)
Definition: EnvironmentXYZTheta.cpp:341
virtual int GetStartHeuristic(int stateID)
heuristic estimate from start state to state with stateID
Definition: EnvironmentXYZTheta.cpp:489
traversability_generator3d::TraversabilityGenerator3d & getTravGen()
Definition: EnvironmentXYZTheta.cpp:1222
virtual void SetAllPreds(CMDPSTATE *state)
Definition: EnvironmentXYZTheta.cpp:370
void clear()
Definition: EnvironmentXYZTheta.cpp:58
maps::grid::TraversabilityMap3d< XYZNode * > searchGrid
Definition: EnvironmentXYZTheta.hpp:80
std::shared_ptr< MLGrid > mlsGrid
Definition: EnvironmentXYZTheta.hpp:31
traversability_generator3d::TravGenNode * findObstacleNode(const traversability_generator3d::TravGenNode *travNode) const
Definition: EnvironmentXYZTheta.cpp:1238
virtual int SizeofCreatedEnv()
Definition: EnvironmentXYZTheta.cpp:906
virtual void GetSuccs(int SourceStateID, std::vector< int > *SuccIDV, std::vector< int > *CostV)
traversability_generator3d::TraversabilityGenerator3d travGen
Definition: EnvironmentXYZTheta.hpp:29
void setInitialPatch(const Eigen::Affine3d &ground2Mls, double patchRadius)
Definition: EnvironmentXYZTheta.cpp:98
virtual void GetPreds(int TargetStateID, std::vector< int > *PredIDV, std::vector< int > *CostV)
Definition: EnvironmentXYZTheta.cpp:900
const PreComputedMotions & getAvailableMotions() const
Definition: EnvironmentXYZTheta.cpp:1161
maps::grid::TraversabilityNode< PlannerData > XYZNode
Definition: EnvironmentXYZTheta.hpp:77
virtual int GetFromToHeuristic(int FromStateID, int ToStateID)
heuristic estimate from state FromStateID to state ToStateID
Definition: EnvironmentXYZTheta.cpp:385
bool checkStartGoalNode(const std::string &name, traversability_generator3d::TravGenNode *node, double theta)
Definition: EnvironmentXYZTheta.cpp:220
XYZNode * startXYZNode
Definition: EnvironmentXYZTheta.hpp:102
virtual void PrintEnv_Config(FILE *fOut)
Definition: EnvironmentXYZTheta.cpp:911
const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > & getObstacleMap() const
Definition: EnvironmentXYZTheta.cpp:1151
traversability_generator3d::TravGenNode * obstacleStartNode
Definition: EnvironmentXYZTheta.hpp:107
virtual void SetAllActionsandAllOutcomes(CMDPSTATE *state)
Definition: EnvironmentXYZTheta.cpp:378
void enablePathStatistics(bool enable)
Definition: EnvironmentXYZTheta.cpp:485
ObstacleMapGenerator3D obsGen
Definition: EnvironmentXYZTheta.hpp:30
void updateMap(std::shared_ptr< MLGrid > mlsGrid)
Definition: EnvironmentXYZTheta.cpp:104
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())
Definition: EnvironmentXYZTheta.cpp:944
std::shared_ptr< trajectory_follower::SubTrajectory > findTrajectoryOutOfObstacle(const Eigen::Vector3d &start, double theta, const Eigen::Affine3d &ground2Body)
Definition: EnvironmentXYZTheta.cpp:1264
virtual bool InitializeMDPCfg(MDPConfig *MDPCfg)
Definition: EnvironmentXYZTheta.cpp:510
EnvironmentXYZTheta(std::shared_ptr< MLGrid > mlsGrid, const traversability_generator3d::TraversabilityConfig &travConf, const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig, const Mobility &mobilityConfig)
Definition: EnvironmentXYZTheta.cpp:30
virtual void GetSuccs(int SourceStateID, std::vector< int > *SuccIDV, std::vector< int > *CostV, std::vector< size_t > &motionIdV)
virtual int GetGoalHeuristic(int stateID)
heuristic estimate from state with stateID to goal state
Definition: EnvironmentXYZTheta.cpp:430
ThetaNode * goalThetaNode
Definition: EnvironmentXYZTheta.hpp:103
std::vector< Hash > idToHash
Definition: EnvironmentXYZTheta.hpp:93
void expandMap(const std::vector< Eigen::Vector3d > &positions)
Definition: EnvironmentXYZTheta.cpp:322
PreComputedMotions availableMotions
Definition: EnvironmentXYZTheta.hpp:99
void setGoal(const Eigen::Vector3d &goalPos, double theta)
Definition: EnvironmentXYZTheta.cpp:240
bool obstacleCheck(const maps::grid::Vector3d &pos, double theta, const ObstacleMapGenerator3D &obsGen, const traversability_generator3d::TraversabilityConfig &travConf, const sbpl_spline_primitives::SplinePrimitivesConfig &splineConf, const std::string &nodeName="node")
Definition: EnvironmentXYZTheta.cpp:159
const Motion & getMotion(const int fromStateID, const int toStateID)
Definition: EnvironmentXYZTheta.cpp:400
traversability_generator3d::TraversabilityGenerator3d::MLGrid MLGrid
Definition: EnvironmentXYZTheta.hpp:27
const MLGrid & getMlsMap() const
Definition: EnvironmentXYZTheta.cpp:1156
ThetaNode * createNewStateFromPose(const std::string &name, const Eigen::Vector3d &pos, double theta, ugv_nav4d::EnvironmentXYZTheta::XYZNode **xyzBackNode)
Definition: EnvironmentXYZTheta.cpp:129
traversability_generator3d::TraversabilityGenerator3d & getObstacleGen()
Definition: EnvironmentXYZTheta.cpp:1227
XYZNode * goalXYZNode
Definition: EnvironmentXYZTheta.hpp:104
ThetaNode * createNewState(const DiscreteTheta &curTheta, EnvironmentXYZTheta::XYZNode *curNode)
Definition: EnvironmentXYZTheta.cpp:522
maps::grid::Vector3d getStatePosition(const int stateID) const
Definition: EnvironmentXYZTheta.cpp:391
Definition: PreComputedMotions.hpp:29
Definition: EnvironmentXYZTheta.hpp:19
Definition: EnvironmentXYZTheta.hpp:20
Definition: ObstacleMapGenerator3D.hpp:7
Definition: EnvironmentXYZTheta.hpp:21
Definition: PreComputedMotions.hpp:88
Definition: EnvironmentXYZTheta.hpp:18
Definition: Dijkstra.cpp:8
Definition: EnvironmentXYZTheta.hpp:70
Distance(double toStart, double toGoal)
Definition: EnvironmentXYZTheta.hpp:73
double distToStart
Definition: EnvironmentXYZTheta.hpp:71
double distToGoal
Definition: EnvironmentXYZTheta.hpp:72
Definition: EnvironmentXYZTheta.hpp:34
virtual const char * what() const
Definition: EnvironmentXYZTheta.hpp:38
EnvironmentXYZThetaException(const std::string &what)
Definition: EnvironmentXYZTheta.hpp:35
const std::string msg
Definition: EnvironmentXYZTheta.hpp:42
Definition: EnvironmentXYZTheta.hpp:84
Hash(XYZNode *node, ThetaNode *thetaNode)
Definition: EnvironmentXYZTheta.hpp:85
XYZNode * node
Definition: EnvironmentXYZTheta.hpp:88
ThetaNode * thetaNode
Definition: EnvironmentXYZTheta.hpp:89
Definition: EnvironmentXYZTheta.hpp:56
traversability_generator3d::TravGenNode * travNode
Definition: EnvironmentXYZTheta.hpp:57
PlannerData()
Definition: EnvironmentXYZTheta.hpp:57
std::map< DiscreteTheta, ThetaNode * > thetaToNodes
Definition: EnvironmentXYZTheta.hpp:65
Definition: EnvironmentXYZTheta.hpp:48
DiscreteTheta theta
Definition: EnvironmentXYZTheta.hpp:51
ThetaNode(const DiscreteTheta &t)
Definition: EnvironmentXYZTheta.hpp:49
int id
Definition: EnvironmentXYZTheta.hpp:49
Definition: Mobility.hpp:12