[Code Review] 2023_[T-ITS]An Efficient Spatial-Temporal Trajectory Planner forAutonomous Vehicles in Unstructured Environments

park.cc (main 문)

 

traj_server_ros.cpp

 

traj_manager.cpp

 

kino_aster.cpp

 

traj_optimizer.cpp

s1 = 0.0;
s2 = s1 * s1;
s3 = s2 * s1;
s4 = s2 * s2;
s5 = s4 * s1;
beta0 << 1.0, s1, s2, s3, s4, s5;
beta1 << 0.0, 1.0, 2.0 * s1, 3.0 * s2, 4.0 * s3, 5.0 * s4;
beta2 << 0.0, 0.0, 2.0, 6.0 * s1, 12.0 * s2, 20.0 * s3;
beta3 << 0.0, 0.0, 0.0, 6.0, 24.0 * s1, 60.0 * s2;
beta4 << 0.0, 0.0, 0.0, 0.0, 24.0, 120 * s1;
alpha = 1.0 / K * j;

//update s1 for the next iteration
s1 += step;
pointid++;

sigma = c.transpose() * beta0;
dsigma = c.transpose() * beta1;
ddsigma = c.transpose() * beta2;
dddsigma = c.transpose() * beta3;
ddddsigma = c.transpose() * beta4;

// ctrl_points_.col(i_dp) = sigma;
omg = (j == 0 || j == K) ? 0.5 : 1.0;

// some help values

z_h0 = dsigma.norm();
z_h1 = ddsigma.transpose() * dsigma;
z_h2 = dddsigma.transpose() * dsigma;
z_h3 = ddsigma.transpose() * B_h * dsigma;

n1 = z_h0;
n2 = n1 * n1;
n3 = n2 * n1;
n4 = n2 * n2;
n5 = n3 * n2;
n6 = n3 * n3;

z1 = dddsigma.transpose() * B_h * dsigma;
z2 = ddsigma.transpose() * B_h * dsigma;
z3 = dsigma.transpose() * ddsigma;  

if (j != K || (j == K && i == N - 1))
{
  ++i_dp;
}


// add cost z_h0 = ||v||
if ( z_h0 < 1e-4 || (j==0&&i==0) || (i==N-1&&j==K))
{
  continue;
}
//avoid siguality

vel2_reci = 1.0 / (z_h0 * z_h0);
vel2_reci_e = 1.0 / (z_h0 * z_h0+epis);
vel3_2_reci_e = vel2_reci_e * sqrt(vel2_reci_e);
z_h0 = 1.0 / z_h0;

z_h4 = z_h1 * vel2_reci;
violaVel = 1.0 / vel2_reci - max_vel * max_vel;
acc2 = z_h1 * z_h1 * vel2_reci;
latacc2 = z_h3 * z_h3 * vel2_reci;
cur2 = z_h3 * z_h3 * (vel2_reci_e * vel2_reci_e * vel2_reci_e);
cur = z_h3 * vel3_2_reci_e;
violaAcc = acc2 - max_acc * max_acc;
violaLatAcc = latacc2 - max_latacc_ * max_latacc_;

phidot_denominator = n6 + L_ * L_ * z2 * z2;
phidot_nominator = L_ * (n3 * z1 - 3 * z2 * z3 * n1);
phi_dot = phidot_nominator / phidot_denominator; // S/M

//@hzc: add feasibility with curvature
violaCur = cur2 - max_cur * max_cur;
violaCurL = cur-max_cur;
violaCurR = -cur-max_cur;
violaPhidotL = phi_dot - max_phidot_;
violaPhidotR = -phi_dot - max_phidot_;