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();
}
PlanningProblem::ExtendResult PlanningProblem::RRTStep(float extension_len, float max_len)
{
ExtendResult result;
try {
Station rand_st;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
rand_st.set(goal.goal_point);
else
{
if(max_len < INFINITY) {
Vector2D rand_point = randomSampleFromEllipse(initialState.getPosition().to2D(),
goal.goal_point.getPosition().to2D(),
max_len);
Station temp;
temp.setPosition(Vector3D(rand_point, uni_rand(-M_PI, M_PI)));
rand_st.set(temp);
}
else {
Station temp = SampleStateUniform();
rand_st.set(temp);
}
}
if(!rand_st.isValid())
throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(rand_st);
// if(near_ver->state.isValid())
if(near_ver == NULL)
throw "can not find nearest!";
Station new_st = RRTExtend((near_ver->state), rand_st, extension_len);
if(!new_st.isValid())
{
result = eTrapped;
throw "can not extend tree!";
}
if(goal.minDistTo(new_st) < agent->radius() * 1)
result = eReached;
else
result = eAdvanced;
randomTree.appendNewStation(near_ver, new_st);
} catch (const char* msg)
{
// cerr << "Exception in RRTStep: " << msg << endl;
}
return result;
}