(十四) 可行性 isTrajectoryFeasible

// Do not allow config changes during the following optimization step
boost::mutex::scoped_lock cfg_lock(cfg_.configMutex());
bool success = planner_->plan(robot_pose_, robot_goal_, &robot_vel_, 
               cfg_.goal_tolerance.free_goal_vel); // straight line init
if (!success)
{
  planner_->clearPlanner();  // force reinitialization for next time
  ROS_WARN("not able to obtain a local plan for the current setting");
  
  ++no_infeasible_plans_;  // increase number of infeasible solutions in a row
  time_last_infeasible_plan_ = ros::Time::now();
  last_cmd_ = cmd_vel;
  return false;
}
// Check feasibility (but within the first few states only)
if(cfg_.robot.is_footprint_dynamic)
{
  // Update footprint of the robot and minimum and maximum distance 
  // from the center of the robot to its footprint vertices.
  footprint_spec_ = costmap_ros_->getRobotFootprint();
  costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
  // is a circular footprint
  if( robot_circumscribed_radius / robot_inscribed_radius_ < 1.02 )   
      cfg_.obstacles.min_obstacle_dist = robot_circumscribed_radius; 
   // fit dynamic footprint
  cfg_.robot.robot_inscribed_radius =  robot_inscribed_radius_;
  cfg_.robot.robot_circumscribed_radius = robot_circumscribed_radius;
}

isTrajectoryFeasible

接下来算法遍历前n个位姿(n就是参数feasibility_check_no_poses)，从当前的机器人位姿开始，检查这几个位姿点是否有碰撞。为了检测是否发生碰撞，validation-model可能用于optimization的footprint更复杂

参数feasibility_check_no_poses不能太大，因为optimizer可能不完全收敛。小的障碍碰撞在未来可以获得纠正，所以设置太大导致的失败不一定是有效的。

狭窄环境中，调试要慎重。局部规划可能拒绝一个infeasible trajectory，但全局规划会认为全局路径是feasible，结果就是机器人stucked。如果禁止了后退速度，会经常出现not feasible，尤其是在cluttered环境

源码中是这样调用的:

bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, 
      robot_inscribed_radius_,  robot_circumscribed_radius, 
      cfg_.trajectory.feasibility_check_no_poses);
if (!feasible)
{
    cmd_vel.linear.x = 0;   cmd_vel.linear.y = 0;   cmd_vel.angular.z = 0;
    // reset everything to start again with the initialization of new trajectories.
    planner_->clearPlanner();
    ROS_WARN("trajectory is not feasible. Resetting planner...");
    ++no_infeasible_plans_;
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel;
    return false;
}

如果用的是HomotopyClassPlanner，实际上还是对best TebOptimalPlanner验证轨迹可行性，即TebOptimalPlannerPtr best = findBestTeb();，实际用的是同名函数

costmapmodel: costmap model的指针, `costmap_model = boost::makeshared<
base_local_planner::CostmapModel>(*costmap);`
footprintspec: 机器人世界坐标系的轮廓, `costmap_ros->getRobotFootprint();`
look_ahead_idx: 其实就是参数feasibility_check_no_poses，沿着trajectory需要验证的位姿点，
如果是-1，那就验证整个trajectory。

这里的障碍物是以代价地图代表，而不是算法内部的ObstacleContainer

bool TebOptimalPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model,
              const std::vector<geometry_msgs::Point>& footprint_spec,
              double inscribed_radius,  double circumscribed_radius, 
              int look_ahead_idx )
{
  if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses() )
      look_ahead_idx = teb().sizePoses() - 1;
  
  for (int i=0; i <= look_ahead_idx; ++i)
  {
    // 只考虑覆盖一个障碍的情况，撞了则直接 return
    if ( costmap_model->footprintCost(teb().Pose(i).x(), 
        teb().Pose(i).y(), 
        teb().Pose(i).theta(), 
        footprint_spec, inscribed_radius, circumscribed_radius) == -1 )
    {
      if (visualization_)
          visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_);
      
      return false;
    }
    // 检查两个位姿点的间距是否比机器人内接半径大；或者它们的偏航角差距大于
    // 参数min_resolution_collision_check_angular
    // 如果符合一种情况， 需要增加插值(临时位姿点)
    // 如果两个位姿点被障碍物push away，那么它们的中心可能与障碍碰撞
    if (i < look_ahead_idx )
    {
      double delta_rot = g2o::normalize_theta( g2o::normalize_theta(teb().Pose(i+1).theta()) -
                         g2o::normalize_theta(teb().Pose(i).theta() )  );
      Eigen::Vector2d delta_dist = teb().Pose(i+1).position()-teb().Pose(i).position();
      if( fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular || 
          delta_dist.norm() > inscribed_radius  )
      {
        int  n_additional_samples = std::max( std::ceil(fabs(delta_rot) / 
          cfg_->trajectory.min_resolution_collision_check_angular ), 
                std::ceil(delta_dist.norm() / inscribed_radius) ) - 1;
        ROS_INFO("n_additional_samples: %d", n_additional_samples);
        PoseSE2  intermediate_pose = teb().Pose(i);
        for(int step = 0; step < n_additional_samples; ++step)
        {
          intermediate_pose.position() = intermediate_pose.position() + delta_dist / 
             (n_additional_samples + 1.0);
          intermediate_pose.theta() = g2o::normalize_theta(
            intermediate_pose.theta() + delta_rot / (n_additional_samples + 1.0) );

          if ( costmap_model->footprintCost(intermediate_pose.x(), 
                intermediate_pose.y(), intermediate_pose.theta(),
                footprint_spec, inscribed_radius, circumscribed_radius) == -1 )
          {
            if (visualization_) 
               visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_);
            
            return false;
          }
        }
      }
    }
  }
  return true;
}

实质上比较简单，判断路径feasible用的是base_local_planner::CostmapModel的footprintCost函数，位姿点间距大则增加插值。详细参考文章CostmapModel 判断是否碰撞

有时会无法把footprint point转为map坐标系的坐标，因为它在局部代价地图之外。如果isTrajectoryFeasible函数检查的局部路径点太多，有的会超出局部代价地图，这样会一直返回false

小车撞障碍但又是 isTrajectoryFeasible

日志:

footprint cost in isTrajectoryFeasible: -1.000000
[17:17:26.883 - WARN] ():   trajectory is not feasible. Resetting planner..
[17:17:26.940 - INFO] (): footprint cost in isTrajectoryFeasible: 0.00
[17:17:26.940 - INFO] (): footprint cost in isTrajectoryFeasible: 0.00
# 后面一直是 0

costmap_model->footprintCost如果是-1，就是撞了障碍，但只有第一次是-1，后面全是0. 撞一次就离开了障碍，自然是0. 之所以继续撞，是因为路径规划的开头，手动让机器人位姿先调整到transformed_plan的第一个元素的朝向，这二者没有关系。