AnytimeBidirectionalEST(const Workspace &workspace, const Agent &agent, const InstanceFileMap &args, bool quiet = false,
double gBound = std::numeric_limits<double>::infinity()) :
workspace(workspace), agent(agent),
forwardKDTree(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()), backwardKDTree(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()),
quiet(quiet), gBound(gBound) {
collisionCheckDT = args.doubleVal("Collision Check Delta t");
goalBias = args.doubleVal("Goal Bias");
howManySamplePoints = args.integerVal("Number of Sample Points");
samplePointRadius = args.doubleVal("Sample Point Radius");
linkRadius = args.doubleVal("Link Radius");
if(!quiet) {
dfpair(stdout, "collision check dt", "%g", collisionCheckDT);
dfpair(stdout, "number of sample points", "%u", howManySamplePoints);
}
unsigned int regionCount = discretization.getCellCount();
forwardRegions.reserve(regionCount);
backwardRegions.reserve(regionCount);
for(unsigned int i = 0; i < regionCount; ++i) {
forwardRegions.push_back(new RegionNode(i));
backwardRegions.push_back(new RegionNode(i));
}
samplesGenerated = edgesGenerated = samplesAdded = edgesAdded = 0;
timeout = args.doubleVal("Timeout");
selfPtr = this;
}
void go_AnytimeRRT(const InstanceFileMap &args, const Agent &agent, const Workspace &workspace,
const typename Agent::State &start, const typename Agent::State &goal) {
clock_t startT = clock();
dfpair(stdout, "planner", "%s", "Anytime RRT");
// typedef flann::KDTreeSingleIndexParams KDTreeType;
typedef flann::KDTreeIndexParams KDTreeType;
typedef FLANN_KDTreeWrapper<KDTreeType, typename Agent::DistanceEvaluator, typename Agent::Edge> KDTree;
typedef UniformSampler<Workspace, Agent, KDTree> USampler;
typedef GoalBiasSampler<Agent, USampler> GBSampler;
typedef TreeInterface<Agent, KDTree, GBSampler> TreeInterface;
typedef AnytimeRRT<Workspace, Agent, TreeInterface> Planner;
/* planner config */
KDTreeType kdtreeType(1);
KDTree kdtree(kdtreeType, agent.getDistanceEvaluator(), agent.getTreeStateSize());
USampler uniformsampler(workspace, agent, kdtree);
double goalBias = args.exists("Goal Bias") ? args.doubleVal("Goal Bias") : 0;
dfpair(stdout, "goal bias", "%g", goalBias);
GBSampler goalbiassampler(uniformsampler, goal, goalBias);
TreeInterface treeInterface(kdtree, goalbiassampler);
Planner planner(workspace, agent, treeInterface, args);
go_COMMONANYTIME<Planner, Workspace, Agent>(args, planner, workspace, agent, start, goal, startT);
}
SST(const Workspace &workspace, const Agent &agent,
InsertionInteface &insertionInterface, QueryInterface &queryInterface, double startingRadius, double resizeThreshold,
unsigned int historySize = 100) : insertionInterface(insertionInterface), queryInterface(queryInterface),
witnessInterface(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()), radius(startingRadius), resizeThreshold(resizeThreshold),
history(historySize), historyIndex(0), historyFails(0), historyFilled(false) {}