TEB算法4 - teb源码阅读笔记之TebLocalPlannerROS类

1.TebLocalPlannerROS类概述

TebLocalPlannerROS类是对外交互类，move_base对该算法的调用接口，均在该类中实现。主要包括：

• initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_); //初始化
• setPlan(*controller_plan_) //设置全局路径规划结果
• isGoalReached() //判断目标是否到达
• computeVelocityCommands(cmd_vel) //计算速度

2.initialize()初始化

初始化中主要实现加载参数、配置障碍、可视化、获得机器人footprint、获取costmap、获取全局、局部坐标系名称、
costmap_converter_插件、订阅自定义障碍、动态配置、创建局部规划器实例（是否开启同伦规划）等；
代码如下：

//初始化
void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
  // 检查插件是否已经初始化
  if(!initialized_)
  {
    name_ = name;
    // create Node Handle with name of plugin (as used in move_base for loading)
    ros::NodeHandle nh("~/" + name);

    // 通过nodehandle获得TebConfig类的参数以及覆盖默认配置
    cfg_.loadRosParamFromNodeHandle(nh);

    // 为障碍物保留一定的存储空间
    obstacles_.reserve(500);

    // 创建可视化的实例
    visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_));

    // 创建机器人的footprint（轮廓）模型，用于优化
    RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh, cfg_);

    // 创建局部规划器实例
    if (cfg_.hcp.enable_homotopy_class_planning)
    {
      planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies enabled.");
    }
    else
    {
      planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies disabled.");
    }

    // 初始化其他的变量
    tf_ = tf;
    costmap_ros_ = costmap_ros;
    costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase.

    costmap_model_ = boost::make_shared<base_local_planner::CostmapModel>(*costmap_);

    global_frame_ = costmap_ros_->getGlobalFrameID();
    cfg_.map_frame = global_frame_; // TODO
    robot_base_frame_ = costmap_ros_->getBaseFrameID();

    // 初始化用于多边形转换器的代价地图
    if (!cfg_.obstacles.costmap_converter_plugin.empty())
    {
      try
      {
        // 获取插件名称，创建插件实例
        costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin);
        std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin);
        // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
        boost::replace_all(converter_name, "::", "/");
        costmap_converter_->setOdomTopic(cfg_.odom_topic);
        costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));
        costmap_converter_->setCostmap2D(costmap_);

        costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread);
        ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded.");
      }
      catch(pluginlib::PluginlibException& ex)
      {
        ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what());
        costmap_converter_.reset();
      }
    }
    else
      ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");

    // 获得机器人的footprint以及机器人中心到footprint顶点的最小和最大距离
    footprint_spec_ = costmap_ros_->getRobotFootprint();
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);

    // 初始化odom helper,用于从odom消息中接收机器人的速度
    odom_helper_.setOdomTopic(cfg_.odom_topic);

    // 动态配置
    dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(nh);
    dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);

    // 校验用于优化footprint和代价的footprint
    validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_.obstacles.min_obstacle_dist);

    // 设置自定义障碍物的回调函数
    custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

    // 设置自定义via-points(路径点)的回调函数
    via_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this);

    // 初始化检查失败的机制
    ros::NodeHandle nh_move_base("~");
    double controller_frequency = 5;
    nh_move_base.param("controller_frequency", controller_frequency, controller_frequency);
    failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency));

    // 初始化完成，设置初始化的标志位为true
    initialized_ = true;

    ROS_DEBUG("teb_local_planner plugin initialized.");
  }
  else
  {
    ROS_WARN("teb_local_planner has already been initialized, doing nothing.");
  }
}

3.computeVelocityCommands()速度计算

结果反馈

下载的版本是melodic，其中增加了对move_base_flex接口的支持，返回的结果中采用了mbf_msgs::ExePath消息体，从返回码中可获得局部路径规划的成功或失败的原因。

其中[mbf_msgs::ExePath.action中定义的返回码如下

uint8 SUCCESS         = 0
uint8 FAILURE         = 100  # Unspecified failure, only used for old, non-mfb_core based plugins
uint8 CANCELED        = 101
uint8 NO_VALID_CMD    = 102
uint8 PAT_EXCEEDED    = 103
uint8 COLLISION       = 104
uint8 OSCILLATION     = 105
uint8 ROBOT_STUCK     = 106
uint8 MISSED_GOAL     = 107
uint8 MISSED_PATH     = 108
uint8 BLOCKED_PATH    = 109
uint8 INVALID_PATH    = 110
uint8 TF_ERROR        = 111
uint8 NOT_INITIALIZED = 112
uint8 INVALID_PLUGIN  = 113
uint8 INTERNAL_ERROR  = 114
uint8 OUT_OF_MAP      = 115  # The start and / or the goal are outside the map
uint8 MAP_ERROR       = 116  # The map is not running properly
uint8 STOPPED         = 117  # The controller execution has been stopped rigorously.

目前在teb中已实现的返回结果，包括下面几种

mbf_msgs::ExePathResult::SUCCESS     #成功
mbf_msgs::ExePathResult::NOT_INITIALIZED;  #初始化失败
mbf_msgs::ExePathResult::INTERNAL_ERROR;   #内部错误（全局路径坐标系转换失败）
mbf_msgs::ExePathResult::INVALID_PATH      #收到的全局路径无效
mbf_msgs::ExePathResult::NO_VALID_CMD       #局部路径规划结果无效

相比move_base调用速度计算只得到成功或失败两种结果，无法获得失败的具体原因，move_base_flex显得更加详细。当然mbf的最主要特色是够灵活。

实现流程

具体的计算流程如下：

使用move_base调用的该接口计算速度

// 使用move_base调用的该接口计算速度
bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
  std::string dummy_message;
  geometry_msgs::PoseStamped dummy_pose;
  geometry_msgs::TwistStamped dummy_velocity, cmd_vel_stamped;
  uint32_t outcome = computeVelocityCommands(dummy_pose, dummy_velocity, cmd_vel_stamped, dummy_message);
  cmd_vel = cmd_vel_stamped.twist;
  return outcome == mbf_msgs::ExePathResult::SUCCESS;
}

在mov_base_flex直接调用该接口计算速度

主要流程：
大致流程就是先对全局规划器规划出的*global_plan_*进行一些预处理，包括截取部分路径、转换坐标系，
然后更新via_points（在全局路径上等间隔选出），更新障碍物信息
若使用costmap_converter插件，可以将障碍物做一定的简化，简化为线段、多边形等，可在一定程度上提高规划效率 ，
然后执行局部轨迹规划，最后保证轨迹是无碰撞的情况下则返回规划结果即机器人需要执行的速度指令

这里主要是包工头角色，本质还是调用planner_->plan（）去执行算法优化计算过程。

代码如下：

uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose,
                                                     const geometry_msgs::TwistStamped& velocity,
                                                     geometry_msgs::TwistStamped &cmd_vel,
                                                     std::string &message)
{
  // 插件是否已经初始化
  if(!initialized_)
  {
    ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner");
    message = "teb_local_planner has not been initialized";
    return mbf_msgs::ExePathResult::NOT_INITIALIZED;
  }

  static uint32_t seq = 0;
  cmd_vel.header.seq = seq++;
  cmd_vel.header.stamp = ros::Time::now();
  cmd_vel.header.frame_id = robot_base_frame_;
  cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
  goal_reached_ = false;

  // 获得机器人位姿
  geometry_msgs::PoseStamped robot_pose;
  costmap_ros_->getRobotPose(robot_pose);
  robot_pose_ = PoseSE2(robot_pose.pose);

  // 获取机器人速度
  geometry_msgs::PoseStamped robot_vel_tf;
  odom_helper_.getRobotVel(robot_vel_tf);
  robot_vel_.linear.x = robot_vel_tf.pose.position.x;
  robot_vel_.linear.y = robot_vel_tf.pose.position.y;
  robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);

  // 裁剪已经走过的全局路径 (spatially before the robot)
  pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);

  // 转换全局路径到特定坐标系下(w.r.t. the local costmap)
  std::vector<geometry_msgs::PoseStamped> transformed_plan;
  int goal_idx;
  geometry_msgs::TransformStamped tf_plan_to_global;
  if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist,
                           transformed_plan, &goal_idx, &tf_plan_to_global))
  {
    ROS_WARN("Could not transform the global plan to the frame of the controller");
    message = "Could not transform the global plan to the frame of the controller";
    return mbf_msgs::ExePathResult::INTERNAL_ERROR;
  }

  // 更新via-points容器
  if (!custom_via_points_active_)
    updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);

  nav_msgs::Odometry base_odom;
  odom_helper_.getOdom(base_odom);

  // 检查是否已经到了目标点
  geometry_msgs::PoseStamped global_goal;
  tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global);
  double dx = global_goal.pose.position.x - robot_pose_.x();
  double dy = global_goal.pose.position.y - robot_pose_.y();
  double delta_orient = g2o::normalize_theta( tf2::getYaw(global_goal.pose.orientation) - robot_pose_.theta() );
  //距离及角度误差同时满足阈值，差速底盘可将此处改为先到点，然后原地转向调整角度
  if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
    && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
    && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0)
    && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel)
        || cfg_.goal_tolerance.free_goal_vel))
  {
    goal_reached_ = true;
    return mbf_msgs::ExePathResult::SUCCESS;
  }

  // 检查是否进入备份模式并进行相关的设置
  configureBackupModes(transformed_plan, goal_idx);


  // 如果转换后的全局路径为空，返回false
  if (transformed_plan.empty())
  {
    ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
    message = "Transformed plan is empty";
    return mbf_msgs::ExePathResult::INVALID_PATH;
  }

  // 获得当前的目标点，也就是transformed_plan的最后一个点
  robot_goal_.x() = transformed_plan.back().pose.position.x;
  robot_goal_.y() = transformed_plan.back().pose.position.y;
  // 覆盖目标点的朝向，如果有必要的话
  if (cfg_.trajectory.global_plan_overwrite_orientation)
  {
    robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);
    //  (enable using the plan as initialization) 用真实的目标点朝向覆盖transformed_plan的目标点朝向
    tf2::Quaternion q;
    q.setRPY(0, 0, robot_goal_.theta());
    tf2::convert(q, transformed_plan.back().pose.orientation);
  }
  else
  {
    //直接使用全局路径的朝向
    robot_goal_.theta() = tf2::getYaw(transformed_plan.back().pose.orientation);
  }

  // 用真实的起始点位置覆盖transformed_plan的起始点位置 (allows using the plan as initial trajectory)
  if (transformed_plan.size()==1) // 路径中只有目标点
  {
    transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // 插入起始位姿（还没有初始化）
  }
  transformed_plan.front() = robot_pose; // 更新起始点

  // 清除掉现存的障碍物
  obstacles_.clear();

  // 用代价地图信息或者costmap_converter提供的多边形信息来更新障碍物容器
  if (costmap_converter_)
    updateObstacleContainerWithCostmapConverter();
  else
    updateObstacleContainerWithCostmap();

  // 也考虑自定义障碍物，必须在其他的更新后在被调用，因为该容器没有被清理
  updateObstacleContainerWithCustomObstacles();


  // 加锁，在接下来的优化过程中不允许配置被修改
  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
  bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
  if (!success)
  {
    planner_->clearPlanner(); // 强制重新初始化
    ROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting.");

    ++no_infeasible_plans_; // 不可行方案数+1
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel.twist;
    message = "teb_local_planner was not able to obtain a local plan";
    return mbf_msgs::ExePathResult::NO_VALID_CMD;
  }

  // 是否发散
  if (planner_->hasDiverged())
  {
    cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;

    // 重置所有变量，再次开始新轨迹的初始化
    planner_->clearPlanner();
    ROS_WARN_THROTTLE(1.0, "TebLocalPlannerROS: the trajectory has diverged. Resetting planner...");

    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel.twist;
    return mbf_msgs::ExePathResult::NO_VALID_CMD;
  }

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

  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.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;

    // 重置所有变量，再次开始新轨迹的初始化
    planner_->clearPlanner();
    ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...");

    ++no_infeasible_plans_; // 不可行路径的数量+1
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel.twist;
    message = "teb_local_planner trajectory is not feasible";
    return mbf_msgs::ExePathResult::NO_VALID_CMD;
  }

  // 为这个采样区间获取速度命令
  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();
    ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner...");
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel.twist;
    message = "teb_local_planner velocity command invalid";
    return mbf_msgs::ExePathResult::NO_VALID_CMD;
  }

  //速度硬约束
  // 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();
      ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...");
      ++no_infeasible_plans_; // increase number of infeasible solutions in a row
      time_last_infeasible_plan_ = ros::Time::now();
      message = "teb_local_planner steering angle is not finite";
      return mbf_msgs::ExePathResult::NO_VALID_CMD;
    }
  }

  // 找到可行的局部轨迹，重置计数器
  no_infeasible_plans_ = 0;

  // 存储上个命令，方便恢复的分析
  last_cmd_ = cmd_vel.twist;

  // 可视化障碍物，路过点，全局路径
  planner_->visualize();
  visualization_->publishObstacles(obstacles_);
  visualization_->publishViaPoints(via_points_);
  visualization_->publishGlobalPlan(global_plan_);
  return mbf_msgs::ExePathResult::SUCCESS;
}