コード例 #1
0
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();
}
コード例 #2
0
// *************************************************
// ************** 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();
}