ros2对teb调用代码流程

2022.09.06

2022.10.22

1.teb

(1) bt对teb的调用

加载
/home/liuyueming/catkin_ws/navigation/src/navigation2/nav2_bringup/launch/navigation_launch.py

/home/liuyueming/catkin_ws/navigation/src/navigation2/nav2_controller/src/main.cpp
auto node = std::make_shared<nav2_controller::ControllerServer>();
rclcpp::spin(node->get_node_base_interface());
#编译为
controller_server
#在
/home/liuyueming/catkin_ws/navigation/src/navigation2/nav2_bringup/launch/navigation_launch.py
#中调用

以action的方式被调用conputeControl()函数
/home/liuyueming/catkin_ws/navigation/src/navigation2/nav2_controller/src/controller_server.cpp

//controller_server.cpp中on_configure()注册action
action_server_ = std::make_unique<ActionServer>(
    shared_from_this(),
    "follow_path",
    std::bind(&ControllerServer::computeControl, this),
    nullptr,
    std::chrono::milliseconds(500),
    true);

action的定义
/home/liuyueming/catkin_ws/navigation/src/navigation2/nav2_msgs/action/FollowPath.action

/home/liuyueming/catkin_ws/navigation/src/navigation2/nav2_msgs/action/FollowPath.action
#中定义了action
#goal definition
nav_msgs/Path path
string controller_id
string goal_checker_id
---
#result definition
std_msgs/Empty result
---
#feedback definition
float32 distance_to_goal
float32 speed

computeControl()的实现
/home/liuyueming/catkin_ws/navigation/src/navigation2/nav2_controller/src/controller_server.cpp

//使用哪种局部规划器
std::string c_name = action_server_->get_current_goal()->controller_id;
//使用哪种目标检测器
std::string gc_name = action_server_->get_current_goal()->goal_checker_id;
//setPlannerPath设置路径
setPlannerPath(action_server_->get_current_goal()->path);
//setPlannerPath中
controllers_[current_controller_]->setPlan(path);//调用controller中的**setPlan**设置路径
//while循环中处理action
//1.检测action和costmap的合法性
//2.调用action_server_更新路径
updateGlobalPath();
//3.优化局部路径computeAndPublishVelocity();
//(1)调用controller的**computeVelocityCommands()**优化路径返回速度：
	cmd_vel_2d =
      controllers_[current_controller_]->computeVelocityCommands(
      pose,
      nav_2d_utils::twist2Dto3D(twist),
      goal_checkers_[current_goal_checker_].get());
//(2)返回速度和距离终点的距离
feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y);
feedback->distance_to_goal = nav2_util::geometry_utils::calculate_path_length(current_path_, find_closest_pose_idx());
//(3)发布速度
publishVelocity(cmd_vel_2d);
//4.调用goal_checkers_检测是否达到目标点
isGoalReached()；

(2) teb的实现

在action中被调用setPlan()和computeVelocityCommands()，分别用来设置全局路径和优化路径

---

ros接口
/home/liuyueming/catkin_ws/navigation/src/navigation2/teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp

void TebLocalPlannerROS::setPlan(const nav_msgs::msg::Path & orig_global_plan)
{
  // store the global plan
  global_plan_.clear();
  global_plan_.reserve(orig_global_plan.poses.size());
  for(const auto &in_pose :orig_global_plan.poses) {
    geometry_msgs::msg::PoseStamped out_pose;
    out_pose.pose = in_pose.pose;
    out_pose.header = orig_global_plan.header;
    global_plan_.push_back(out_pose);
  }
}

geometry_msgs::msg::TwistStamped TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
  const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker)
{
  // check if plugin initialized
  if(!initialized_)
  {
    throw nav2_core::PlannerException(
      std::string("teb_local_planner has not been initialized, please call initialize() before using this planner")
    );
  }
  geometry_msgs::msg::TwistStamped cmd_vel;
  
  cmd_vel.header.stamp = clock_->now();
  cmd_vel.header.frame_id = robot_base_frame_;
  cmd_vel.twist.linear.x = 0;
  cmd_vel.twist.linear.y = 0;
  cmd_vel.twist.angular.z = 0;

  // Update for the current goal checker's state
  geometry_msgs::msg::Pose pose_tolerance;
  geometry_msgs::msg::Twist vel_tolerance;
  if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) {
    RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!");
  } else {
    cfg_->goal_tolerance.xy_goal_tolerance = pose_tolerance.position.x;
  }
  
  // Get robot pose
  robot_pose_ = PoseSE2(pose.pose);
  geometry_msgs::msg::PoseStamped robot_pose;
  robot_pose.header = pose.header;
  robot_pose_.toPoseMsg(robot_pose.pose);
  
  // Get robot velocity
  robot_vel_ = velocity;
  
  // prune global plan to cut off parts of the past (spatially before the robot)在全局路径上修剪去掉机器人已经走过的路径，直到路径起始点距离机器人的位置小于global_plan_prune_distance。
  pruneGlobalPlan(robot_pose, global_plan_, cfg_->trajectory.global_plan_prune_distance); //1m

  // Transform global plan to the frame of interest (w.r.t. the local costmap)将global_plan转到global_frame_，global_plan根据costmap范围和max_global_plan_lookahead_dist路径长度对路径进行修剪
  std::vector<geometry_msgs::msg::PoseStamped> transformed_plan;
  int goal_idx;
  geometry_msgs::msg::TransformStamped tf_plan_to_global;
  if (!transformGlobalPlan(global_plan_, robot_pose, *costmap_, global_frame_, cfg_->trajectory.max_global_plan_lookahead_dist,
                           transformed_plan, &goal_idx, &tf_plan_to_global))
  {
    throw nav2_core::PlannerException(
      std::string("Could not transform the global plan to the frame of the controller")
    );
  }

  // update via-points container , 设置false
  if (!custom_via_points_active_)
    updateViaPointsContainer(transformed_plan, cfg_->trajectory.global_plan_viapoint_sep);//0.3

  // check if we should enter any backup mode and apply settings解决机器人的一些case，机器人规划异常则路径减半，之后检测振荡https://blog.csdn.net/Draonly/article/details/125123756
  configureBackupModes(transformed_plan, goal_idx);
    
  // Return false if the transformed global plan is empty
  if (transformed_plan.empty())
  {
    throw nav2_core::PlannerException(
      std::string("Transformed plan is empty. Cannot determine a local plan.")
    );
  }
              
  // Get current goal point (last point of the transformed plan)
  const geometry_msgs::msg::PoseStamped &goal_point = transformed_plan.back();
  robot_goal_.x() = goal_point.pose.position.x;
  robot_goal_.y() = goal_point.pose.position.y;
  if (cfg_->trajectory.global_plan_overwrite_orientation)//机器人的局部路径终点是否使用全局路径终点的方位
  {
    robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global);
    // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
    //transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());
    tf2::Quaternion q;
    q.setRPY(0, 0, robot_goal_.theta());
    transformed_plan.back().pose.orientation = tf2::toMsg(q);
  }  
  else
  {
    robot_goal_.theta() = tf2::getYaw(goal_point.pose.orientation);
  }

  // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory)
  if (transformed_plan.size()==1) // plan only contains the goal
  {
    transformed_plan.insert(transformed_plan.begin(), geometry_msgs::msg::PoseStamped()); // insert start (not yet initialized)
  }
  //tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start;
  transformed_plan.front().pose = robot_pose.pose;//用机器人的位置代替规划起点，是否会造成和第二点之间距离过远的问题
    
  // clear currently existing obstacles
  obstacles_.clear();
  
  // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin，将障碍物转换为多边形，便于查找障碍物，添加地图中的障碍物
  if (costmap_converter_)
    updateObstacleContainerWithCostmapConverter();
  else
    updateObstacleContainerWithCostmap();
  
  // also consider custom obstacles (must be called after other updates, since the container is not cleared)；用户发布的障碍物信息
  updateObstacleContainerWithCustomObstacles();
  
    
  // Do not allow config changes during the following optimization step
  std::lock_guard<std::mutex> cfg_lock(cfg_->configMutex());
    
  // Now perform the actual planning
//   bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_->goal_tolerance.free_goal_vel); // straight line init
  bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_->goal_tolerance.free_goal_vel);//g2o求解
  if (!success)
  {
    planner_->clearPlanner(); // force reinitialization for next time
    
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = clock_->now();
    last_cmd_ = cmd_vel.twist;
    
    throw nav2_core::PlannerException(
      std::string("teb_local_planner was not able to obtain a local plan for the current setting.")
    );
  }

  // Check for divergence,g2o求解质量，没看懂参数意义，是发散还是收敛
  if (planner_->hasDiverged())
  {
    cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;

    // Reset everything to start again with the initialization of new trajectories.
    planner_->clearPlanner();
    RCLCPP_WARN_THROTTLE(logger_, *(clock_), 1, "TebLocalPlannerROS: the trajectory has diverged. Resetting planner...");

    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = clock_->now();
    last_cmd_ = cmd_vel.twist;
    throw nav2_core::PlannerException(
      std::string("TebLocalPlannerROS: velocity command invalid (hasDiverged). Resetting planner...")
    );
  }
         
  // Check feasibility (but within the first few states only)，false
  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();
    nav2_costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
  }
  //两点之间距离过大或者转角过大，就插值，并检测每个位置点的合法性，是否和障碍物碰撞
  bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_->trajectory.feasibility_check_no_poses, cfg_->trajectory.feasibility_check_lookahead_distance);
  if (!feasible)
  {
    cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
   
    // now we reset everything to start again with the initialization of new trajectories.
    planner_->clearPlanner();

    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = clock_->now();
    last_cmd_ = cmd_vel.twist;
    
    throw nav2_core::PlannerException(
      std::string("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...")
    );
  }

  // Get the velocity command for this sampling interval.根据前瞻距离control_look_ahead_poses计算机器人速度
  if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->trajectory.control_look_ahead_poses))
  {
    planner_->clearPlanner();
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = clock_->now();
    last_cmd_ = cmd_vel.twist;
    
    throw nav2_core::PlannerException(
      std::string("TebLocalPlannerROS: velocity command invalid. Resetting planner...")
    );
  }
  
  // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).限制速度在安全范围
  saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->robot.max_vel_x, cfg_->robot.max_vel_y,
                   cfg_->robot.max_vel_theta, cfg_->robot.max_vel_x_backwards);

  // convert rot-vel to steering angle if desired (carlike robot).将角速度转换为转向角度，对汽车模型有用，差速无用
  // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
  // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.
  if (cfg_->robot.cmd_angle_instead_rotvel)
  {
    cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z, cfg_->robot.wheelbase, 0.95*cfg_->robot.min_turning_radius);
    if (!std::isfinite(cmd_vel.twist.angular.z))
    {
      cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
      last_cmd_ = cmd_vel.twist;
      planner_->clearPlanner();

      ++no_infeasible_plans_; // increase number of infeasible solutions in a row
      time_last_infeasible_plan_ = clock_->now();
      
      throw nav2_core::PlannerException(
        std::string("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...")
      );
    }
  }
  
  // a feasible solution should be found, reset counter
  no_infeasible_plans_ = 0;
  
  // store last command (for recovery analysis etc.)
  last_cmd_ = cmd_vel.twist;
  
  // Now visualize everything    
  planner_->visualize();
  visualization_->publishObstacles(obstacles_);
  visualization_->publishViaPoints(via_points_);
  visualization_->publishGlobalPlan(global_plan_);
  
  return cmd_vel;
}

---

调用g2o优化器求解
/home/liuyueming/catkin_ws/navigation/src/navigation2/teb_local_planner/teb_local_planner/src/optimal_planner.cpp

ool TebOptimalPlanner::plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
{ 
  //维护了pose和dt两个vector   
  TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
  //pose的大小为点的数量，dt按照匀速速度计算
  if (!teb_.isInit())//teb作初始化bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}
  {//计算方位并插入每个路径点，如果小于最小采样min_samples，则在倒数第二个点和终点之间进行插值
    teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
      cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
  }
  else // warm start
  {
    PoseSE2 start_(initial_plan.front().pose);
    PoseSE2 goal_(initial_plan.back().pose);
    //新路径的位置和与上一次规划的目标点goal相近，就不重新规划，而是基于上次轨迹，找到最近的点，剪枝后，优化上次的轨迹
    if (teb_.sizePoses()>0
        && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist
        && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
      teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB根据新起点修剪pose_vector和time_vector，并将起点和终点替换为新起点和终点
    else // goal too far away -> reinit
    {
      RCLCPP_DEBUG(node_->get_logger(), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
      teb_.clearTimedElasticBand();//清除pose_vector和time_vector
      teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
        cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);//再次初始化
    }
  }
  if (start_vel)
    setVelocityStart(*start_vel);
  if (free_goal_vel)
    setVelocityGoalFree();
  else
    vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
  
  // now optimize优化启动！
  return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);//优化的次数，g2o内部迭代次数
}

---

bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,
                                    double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
  if (cfg_->optim.optimization_activate==false) 
    return false;
  
  bool success = false;
  optimized_ = false;
  
  double weight_multiplier = 1.0;

  // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles
  //                (which leads to better results in terms of x-y-t homotopy planning).
  //                 however, we have not tested this mode intensively yet, so we keep
  //                 the legacy fast mode as default until we finish our tests.
  bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles;
  
  for(int i=0; i<iterations_outerloop; ++i)
  {
    if (cfg_->trajectory.teb_autosize)
    {
      //对路径进行插值，调整路径点之间的时间差，使得小于规划频率dt_ref+dt_hysteresis，但是不要超过max_samples；如果小于dt_ref-dt_hysteresis就去掉
      //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
      teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
    }
    //构建图，添加边和点
    success = buildGraph(weight_multiplier);
    if (!success) 
    {
        clearGraph();
        return false;
    }
    success = optimizeGraph(iterations_innerloop, false);
    if (!success) 
    {
        clearGraph();
        return false;
    }
    optimized_ = true;
    
    if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration
      computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
      
    clearGraph();
    
    weight_multiplier *= cfg_->optim.weight_adapt_factor;
  }

  return true;
}

---

2022.09.07

(3) 同伦类规划

采样：在每个障碍物的左侧和右侧采样一个无碰撞的点，和起点终点连接，构成概率路线图。
同伦类：如果两条路径构成的封闭环内没有障碍物，就是同伦，那么只选取其中的一条路径，以此方式去除多余的路径。
并行优化：开启多个线程，优化多条路径，选择最优的路径。
S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI,2010
C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.

(4) 障碍物处理

//optimal_planner
ObstContainer* obstacles_; //所有的障碍物
std::vector<ObstContainer> obstacles_per_vertex_;//和顶点对应的障碍物
//void TebOptimalPlanner::AddTEBVertices()
auto iter_obstacle = obstacles_per_vertex_.begin();
iter_obstacle->clear();
(iter_obstacle++)->reserve(obstacles_->size()); //ObstContainer obstacles_;
//void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier)
//障碍物距离<1.5*min_dis，则直接考虑，>5.0*min_dis直接忽略，之间的左右各保留一个最近障碍物
// force considering obstacle if really close to the current pose
        if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor)1.5
          {
              iter_obstacle->push_back(obst);
              continue;
          }
          // cut-off distance
          if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor)//5.0
            continue;
        
          // determine side (left or right) and assign obstacle if closer than the previous one
          if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
          {
              if (dist < left_min_dist)
              {
                  left_min_dist = dist;
                  left_obstacle = obst;
              }
          }
          else
          {
              if (dist < right_min_dist)
              {
                  right_min_dist = dist;
                  right_obstacle = obst;
              }
          }
	  if (left_obstacle)
        iter_obstacle->push_back(left_obstacle);
      if (right_obstacle)
        iter_obstacle->push_back(right_obstacle);

      // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges
      if (i == 0)
      {
        ++iter_obstacle;
        continue;
      }

      // create obstacle edges
      for (const ObstaclePtr obst : *iter_obstacle)
        create_edge(i, obst.get());
      ++iter_obstacle;
//obstacles.h
typedef std::vector<ObstaclePtr> ObstContainer;
typedef std::shared_ptr<Obstacle> ObstaclePtr;
class Obstacle

（6）障碍物处理

//W:\work\robot_slam\Robot\localplanner\teb\src\optimal_planner.cpp
double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get());
//W:\work\robot_slam\Robot\localplanner\teb\inc\robot_footprint_model.h
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
{
            Point2dContainer polygon_world(vertices_.size());//机器人外包轮廓
            transformToWorld(current_pose, polygon_world);//外包轮廓转到世界坐标系下
            return obstacle->getMinimumDistance(polygon_world);
}
//W:\work\robot_slam\Robot\localplanner\teb\inc\obstacles.h
virtual double getMinimumDistance(const Point2dContainer& polygon) const
{
            return distance_polygon_to_polygon_2d(polygon, vertices_);
}
//W:\work\robot_slam\Robot\localplanner\teb\inc\distance_calculations.h
inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2)
    {
        double dist = HUGE_VAL;

        // the polygon1 is a point
        if (vertices1.size() == 1)
        {
            return distance_point_to_polygon_2d(vertices1.front(), vertices2);//算点到各直线距离
        }

        // check each edge of polygon1每个点到各边直线的距离
        for (int i=0; i<(int)vertices1.size()-1; ++i)
        {
            double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2);
            if (new_dist < dist)
                dist = new_dist;
        }

        if (vertices1.size()>2) // if not a line close polygon1
        {
            double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edge
            if (new_dist < dist)
                return new_dist;
        }

        return dist;
    }

//

//W:\work\robot_slam\Robot\localplanner\teb\inc\robot_footprint_model.h
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
        {
            return obstacle->getMinimumDistance(current_pose.position()) - radius_;
        }
//W:\work\robot_slam\Robot\localplanner\teb\inc\obstacles.h
virtual double getMinimumDistance(const Eigen::Vector2d& position) const
        {
            return distance_point_to_polygon_2d(position, vertices_);
        }

//W:\work\robot_slam\Robot\localplanner\teb\inc\distance_calculations.h
inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices)
    {
        double dist = HUGE_VAL;

        // the polygon is a point
        if (vertices.size() == 1)
        {
            return (point - vertices.front()).norm();
        }

        // check each polygon edge
        for (int i=0; i<(int)vertices.size()-1; ++i)
        {
            double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1));
//       double new_dist = calc_distance_point_to_segment( position,  vertices.at(i), vertices.at(i+1));
            if (new_dist < dist)
                dist = new_dist;
        }

        if (vertices.size()>2) // if not a line close polygon
        {
            double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edge
            if (new_dist < dist)
                return new_dist;
        }

        return dist;
    }

（7）多边形机器人轮廓问题

1. I realized that this is an effect of setting inflation weight to 1.
   Reducing it to 0.1 solved the problem
2. I had a similar issue using include_dynamic_obstacles=true. It seems that teb behavior changes quite a lot when include_dynamic_obstaclesis set to true. Interestingly the implementation is faster, but more buggy.Maybe try again without dynamic obstacles?
3. 使用line代替多边形
4. costmap检测多边形的代码要看，与TEB不同，需要检测轨迹的可行性

obstacle_poses_affected: 25 -> 30
dt_ref: 3 -> 4
# and for a shorter computation time
no_inner_iterations: 8  -> 5 (default)
no_outer_iterations: 7  -> 4 (default)

6. computeCurrentCost()计算cost，加上。
7. preferRotIdr限制条件，

for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i)

8. 障碍物处降低速度
9. 计算收敛性

optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable);

10. 做插值处理，再看速度
11. Decreased inflation_dist and min_obstacle_dist
    Increased max_vel_theta and acc_lim_theta
    12.statusVelocity

（8）warm_start

1. https://github.com/rst-tu-dortmund/teb_local_planner/issues/314

（9）碰撞检测

teb_ros

footprint_spec_ = costmap_ros_->getRobotFootprint();//得到机器人轮廓点序列
costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); //根据机器人轮廓计算最小内接圆和最大外接圆

costmap

 std::vector<geometry_msgs::Point> getRobotFootprint()
  {
    return padded_footprint_;
  }
  ---
  void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
{
  min_dist = std::numeric_limits<double>::max();
  max_dist = 0.0;

  if (footprint.size() <= 2)
  {
    return;
  }

  for (unsigned int i = 0; i < footprint.size() - 1; ++i)
  {
    // check the distance from the robot center point to the first vertex
    double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
    double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
                                      footprint[i + 1].x, footprint[i + 1].y);
    min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
    max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
  }

  // we also need to do the last vertex and the first vertex
  double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
  double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
                                      footprint.front().x, footprint.front().y);
  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}

teb_ros

bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);

opt
// returns:
// -1 if footprint covers at least a lethal obstacle cell, or
// -2 if footprint covers at least a no-information cell, or
// -3 if footprint is [partially] outside of the map, or
// a positive value for traversable space

costmap_model->footprintCost(teb().Pose(i).x(), teb().Pose(i).y(), teb().Pose(i).theta(), footprint_spec, inscribed_radius, circumscribed_radius)

world_model.h，局部机器人轮廓转到全局坐标系

double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0){

        double cos_th = cos(theta);
        double sin_th = sin(theta);
        std::vector<geometry_msgs::Point> oriented_footprint;
        for(unsigned int i = 0; i < footprint_spec.size(); ++i){
          geometry_msgs::Point new_pt;
          new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
          new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
          oriented_footprint.push_back(new_pt);
        }

        geometry_msgs::Point robot_position;
        robot_position.x = x;
        robot_position.y = y;

        if(inscribed_radius==0.0){
          costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius);
        }

        return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
      }

costmap_model.cpp
计算多变形是否在障碍物上

double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
      double inscribed_radius, double circumscribed_radius){
    // returns:
    //  -1 if footprint covers at least a lethal obstacle cell, or
    //  -2 if footprint covers at least a no-information cell, or
    //  -3 if footprint is [partially] outside of the map, or
    //  a positive value for traversable space

    //used to put things into grid coordinates
    unsigned int cell_x, cell_y;

    //get the cell coord of the center point of the robot
    if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
      return -3.0;

    //if number of points in the footprint is less than 3, we'll just assume a circular robot
    if(footprint.size() < 3){
      unsigned char cost = costmap_.getCost(cell_x, cell_y);
      if(cost == NO_INFORMATION)
        return -2.0;
      if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
        return -1.0;
      return cost;
    }

    //now we really have to lay down the footprint in the costmap grid
    unsigned int x0, x1, y0, y1;
    double line_cost = 0.0;
    double footprint_cost = 0.0;

    //we need to rasterize each line in the footprint
    for(unsigned int i = 0; i < footprint.size() - 1; ++i){
      //get the cell coord of the first point
      if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
        return -3.0;

      //get the cell coord of the second point
      if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
        return -3.0;

      line_cost = lineCost(x0, x1, y0, y1);
      footprint_cost = std::max(line_cost, footprint_cost);

      //if there is an obstacle that hits the line... we know that we can return false right away
      if(line_cost < 0)
        return line_cost;
    }

    //we also need to connect the first point in the footprint to the last point
    //get the cell coord of the last point
    if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
      return -3.0;

    //get the cell coord of the first point
    if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
      return -3.0;

    line_cost = lineCost(x0, x1, y0, y1);
    footprint_cost = std::max(line_cost, footprint_cost);

    if(line_cost < 0)
      return line_cost;

    //if all line costs are legal... then we can return that the footprint is legal
    return footprint_cost;

  }

  //calculate the cost of a ray-traced line
  double CostmapModel::lineCost(int x0, int x1, int y0, int y1) const {
    double line_cost = 0.0;
    double point_cost = -1.0;

    for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() )
    {
      point_cost = pointCost( line.getX(), line.getY() ); //Score the current point

      if(point_cost < 0)
        return point_cost;

      if(line_cost < point_cost)
        line_cost = point_cost;
    }

    return line_cost;
  }

  double CostmapModel::pointCost(int x, int y) const {
    unsigned char cost = costmap_.getCost(x, y);
    //if the cell is in an obstacle the path is invalid
    if(cost == NO_INFORMATION)
      return -2;
    if(cost == LETHAL_OBSTACLE)
      return -1;

    return cost;
  }