コード例 #1
0
bool IsStraight(Trajectory &trajectory)
{
  return boost::geometry::length(trajectory)/boost::geometry::distance(trajectory.front(),trajectory.back()) < 1.05;
}
コード例 #2
0
void RRT::optimize_trajectory(){
    Trajectory traj;
    double length, new_length;
    double v_max = world.leader->get_limits().vMax*0.8;
    double c_max = world.leader->get_limits().cMax;
    double c_min = world.leader->get_limits().cMin;
    State st = states[0],
            st2;
    for (int i = 0; i < out_trajectory.size(); ++i) {
        traj.push_back(Desc(st,out_trajectory[i]));
        st2 = world.leader->calculate_state(st, out_trajectory[i]);
        st = st2;
    }
    traj.push_back(Desc(st,Control()));

#ifdef DEBUG_RRT
    length = 0;
    for (int i = 0; i < traj.size(); ++i) {
        length += traj[i].control.t * traj[i].control.v;
    }
    TimeMeasurement tm;
    tm.start();
    std::cout << "RRT: original trajectory length: " << length <<  std::endl;
    /*
    std::cout << std::endl << "LEADER PLANNED POSITIONS:" << std::endl;
    for (int i = 0; i < traj.size(); ++i) {
        std::cout << "	X = [ " << traj[i].state.X(0) << ", " << traj[i].state.X(1) << ", " << traj[i].state.X(2) << " ]" << std::endl;
    }
*/
#endif
    int loop = 0;
    bool isValid;
    bool endPoint;
    while(loop < 100){
        ++loop;
        int n_first = rand() % traj.size();
        int n_second = rand() % traj.size();
        while(n_first == n_second){
            n_second = rand() % traj.size();
        }

        if (n_first > n_second){
            const int p = n_first;
            n_first = n_second;
            n_second = p;
        }

        length = 0;
        for (int i = n_first; i < n_second; ++i) {
            length += traj[i].control.t * fabs(traj[i].control.v);
        }
        endPoint = false;
        if (n_second != traj.size()-1){
            dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
                                      geom::Position(geom::Point(traj[n_second].state.X(0),traj[n_second].state.X(1)), traj[n_second].state.phi),
                                      1/world.leader->cMax());
        }else{
            endPoint = true;
            dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
                                      geom::Point(goal[0],goal[1]),
                    1/world.leader->cMax());
            length += dist2D(traj.back().state.X,Vector(goal[0],goal[1],0));
        }
        new_length = dubins->getLength();

        if (new_length < length){
#ifdef DEBUG_RRT_DUBINS
            if (endPoint){
            std::cout << "DUBINS: original part length   : " << length <<  std::endl;
            std::cout << "        simplified part length : " << new_length <<  std::endl;
            std::cout << "        type of Dubins maneuver: " << dubins->getTypeOfManeuver() << " [RSR, LSL, LSR, RSL, RLR, LRL, LS, RS]" << std::endl;
            }
#endif

            Trajectory new_traj;
            for (int i = 0; i < n_first; ++i) {
                new_traj.push_back(traj[i]);
            }
            double t1, t2, t3, c1, c2, c3;
            State state_first = traj[n_first].state;
            Control cnt;
            switch (dubins->getTypeOfManeuver()) {
            case geom::RSR:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_min;
                c2 = 0;
                c3 = c_min;
                break;
            case geom::LSL:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_max;
                c2 = 0;
                c3 = c_max;
                break;
            case geom::LSR:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_max;
                c2 = 0;
                c3 = c_min;
                break;
            case geom::RSL:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_min;
                c2 = 0;
                c3 = c_max;
                break;
            case geom::RLR:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_min;
                c2 = c_max;
                c3 = c_min;
                break;
            case geom::LRL:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_max;
                c2 = c_min;
                c3 = c_max;
                break;
            default:
                break;
            }

            State s_new, s_old = state_first;
            int n_added = 0;
            if (t1 > eps){
                while (t1-RRT_TIME_STEP>eps){
                    cnt = Control(v_max,0,c1,RRT_TIME_STEP);
                    new_traj.push_back(Desc(s_old,cnt));
                    t1 -= RRT_TIME_STEP;

                    s_new = world.leader->calculate_state(s_old, cnt);
                    s_old = s_new;
                    ++n_added;
                }
                cnt = Control(v_max,0,c1,t1);
                new_traj.push_back(Desc(s_old,cnt));
                s_new = world.leader->calculate_state(s_old, cnt);
                s_old = s_new;
                ++n_added;
            }

            if (t2 > eps){
                while (t2-RRT_TIME_STEP>eps){
                    cnt = Control(v_max,0,c2,RRT_TIME_STEP);
                    new_traj.push_back(Desc(s_old,cnt));
                    t2 -= RRT_TIME_STEP;

                    s_new = world.leader->calculate_state(s_old, cnt);
                    s_old = s_new;
                    ++n_added;
                }
                cnt = Control(v_max,0,c2,t2);
                new_traj.push_back(Desc(s_old,cnt));
                s_new = world.leader->calculate_state(s_old, cnt);
                s_old = s_new;
                ++n_added;
            }
            if (t3 > eps){
                while (t3-RRT_TIME_STEP>eps){
                    cnt = Control(v_max,0,c3,RRT_TIME_STEP);
                    new_traj.push_back(Desc(s_old,cnt));
                    t3 -= RRT_TIME_STEP;

                    s_new = world.leader->calculate_state(s_old, cnt);
                    s_old = s_new;
                    ++n_added;
                }
                cnt = Control(v_max,0,c3,t3);
                new_traj.push_back(Desc(s_old,cnt));
                ++n_added;
            }

            isValid = true;
            for (int i = 0; i < n_added; ++i) {
                const int index = new_traj.size()-n_added+i;
                Points pts = world.leader->getSampledTrajectory(new_traj[index].state,&new_traj[index].control,1);
                for (int j = 0; j < pts.size(); ++j) {
                    isValid &= world.map->wall_distance(pts[j]) > world.leader->get_ra();
                    if (!isValid)
                        break;
                }
                if (!isValid)
                    break;
            }
            if (!isValid){
                continue;
            }

            loop = 0;
            if (!endPoint){
                for (int i = n_second; i < traj.size(); ++i) {
                    new_traj.push_back(traj[i]);
                }
            }else{
                s_new = world.leader->calculate_state(new_traj.back().state, new_traj.back().control);
                new_traj.push_back(Desc(State(goal[0],goal[1],0,0),Control()));
            }
            traj = new_traj;

        }
    }
    out_trajectory.clear();
    for (int i = 0; i < traj.size()-1; ++i) {
        out_trajectory.push_back(traj[i].control);
    }

#ifdef DEBUG_RRT
    length = 0;
    for (int i = 0; i < traj.size(); ++i) {
        length += traj[i].control.t * traj[i].control.v;
    }
    tm.end();
    std::cout << "RRT: optimized trajectory length: " << length <<  std::endl;
    std::cout << "                            size: " << traj.size()-1 <<  std::endl;

    /*
    std::cout << std::endl;
    for (int i = 0; i < traj.size(); ++i) {
        std::cout << "   v = " << traj[i].control.v << ", w = " << traj[i].control.w << ", c = " << traj[i].control.c << ", t = " << traj[i].control.t << std::endl;
    }
    */
#endif

}