//create and score a trajectory given the current pose of the robot and selected velocities
void TrajectoryController::generateTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta, double impossible_cost,
Trajectory& traj){
double x_i = x;
double y_i = y;
double theta_i = theta;
double vx_i = vx;
double vy_i = vy;
double vtheta_i = vtheta;
double dt = sim_time_ / num_steps_;
//create a potential trajectory
traj.xv_ = vx_samp;
traj.yv_ = vy_samp;
traj.thetav_ = vtheta_samp;
//initialize the costs for the trajectory
double path_dist = 0.0;
double goal_dist = 0.0;
double occ_cost = 0.0;
for(int i = 0; i < num_steps_; ++i){
//get map coordinates of a point
unsigned int cell_x, cell_y;
//we don't want a path that goes off the know map
if(!ma_.WC_MC(x_i, y_i, cell_x, cell_y)){
traj.cost_ = -1.0;
return;
}
//we want to check if this cell definitely contains an obstacle
if(ma_.getCost(cell_x, cell_y) >= costmap_2d::ObstacleMapAccessor::INSCRIBED_INFLATED_OBSTACLE){
traj.cost_ = -1.0;
return;
}
//we need to check if we need to lay down the footprint of the robot
if(ma_.getCost(cell_x, cell_y) >= costmap_2d::ObstacleMapAccessor::CIRCUMSCRIBED_INFLATED_OBSTACLE){
double footprint_cost = footprintCost(x_i, y_i, theta_i);
//if the footprint hits an obstacle this trajectory is invalid
if(footprint_cost < 0){
traj.cost_ = -1.0;
return;
}
occ_cost += footprint_cost;
}
else{
occ_cost += double(ma_.getCost(cell_x, cell_y));
}
double cell_pdist = map_(cell_x, cell_y).path_dist;
double cell_gdist = map_(cell_x, cell_y).goal_dist;
//update path and goal distances
path_dist = cell_pdist;
goal_dist = cell_gdist;
//if a point on this trajectory has no clear path to goal it is invalid
if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
//printf("No path to goal with goal distance = %f, path_distance = %f and max cost = %f\n", goal_dist, path_dist, impossible_cost);
traj.cost_ = -1.0;
return;
}
//the point is legal... add it to the trajectory
traj.setPoint(i, x_i, y_i, theta_i);
//calculate velocities
vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
//calculate positions
x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
}
double cost = pdist_scale_ * path_dist + gdist_scale_ * goal_dist + dfast_scale_ * (1.0 / ((.05 + traj.xv_) * (.05 + traj.xv_))) + occdist_scale_ * occ_cost;
traj.cost_ = cost;
}
