ObtacleCostFunction

定义了一个ObstacleCostFunction类，继承自Trajectory类，Trajectory类有8个类参
总共有8个类参
double xv_,yv_,thetav_ 这三分别是用于生成轨迹的x,y,角速度
double cost_ 轨迹得分
double time_delta_ 轨迹点之间的时间间隔
std::vector<double> x_pts_,y_pts_,theta_pts_,轨迹上的x,y角度点集
它的类参是
 costmap_2d::Costmap2D* costmap_; 
  std::vector<geometry_msgs::Point> footprint_spec_; //geometry_msgs::Point如果单就point,就x,y,z三个值
  base_local_planner::WorldModel* world_model_;
  double max_trans_vel_;
  bool sum_scores_;
  double max_scaling_factor_, scaling_speed_;
这个类的主要作用是如果机器人足迹在轨迹的任意点上遇到障碍物的话，就用costmap 2d去分配负的代价值。
下面是几个函数
1.ObstacleCostFunction(costmap_2d::Costmap2D* costmap);
这个构造函数直接把costmap赋值给类参 costmap_,都是指针，另外把false赋值给sum_score_,所以默认是返回轨迹最后一个点的足迹变换
后的代价值。
如果costmap_不为空的话，直接把costmap变一变赋值给类参world_model_
world_model_ = new base_local_planner::CostmapModel(*costmap_);
2.~ObstacleCostFunction();
这个是用来删除world_model_的，当world_model_不为空的时候，直接删除它。也是指针
3. bool prepare();
直接返回true.
4.void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed);
直接把里面的三个值依次赋值给类参max_trans_vel_,max_scaling_factor_,scaling_speed_
5.void setFootprint(std::vector<geometry_msgs::Point> footprint_spec);
直接把里面值赋值给类参footprint_spec_.
6. static double footprintCost(
      const double& x,
      const double& y,
      const double& th,
      double scale,
      std::vector<geometry_msgs::Point> footprint_spec,
      costmap_2d::Costmap2D* costmap,
      base_local_planner::WorldModel* world_model);
这个函数用来得footprintCost的，先调用WorldModel类里面的footprintCost函数，用x,y,th把footprint_spec变成世界坐标下坐标，还得到原点在世界坐标系坐标，内切半径，外切半径。根据这些，调用CostmapModel类里的footprintCost函数，转换到costmap坐标系下，用光栅化算法得到栅格点，由栅格点的代价值得到足迹点线段的代价值，再得到整个足迹点集的代价值就可以了。
 7.static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor);
这个函数是用来得到scale的，它是根据traj的x,y速度值得到它们的平方和的根，然后和scaling_speed比较，如果小于的话，根据
max_trans_vel得到ratio,用ratio修改max_scaling_factor得到scale
8.  void setSumScores(bool score_sums){ sum_scores_=score_sums; }
这个直接用变量值设置sum_scores_的值。
9. double scoreTrajectory(Trajectory &traj);
这个会先根据类参算得scale,然后判断类参footprint_spec_,如果足迹为空，返回-9.
否则算足迹点集在经过轨迹点变换之后的footprintCost设为f_cost,如果f_cost小于0，得分就是f_cost,如果不小于0，就根据
sum_score_是否为true,选择返回f_cost的和还是最后一个f_cost.
具体实现：
#include <base_local_planner/obstacle_cost_function.h>
#include <cmath>
#include <Eigen/Core>
#include <ros/console.h>
 
namespace base_local_planner {
 
ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap) 
    : costmap_(costmap), sum_scores_(false) {
  if (costmap != NULL) {
    world_model_ = new base_local_planner::CostmapModel(*costmap_);//CostmapModel也只有一个类参costmap_,
  }
}
//这个析构函数用来删除world_model_的
ObstacleCostFunction::~ObstacleCostFunction() {
  if (world_model_ != NULL) {
    delete world_model_;
  }
}
 
//设置参数里依次把变量值赋值给障碍代价函数类里的max_trans_vel_,max_scaling_factor_,scaling_speed_
void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
  // TODO: move this to prepare if possible 如果可能的话，删除这些去prepare?
  max_trans_vel_ = max_trans_vel;
  max_scaling_factor_ = max_scaling_factor;
  scaling_speed_ = scaling_speed;
}
//设置足迹是把变量值赋值给障碍代价函数类的footprint_spec_
void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
  footprint_spec_ = footprint_spec;
}
//这个类的prepare函数直接返回true
bool ObstacleCostFunction::prepare() {
  return true;
}
/*scoreTrajectory函数是用来给轨迹打分的，很多轨迹点组成一个轨迹，它是利用每个轨迹点把足迹点转换到世界坐标下坐标，再到costmap坐标系，然后得到足迹点
 * 构成的各个栅格点的代价值，取这些代价值的最大值，这个得分是根据这个代价值来的。
 * 过程（1）用getScalingFactor(traj,scaling_speed_,max_trans_vel_,max_scaling_factor_)得到scale,这个是根据traj的x,y速度得到vmag，然后vmag跟scaling_speed_比，
 * 如果小于的话，就修改max_scaling_factor为新的scale.但这个scale没有用啊。
 *           (2)判断footprin_spec_的大小，如果size为0，就发出警告，scoreTrajectory函数返回-9.
 *           (3) 如果footprint_spec_有点，那么造一个for循环，从0到轨迹点数，由traj.getPoint(i, px, py, pth);得到px,py,pth
 *               由 double f_cost = footprintCost(px, py, pth,scale, footprint_spec_,costmap_, world_model_)得到代价值。这个函数首先调用WorldModel里的footprintCost函数
 * ，然后调用类CostmapModel类的footprintCost函数,最后得到代价值。
 *                     如果f_cost小于0,函数返回f_cost;为-6.0,-7.0
 *                     如果ObstacleCostFunction类里的sum_score_为true,那么返回各个f_cost（它们大于等于0的话）的和，如果定义为false,返回最后一个cost
 */
double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }
 
  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);
 
    if(f_cost < 0){
        return f_cost;
    }
 
    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}
/*getScalingFactor这个函数是用来得到scale的，scale是轨迹代价函数的类参。它是利用轨迹traj,scaling_speed,max_trans_vel,max_scaling_factor
 * _factor得到的。
 * （1）由轨迹traj的x,y速度得到vmag.vmag = hypot(traj.xv_, traj.yv_)
 *          如果vmag大于scaleing_speed,那么令ratio为(vmag - scaling_speed) / (max_trans_vel - scaling_speed)
 *          scale就为max_scaling_factor * ratio + 1.0
 */
 
double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
  double vmag = hypot(traj.xv_, traj.yv_);
 
  //if we're over a certain speed threshold, we'll scale the robot's
  //footprint to make it either slow down or stay further from walls 
  double scale = 1.0;
  if (vmag > scaling_speed) {
    //scale up to the max scaling factor linearly... this could be changed later 
    double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
    scale = max_scaling_factor * ratio + 1.0;
  }
  return scale;
}
/*footprintCost函数这里的变量是x,y,th,scale,footprint_spec,costmap,world_model,这里scale没有用到。
 * （1）利用world_model的footprintCost函数得到足迹点的cost值，world_model->footprintCost(x, y, th, footprint_spec);它这里面首先调用WorldModel类的footprintCost
 * 函数得到机器人在世界坐标系下原点坐标，足迹在世界坐标系下坐标，内切半径，外切半径，然后调用类CostmapModel类的footprintCost函数，把footprint_spec
 * 转换成地图下坐标，如果有一次转换不成功，就返回false,如果都转换成功，就返回光栅化后的所有足迹点的最大代价值。
 * 这里把得到的代价值记为footprint_cost,如果footprint_cost小于0,那么障碍代价函数类footprintCost函数返回-6.0
 * (2)把x,y转成cell_x,cell_y如果转换不成功，返回-7.0,之前在类WorldModel里，如果转换不成功，是返回-1.0,这里是区分开了，更详细
 * (3)如果footprint_cost不小于0，而且原点转换成功，那么返回值取footprint_cost,0.0,和原点代价值最大值
 * 也就是说返回值3种情况-6.0,-7.0,footprint_cost,0.0,和原点代价值最大值
 */
double ObstacleCostFunction::footprintCost (
    const double& x,
    const double& y,
    const double& th,
    double scale,
    std::vector<geometry_msgs::Point> footprint_spec,
    costmap_2d::Costmap2D* costmap,
    base_local_planner::WorldModel* world_model) {
 
  //check if the footprint is legal 
  // TODO: Cache inscribed radius  
  double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);
  
 
  if (footprint_cost < 0) {
    return -6.0;
  }/
  unsigned int cell_x, cell_y;
  
 //we won't allow trajectories that go off the map... shouldn't happen that often anyways 
  if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
    return -7.0;
  }
 
  double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));//occ_cost取footprint_cost,0.0,cell_x,cell_y处的代价值的最大值
  
 
  return occ_cost;
}
 
} /* namespace base_local_planner */