Trajectory PlanningProblem::GRRTsolve()
{
this->planningResult = false;
randomTree.clear();
float start_time = currentTimeMSec();
int tryExtensionCounter = 0;
randomTree.appendNewStation(NULL, initialState);
for(int step=0; step < MAX_RRT_STEP_TRY/5; step++)
{
Station target;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
target.setPosition(goal.goal_point.getPosition());
else {
Station tempSt = SampleStateUniform();
target.setPosition(tempSt.getPosition());
}
if( !target.isValid() )
continue;
// throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(target);
if(near_ver == NULL)
continue;
int greedyCounter = 0;
while(greedyCounter < 5){
tryExtensionCounter ++;
Station extended = RRTExtend(near_ver->state, target, agent->radius() * 2);
if(!extended.isValid())
break;
randomTree.appendNewStation(near_ver, extended);
if(Station::dubinDistance(extended, target) < agent->radius() ) {
if((target.getPosition() - goal.goal_point.getPosition()).lenght2D() < agent->radius() /2)
planningResult = true;
break;
}
// if(target != goal.goal_point) break;
greedyCounter ++;
}
cout << "Step = " << step << endl;
}
if(planningResult)
{
float finish_time = currentTimeMSec();
this->planningTime = finish_time - start_time;
// cout << "Greedy RRT Planning succeed in " << planningTime << "mili seconds" << endl;
return buildTrajectoryFromRandomTree();
}
return Trajectory();
}
// *************************************************
// ************** under construction ***************
// *************************************************
Trajectory PlanningProblem::RRTConnectSolve(double arg1)
{
double start_time = currentTimeMSec();
randomTree.clear();
backward_tree.clear();
randomTree.appendNewStation(NULL, initialState);
backward_tree.appendNewStation(NULL, goal.goal_point);
for(uint i=0; i< MAX_RRT_STEP_TRY ; ++i)
{
Station randSt = SampleStateUniform();
if(!randSt.isValid())
continue;
SpatialVertex* near_ver = randomTree.getNearestVertex(randSt);
if(near_ver == NULL)
continue;
Station extended_ = RRTExtend(near_ver->state, randSt, arg1);
if(!extended_.isValid())
continue;
randomTree.appendNewStation(near_ver, extended_);
for(int j=0; j<10; j++) {
SpatialVertex* back_near = backward_tree.getNearestVertex(extended_);
Station back_extended = RRTExtend(back_near->state, extended_, arg1);
if(!back_extended.isValid())
break;
backward_tree.appendNewStation(back_near, back_extended);
if(Station::euclideanDistance(back_extended, extended_) < agent->radius()) {
while(back_near != NULL) {
Station on_back_tree_st = back_near->state;
on_back_tree_st.setPosition(Vector3D(back_near->state.getPosition().to2D(),
back_near->state.getPosition().Teta() + M_PI));
randomTree.appendNewStation(randomTree.lastAddedVertex(), on_back_tree_st);
back_near = back_near->parent;
}
planningTime = currentTimeMSec() - start_time;
return buildTrajectoryFromRandomTree();
}
}
}
planningTime = currentTimeMSec() - start_time;
return Trajectory();
}