(十二)学习笔记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

  • 1
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值