Autoware planning模塊學習筆記（二）：路徑規劃（2）- 節點waypoint_replanner（下）

Autoware planning模塊學習筆記（二）：路徑規劃（2）- 節點waypoint_replanner（下）

這篇博客承接Autoware planning模塊學習筆記（二）：路徑規劃（2）- 節點waypoint_replanner（上）繼續講解waypoint_replanner節點。上一篇將節點waypoint_replanner中函數WaypointReplanner::replanLaneWaypointVel（位於src/autoware/core_planning/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.cpp）所調用的函數WaypointReplanner::resampleLaneWaypoint分析完成。這一篇博客回到函數WaypointReplanner::replanLaneWaypointVel繼續分析。（下面帶上序號了，感覺之前幾篇Autoware代碼分析的博客寫的邏輯有些亂，大家多包涵，不好意思哈～）

1. WaypointReplanner::replanLaneWaypointVel函數

我們仍然不管具體的參數設置，對所有代碼進行分析。

  if (replan_curve_mode_)
  {
    std::vector<double> curve_radius;
    KeyVal curve_list;
    createRadiusList(lane, curve_radius);//將lane中每個軌跡點（除去第一個和最後一個軌跡點）的半徑存入傳入函數的變量curve_radius。
    createCurveList(curve_radius, curve_list);
    if (overwrite_vmax_mode_)
    {// set velocity_max for all_point
      setVelocityByRange(0, last, 0, velocity_max_, lane);
    }
    // set velocity by curve
    for (const auto& el : curve_list)
    {
      const double& radius = el.second.second;
      double vmin = velocity_max_ - vel_param_ * (r_th_ - radius);
      vmin = (vmin < velocity_min_) ? velocity_min_ : vmin;
      limitVelocityByRange(el.first, el.second.first, velocity_offset_, vmin, lane);
    }
  }

1.1 createRadiusList函數

將每個軌跡點（除去第一個和最後一個軌跡點）的半徑存入傳入函數的變量curve_radius。

void WaypointReplanner::createRadiusList(const autoware_msgs::Lane& lane, std::vector<double>& curve_radius)
{
  if (lane.waypoints.empty())
  {
    return;
  }
  curve_radius.resize(lane.waypoints.size());
  curve_radius.at(0) = curve_radius.back() = r_inf_;//賦初值

  for (unsigned long i = 1; i < lane.waypoints.size() - 1; i++)
  {
    CbufGPoint curve_point(getCrvPoints(lane, i));//調用getCrvPoints函數
    const std::vector<double> curve_param(calcCurveParam(curve_point));//calcCurveParam函數在上一篇博客已分析
    //根據上一篇博客我們知道calcCurveParam返回軌跡點所在軌跡的圓心和半徑

    // if going straight
    if (curve_param.empty())
    {
      curve_radius.at(i) = r_inf_;
    }
    // else if turnning curve
    else
    {
      curve_radius.at(i) = (curve_param[2] > r_inf_) ? r_inf_ : curve_param[2];
    }
  }
}

1.1.1 getCrvPoints函數

CbufGPoint的定義是typedef boost::circular_buffer<geometry_msgs::Point> CbufGPoint;（位於src/autoware/core_planning/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.h）。
typedef在這裏的作用是定義一種類型的別名，而不只是簡單的宏替換。可以用作同時聲明指針型的多個對象。更詳細的參考typedef - 濤聲依舊~ - 博客園。circular_buffer的作用是在內存中記錄最近一段時間的數據，如此處的geometry_msgs::Point等。內部用一塊連續內存保存數據，元素的下標從$0\to n-1$依次增大(首位元素下標爲$0$，末位元素下標爲$n - 1$)。如果達到容量上限，繼續push_back方法壓入元素時（防盜標記：zhengkunxian），原來下標爲$0$的元素就會被覆蓋，原來下標爲$1$處的元素佔據下標爲$0$處的位置，push_front功能類似（數據隊列移動的方向相反）。綜上， boost::circular_buffer<geometry_msgs::Point> CbufGPoint的作用就是將數據類型爲geometry_msgs::Point且具備移除舊數據以維持固定容量的數組結構重命名爲CbufGPoint。

接下來const unsigned long curve_index[3] = { (index < n) ? 0 : (index - n), index, (index >= lane.waypoints.size() - n) ? (lane.waypoints.size() - 1) : (index + n) };起到的作用跟上一博客（Autoware planning模塊學習筆記（二）：路徑規劃（2）- 節點waypoint_replanner（上））所分析的WaypointReplanner::getCrvPointsOnResample函數中的const autoware_msgs::Waypoint cp[3] = {(lane.waypoints.size() < n) ? lane.waypoints.front() : lane.waypoints[lane.waypoints.size() - n], original_lane.waypoints[id], (id < original_lane.waypoints.size() - n) ? original_lane.waypoints[id + n] : original_lane.waypoints.back()};的作用是一致的。最後返回包含了選定的三個路徑點的座標的CbufGPoint。

const CbufGPoint WaypointReplanner::getCrvPoints(const autoware_msgs::Lane& lane, unsigned long index) const
{
  CbufGPoint curve_point(3);//定義了容量爲3的CbufGPoint
  const unsigned int n = (lookup_crv_width_ - 1) / 2;//lookup_crv_width_在參數初始化處被定義爲5，因此n=2
  const unsigned long curve_index[3] = { (index < n) ? 0 : (index - n), index, (index >= lane.waypoints.size() - n) ?
                                                                                   (lane.waypoints.size() - 1) :
                                                                                   (index + n) };
  for (int i = 0; i < 3; i++)
  {
    curve_point.push_back(lane.waypoints[curve_index[i]].pose.pose.position);
  }
  return curve_point;
}

1.1.2 calcCurveParam函數

calcCurveParam函數在上一篇博客已分析，根據上一篇博客我們知道calcCurveParam返回軌跡點所在軌跡的圓心和半徑。

1.2 createCurveList函數

傳入createCurveList函數的curve_list爲KeyVal類型變量，KeyVal定義爲typedef std::unordered_map<unsigned long, std::pair<unsigned long, double>> KeyVal;（位於src/autoware/core_planning/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.h）。std::unorederd_map 是一個關聯容器，其中的元素根據鍵來引用，而不是根據（防盜標記：zhengkunxian）索引來引用，並且具有鍵唯一及元素無序排列的特點。由此可知KeyVal實際上是一個鍵類型爲unsigned long，元素爲std::pair<unsigned long, double>類型的關聯容器。pair是一種模板類型，其中包含兩個數據值，兩個數據的類型可以不同。

void WaypointReplanner::createCurveList(const std::vector<double>& curve_radius, KeyVal& curve_list)
{
  unsigned long index = 0;
  bool on_curve = false;
  double radius_localmin = DBL_MAX;//DBL_MAX爲double型的最大值
  for (unsigned long i = 1; i < curve_radius.size(); i++)//計算軌跡點所在曲線的半徑時未計算第一個和最後一個軌跡點
  {
    if (!on_curve && curve_radius[i] <= r_th_ && curve_radius[i - 1] > r_th_)//r_th_：手動設置的一個半徑界限值
    {
      index = i;
      on_curve = true;
    }
    else if (on_curve && curve_radius[i - 1] <= r_th_ && curve_radius[i] > r_th_)
    {
      on_curve = false;
      if (radius_localmin < r_min_)
      {//防盜標記（zhengkunxian）
        radius_localmin = r_min_;
      }
      curve_list[index] = std::make_pair(i, radius_localmin);
      radius_localmin = DBL_MAX;
    }
    if (!on_curve)
    {
      continue;
    }
    if (radius_localmin > curve_radius[i])
    {
      radius_localmin = curve_radius[i];
    }
  }
}

createCurveList函數裏面的for循環的作用是什麼呢？博主做了個流程圖爲大家分析一下。我們假設r_th_=10.0，r_min_=1.0，curve_radius=[ r_inf_= 10*r_th_，5.2，15.8，0.7，10.8，10.8，r_inf_=10*r_th_ ]。分析可知最後的結果是curve_list[1]=(2,5.2)（鍵爲1，值爲（2，5.2）），curve_list[3]=(4,1.0) （鍵爲3，值爲（4，1.0））。

編寫代碼對上面的分析進行驗證，結果如下：

1.3 setVelocityByRange函數

其中last：const unsigned long last = lane.waypoints.size() - 1;
velocity_max_：velocity_max_ = kmph2mps(conf->velocity_max);

首先判斷lane中儲存的路徑點是否爲空，若不爲空則接着判斷offset（偏移）是否大於0，如果大於0則進一步對start_idx和end_idx進行處理。這裏replanLaneWaypointVel函數調用時因爲傳入的offset=0，因此跳過。接着爲了防止下標越界對end_idx進行檢查。檢查完畢後對lane中所有路徑點的速度進行設置。

void WaypointReplanner::setVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
                                             double vel, autoware_msgs::Lane& lane)
{
  if (lane.waypoints.empty())
  {
    return;
  }
  if (offset > 0)
  {
    start_idx = (start_idx > offset) ? (start_idx - offset) : 0;
    end_idx = (end_idx > offset) ? (end_idx - offset) : 0;
  }
  end_idx = (end_idx >= lane.waypoints.size()) ? lane.waypoints.size() - 1 : end_idx;
  for (unsigned long idx = start_idx; idx <= end_idx; idx++)
  {
    lane.waypoints[idx].twist.twist.linear.x = vel;
  }
}

回到replanLaneWaypointVel函數的主流程繼續執行：

    // set velocity by curve
    for (const auto& el : curve_list)
    {
      const double& radius = el.second.second;
      double vmin = velocity_max_ - vel_param_ * (r_th_ - radius);
      vmin = (vmin < velocity_min_) ? velocity_min_ : vmin;
      limitVelocityByRange(el.first, el.second.first, velocity_offset_, vmin, lane);
    }

其中vel_param_的定義爲 double vel_param_ = calcVelParam(velocity_max_); calcVelParam函數如下：

const double WaypointReplanner::calcVelParam(double vmax) const
{
  if (fabs(r_th_ - r_min_) < 1e-8)
  {
    return DBL_MAX;  // error
  }
  return (vmax - velocity_min_) / (r_th_ - r_min_);//velocity_min_ = kmph2mps(conf->velocity_min);
  //返回一個速度範圍(vmax - velocity_min_)與半徑範圍(r_th_ - r_min_)的比率
}

1.4 limitVelocityByRange函數

limitVelocityByRange函數在上一篇博客已分析，根據上一篇博客我們知道limitVelocityByRange對傳入下標範圍內的所有路徑點的速度進行最大值的限制。

回到replanLaneWaypointVel函數的主流程繼續執行：

// set velocity on start & end of lane
  if (replan_endpoint_mode_)
  {
    const unsigned long zerospeed_start = last - end_point_offset_;
    const unsigned long lowspeed_start = zerospeed_start - braking_distance_;
    raiseVelocityByRange(0, last, 0, velocity_min_, lane);
    limitVelocityByRange(0, 0, 0, velocity_min_, lane);
    limitVelocityByRange(lowspeed_start, last, 0, velocity_min_, lane);
    setVelocityByRange(zerospeed_start, last, 0, 0.0, lane);
  }

1.5 raiseVelocityByRange函數

其作用是保證選定範圍內路徑點的速度不小於傳入的vmin。

void WaypointReplanner::raiseVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
                                           double vmin, autoware_msgs::Lane& lane)
{
  if (lane.waypoints.empty())
  {
    return;
  }
  if (offset > 0)
  {
    start_idx = (start_idx > offset) ? (start_idx - offset) : 0;
    end_idx = (end_idx > offset) ? (end_idx - offset) : 0;
  }
  end_idx = (end_idx >= lane.waypoints.size()) ? lane.waypoints.size() - 1 : end_idx;
  for (unsigned long idx = start_idx; idx <= end_idx; idx++)
  {
    if (lane.waypoints[idx].twist.twist.linear.x >= vmin)
    {
      continue;
    }
    lane.waypoints[idx].twist.twist.linear.x = vmin;
  }
}

至此WaypointReplanner::replanLaneWaypointVel函數分析完畢，回溯到waypoint_replanner節點的消息回調函數WaypointReplannerNode::laneCallback進行收尾分析。

2. WaypointReplannerNode::laneCallback函數

void WaypointReplannerNode::laneCallback(const autoware_msgs::LaneArray::ConstPtr& lane_array)
{
  lane_array_ = *lane_array;
  publishLaneArray();
}

publishLaneArray函數

void WaypointReplannerNode::publishLaneArray()
{
  autoware_msgs::LaneArray array(lane_array_);
  if (replanning_mode_)
  {
    replan(array);//replan函數調用replanLaneWaypointVel函數
  }
  lane_pub_["with_decision"].publish(array);//發佈重規劃過的路徑集合，topic="/based/lane_waypoints_array"
  if (withoutDecisionMaker())//查看是否有"/decision_maker"這一節點，若無則返回true
  {
    lane_pub_["without_decision"].publish(array);//發佈重規劃過的路徑集合，topic="/lane_waypoints_array"
  }
}

withoutDecisionMaker函數

bool WaypointReplannerNode::withoutDecisionMaker()
{
  std::vector<std::string> node_list;
  ros::master::getNodes(node_list);
  return (std::find(node_list.begin(), node_list.end(), "/decision_maker") == node_list.end());
}

至此，waypoint_replanner節點分析完畢。

目前，“勾選waypoint_loader”這一操作時所啓動的waypoint_loader節點和waypoint_replanner已經分析結束，後面將繼續分析剩下的waypoint_marker_publisher這個節點。