（十二）学习笔记autoware源码core_planning（velocity_set)

1.libvelocity_set

#include <waypoint_planner/velocity_set/libvelocity_set.h>

// extract edge points from zebra zone
std::vector<geometry_msgs::Point> removeNeedlessPoints(std::vector<geometry_msgs::Point> &area_points)
{
  area_points.push_back(area_points.front());
  std::map<double, int> length_index;
  for (unsigned int i = 0; i < area_points.size() - 1; i++)
    length_index[calcSquareOfLength(area_points[i], area_points[i + 1])] = i;

  std::vector<geometry_msgs::Point> new_points;
  auto it = length_index.end();
  int first = (--it)->second;
  int second = (--it)->second;
  new_points.push_back(area_points[first]);
  new_points.push_back(area_points[first + 1]);
  new_points.push_back(area_points[second]);
  new_points.push_back(area_points[second + 1]);

  return new_points;
}

void CrossWalk::crossWalkCallback(const vector_map::CrossWalkArray &msg)
{
  crosswalk_ = msg;

  loaded_crosswalk = true;
  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
  {
    loaded_all = true;
    ROS_INFO("All VectorMap loaded");
  }
}

void CrossWalk::areaCallback(const vector_map::AreaArray &msg)
{
  area_ = msg;

  loaded_area = true;
  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
  {
    loaded_all = true;
    ROS_INFO("All VectorMap loaded");
  }
}

void CrossWalk::lineCallback(const vector_map::LineArray &msg)
{
  line_ = msg;

  loaded_line = true;
  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
  {
    loaded_all = true;
    ROS_INFO("All VectorMap loaded");
  }
}

void CrossWalk::pointCallback(const vector_map::PointArray &msg)
{
  point_ = msg;

  loaded_point = true;
  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
  {
    loaded_all = true;
    ROS_INFO("All VectorMap loaded");
  }
}

geometry_msgs::Point CrossWalk::getPoint(const int &pid) const
{
  geometry_msgs::Point point;
  for (const auto &p : point_.data)
  {
    if (p.pid == pid)
    {
      point.x = p.ly;
      point.y = p.bx;
      point.z = p.h;
      return point;
    }
  }

  ROS_ERROR("can't find a point of pid %d", pid);
  return point;
}

geometry_msgs::Point CrossWalk::calcCenterofGravity(const int &aid) const
{
  int search_lid = -1;
  for (const auto &area : area_.data)
    if (area.aid == aid)
    {
      search_lid = area.slid;
      break;
    }

  std::vector<geometry_msgs::Point> area_points;
  while (search_lid)
  {
    for (const auto &line : line_.data)
    {
      if (line.lid == search_lid)
      {
        area_points.push_back(getPoint(line.bpid));
        search_lid = line.flid;
      }
    }
  }

  geometry_msgs::Point point;
  point.x = 0.0;
  point.y = 0.0;
  point.z = 0.0;
  if (area_points.size() > 4)
  {
    std::vector<geometry_msgs::Point> filterd_points = removeNeedlessPoints(area_points);
    for (const auto &p : filterd_points)
    {
      point.x += p.x;
      point.y += p.y;
      point.z += p.z;
    }
  }
  else
  {
    for (const auto &p : area_points)
    {
      point.x += p.x;
      point.y += p.y;
      point.z += p.z;
    }
  }

  point.x /= 4;
  point.y /= 4;
  point.z /= 4;
  return point;
}

double CrossWalk::calcCrossWalkWidth(const int &aid) const
{
  int search_lid = -1;
  for (const auto &area : area_.data)
    if (area.aid == aid)
    {
      search_lid = area.slid;
      break;
    }

  std::vector<geometry_msgs::Point> area_points;
  while (search_lid)
  {
    for (const auto &line : line_.data)
    {
      if (line.lid == search_lid)
      {
        area_points.push_back(getPoint(line.bpid));
        //_points.push_back(area_points.back());///
        search_lid = line.flid;
      }
    }
  }

  area_points.push_back(area_points.front());
  double max_length = calcSquareOfLength(area_points[0], area_points[1]);
  for (unsigned int i = 1; i < area_points.size() - 1; i++)
  {
    if (calcSquareOfLength(area_points[i], area_points[i + 1]) > max_length)
      max_length = calcSquareOfLength(area_points[i], area_points[i + 1]);
  }

  return sqrt(max_length);
}

// count the number of crosswalks
int CrossWalk::countAreaSize() const
{
  int count = 0;
  for (const auto &x : crosswalk_.data)
    if (x.type == 0)  // type:0 -> outer frame of crosswalks
      count++;

  return count;
}

/*
getAID函数更新传入函数的bdid2aid_map，键为crosswalk_.data中每个斑马线的 bdid，键值为对应的aid。
*/
void CrossWalk::getAID(std::unordered_map<int, std::vector<int>> &bdid2aid_map) const
{
  for (const auto &x : crosswalk_.data)
    if (x.type == 1)
    {                                         // if it is zebra 如果是斑马线,以bdid 作为键,保存区域工D
      bdid2aid_map[x.bdid].push_back(x.aid);  // save area id
    }
}

//calcDetectionArea函数更新detection_points_,detection_points_为std::unordered_map<int, CrossWalkPoints>类型变量。
void CrossWalk::calcDetectionArea(const std::unordered_map<int, std::vector<int>> &bdid2aid_map)
{
  for (const auto &crosswalk_aids : bdid2aid_map)
  {
    int bdid = crosswalk_aids.first;
    double width = 0.0;
    for (const auto &aid : crosswalk_aids.second)
    {
      //calcCenterofGravity函数计算aid对应区域的重心坐标。
      detection_points_[bdid].points.push_back(calcCenterofGravity(aid));

      //calcCrossWalkWidth函数计算斑马线区域的最大宽度。
      width += calcCrossWalkWidth(aid);
    }
    width /= crosswalk_aids.second.size();
    detection_points_[bdid].width = width;
  }
}

//calcCenterPoints函数中分别计算bdID_中每个元素对应的多个重心的中心，并更新detection_points_。
void CrossWalk::calcCenterPoints()
{
  for (const auto &i : bdID_)
  {
    geometry_msgs::Point center;
    center.x = 0.0;
    center.y = 0.0;
    center.z = 0.0;
    for (const auto &p : detection_points_[i].points)
    {
      center.x += p.x;
      center.y += p.y;
      center.z += p.z;
    }
    center.x /= detection_points_[i].points.size();
    center.y /= detection_points_[i].points.size();
    center.z /= detection_points_[i].points.size();
    detection_points_[i].center = center;
  }
}

/*
更新crosswalk 内的bdID_(std::vector类型变量)，detection_points_(std::unordered_map<int, 
CrossWalkPoints>类型变量)和 set_points(bool类型变量)。
*/
void CrossWalk::setCrossWalkPoints()
{
  // bdid2aid_map[BDID] has AIDs of its zebra zone
  std::unordered_map<int, std::vector<int>> bdid2aid_map;
  getAID(bdid2aid_map);

  // Save key values  保存键值
  for (const auto &bdid2aid : bdid2aid_map)
    bdID_.push_back(bdid2aid.first);

  calcDetectionArea(bdid2aid_map);
  calcCenterPoints();

  ROS_INFO("Set cross walk detection points");
  set_points = true;
}

/*
findClosestCrosswalk函数获得无人车处于当前位置时在搜索范围内最靠近人行横道的轨迹点下标，
同时更新crosswalk 内的 detection_crosswalk_id_,detection_crosswalk_id_记录了所临近的人行横道对应的bdid。
*/
int CrossWalk::findClosestCrosswalk(const int closest_waypoint, const autoware_msgs::Lane &lane,
                                    const int search_distance)
{
  if (!set_points || closest_waypoint < 0)
    return -1;

  double find_distance = 2.0 * 2.0;      // meter
  double ignore_distance = 20.0 * 20.0;  // meter
  static std::vector<int> bdid = getBDID();

  int _return_val = 0;

  initDetectionCrossWalkIDs();  // for multiple

  // Find near cross walk
  for (int num = closest_waypoint; num < closest_waypoint + search_distance && num < (int)lane.waypoints.size(); num++)
  {
    geometry_msgs::Point waypoint = lane.waypoints[num].pose.pose.position;
    waypoint.z = 0.0;  // ignore Z axis
    for (const auto &i : bdid)
    {
      // ignore far crosswalk
      geometry_msgs::Point crosswalk_center = getDetectionPoints(i).center;
      crosswalk_center.z = 0.0;
      if (calcSquareOfLength(crosswalk_center, waypoint) > ignore_distance)
        continue;

      for (auto p : getDetectionPoints(i).points)
      {
        p.z = waypoint.z;
        if (calcSquareOfLength(p, waypoint) < find_distance)
        {
          addDetectionCrossWalkIDs(i);
          if (!this->isMultipleDetection())
          {
            setDetectionCrossWalkID(i);
            return num;
          }
          else if (!_return_val)
          {
            setDetectionCrossWalkID(i);
            _return_val = num;
          }
        }
      }
    }
  }

  if (_return_val)
    return _return_val;

  setDetectionCrossWalkID(-1);
  return -1;  // no near crosswalk
}

geometry_msgs::Point ObstaclePoints::getObstaclePoint(const EControl &kind) const
{
  geometry_msgs::Point point;

  if (kind == EControl::STOP || kind == EControl::STOPLINE)
  {
    for (const auto &p : stop_points_)
    {
      point.x += p.x;
      point.y += p.y;
      point.z += p.z;
    }
    point.x /= stop_points_.size();
    point.y /= stop_points_.size();
    point.z /= stop_points_.size();

    return point;
  }
  else  // kind == DECELERATE
  {
    for (const auto &p : decelerate_points_)
    {
      point.x += p.x;
      point.y += p.y;
      point.z += p.z;
    }
    point.x /= decelerate_points_.size();
    point.y /= decelerate_points_.size();
    point.z /= decelerate_points_.size();

    return point;
  }
}

2.velocity_set_info