C++ Trajectory::setPoint方法代码示例

//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;
}