bool IsStraight(Trajectory &trajectory)
{
return boost::geometry::length(trajectory)/boost::geometry::distance(trajectory.front(),trajectory.back()) < 1.05;
}
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
}