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

1.libvelocity_set

#include 

// extract edge points from zebra zone
std::vector removeNeedlessPoints(std::vector &area_points)
{
  area_points.push_back(area_points.front());
  std::map 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 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 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 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 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> &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类型变量。
void CrossWalk::calcDetectionArea(const std::unordered_map> &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类型变量)和 set_points(bool类型变量)。
*/
void CrossWalk::setCrossWalkPoints()
{
  // bdid2aid_map[BDID] has AIDs of its zebra zone
  std::unordered_map> 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 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

#include 

void joinPoints(const pcl::PointCloud& points1, pcl::PointCloud* points2)
{
  for (const auto& p : points1)
  {
    points2->push_back(p);
  }
}

VelocitySetInfo::VelocitySetInfo()
  : stop_range_(1.3),
    deceleration_range_(0),
    points_threshold_(10),
    detection_height_top_(0.2),
    detection_height_bottom_(-1.7),
    stop_distance_obstacle_(10),
    stop_distance_stopline_(5),
    deceleration_obstacle_(0.8),
    deceleration_stopline_(0.6),
    velocity_change_limit_(2.77),
    temporal_waypoints_size_(100),
    set_pose_(false),
    wpidx_detectionResultByOtherNodes_(-1)
{
  ros::NodeHandle private_nh_("~");
  ros::NodeHandle nh;

  double vel_change_limit_kph = 9.972;
  private_nh_.param("remove_points_upto", remove_points_upto_, 2.3);
  private_nh_.param("stop_distance_obstacle", stop_distance_obstacle_, 10.0);
  private_nh_.param("stop_distance_stopline", stop_distance_stopline_, 5.0);
  private_nh_.param("detection_range", stop_range_, 1.3);
  private_nh_.param("points_threshold", points_threshold_, 10);
  private_nh_.param("detection_height_top", detection_height_top_, 0.2);
  private_nh_.param("detection_height_bottom", detection_height_bottom_, -1.7);
  private_nh_.param("deceleration_obstacle", deceleration_obstacle_, 0.8);
  private_nh_.param("deceleration_stopline", deceleration_stopline_, 0.6);
  private_nh_.param("velocity_change_limit", vel_change_limit_kph, 9.972);
  private_nh_.param("deceleration_range", deceleration_range_, 0);
  private_nh_.param("temporal_waypoints_size", temporal_waypoints_size_, 100.0);

  velocity_change_limit_ = vel_change_limit_kph / 3.6;  // kph -> mps

  health_checker_ptr_ = std::make_shared(nh,private_nh_);
  health_checker_ptr_->ENABLE();
}

VelocitySetInfo::~VelocitySetInfo()
{
}

void VelocitySetInfo::clearPoints()
{
  points_.clear();
}


/*
 VelocitySetInfo类中的回调函数configCallback,pointsCallback, localizerPose-Callback，
 controlPoseCallback和detectionCallback 函数

 configCallback等函数的分析过程与前面VelocitySetPath类中的回调函数基本一致，其余类似函数不再赘述，
 详情请见源码。对于configCallback 函数，其中有些参数的含义需要进行解释。
*/
void VelocitySetInfo::configCallback(const autoware_config_msgs::ConfigVelocitySetConstPtr &config)
{
  //(m)相距障碍物的停车距离
  stop_distance_obstacle_ = config->stop_distance_obstacle;

  //(m)相距停车线的停车距离
  stop_distance_stopline_ = config->stop_distance_stopline;

  //如果障碍物在此范围内，停车
  stop_range_ = config->detection_range;

  //判断为障碍物的阈值
  points_threshold_ = config->threshold_points;
  detection_height_top_ = config->detection_height_top;
  detection_height_bottom_ = config->detection_height_bottom;

  //(m / s^2）遇到障碍物时减速的加速度
  deceleration_obstacle_ = config->deceleration_obstacle;

  //(m / s^2)遇到停车线时减速的减加速度
  deceleration_stopline_ = config->deceleration_stopline;

  //(kmph -> mps)速度变化限制
  velocity_change_limit_ = config->velocity_change_limit / 3.6; // kmph -> mps
  deceleration_range_ = config->deceleration_range;
  temporal_waypoints_size_ = config->temporal_waypoints_size;
}

void VelocitySetInfo::pointsCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
  health_checker_ptr_->CHECK_RATE("topic_rate_points_no_ground_slow", 8, 5, 1, "topic points_no_ground subscribe rate slow.");
  pcl::PointCloud sub_points;
  pcl::fromROSMsg(*msg, sub_points);

  points_.clear();
  for (const auto &v : sub_points)
  {
    if (v.x == 0 && v.y == 0)
      continue;

    if (v.z > detection_height_top_ || v.z < detection_height_bottom_)
      continue;

    // ignore points nearby the vehicle
    if (v.x * v.x + v.y * v.y < remove_points_upto_ * remove_points_upto_)
      continue;

    points_.push_back(v);
  }

}

void VelocitySetInfo::detectionCallback(const std_msgs::Int32 &msg)
{
    wpidx_detectionResultByOtherNodes_ = msg.data;
}

void VelocitySetInfo::controlPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
{
  control_pose_ = *msg;

  if (!set_pose_)
    set_pose_ = true;
}

void VelocitySetInfo::localizerPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
{
  health_checker_ptr_->NODE_ACTIVATE();
  health_checker_ptr_->CHECK_RATE("topic_rate_localizer_pose_slow", 8, 5, 1, "topic localizer_pose subscribe rate slow.");
  localizer_pose_ = *msg;
}

3.velocity_set_path

#include 

VelocitySetPath::VelocitySetPath()
  : set_path_(false),
    current_vel_(0)
{
  ros::NodeHandle private_nh_("~");
  private_nh_.param("velocity_offset", velocity_offset_, 1.2);
  private_nh_.param("decelerate_vel_min", decelerate_vel_min_, 1.3);
}

VelocitySetPath::~VelocitySetPath()
{
}

// check if waypoint number is valid
bool VelocitySetPath::checkWaypoint(int num, const char *name) const
{
  if (num < 0 || num >= getPrevWaypointsSize())
  {
    return false;
  }
  return true;
}

// set about '_temporal_waypoints_size' meter waypoints from closest waypoint
void VelocitySetPath::setTemporalWaypoints(int temporal_waypoints_size, int closest_waypoint, geometry_msgs::PoseStamped control_pose)
{
  if (closest_waypoint < 0)
    return;

  temporal_waypoints_.waypoints.clear();
  temporal_waypoints_.header = new_waypoints_.header;
  temporal_waypoints_.increment = new_waypoints_.increment;

  // push current pose
  autoware_msgs::Waypoint current_point;
  current_point.pose = control_pose;
  current_point.twist = new_waypoints_.waypoints[closest_waypoint].twist;
  current_point.dtlane = new_waypoints_.waypoints[closest_waypoint].dtlane;
  temporal_waypoints_.waypoints.push_back(current_point);

  for (int i = 0; i < temporal_waypoints_size; i++)
  {
    if (closest_waypoint + i >= getNewWaypointsSize())
      return;

    temporal_waypoints_.waypoints.push_back(new_waypoints_.waypoints[closest_waypoint + i]);
  }

  return;
}

double VelocitySetPath::calcChangedVelocity(const double& current_vel, const double& accel, const std::array& range) const
{
  static double current_velocity = current_vel;
  static double square_vel = current_vel * current_vel;
  if (current_velocity != current_vel)
  {
    current_velocity = current_vel;
    square_vel = current_vel * current_vel;
  }
  return std::sqrt(square_vel + 2.0 * accel * calcInterval(range.at(0), range.at(1)));
}

void VelocitySetPath::changeWaypointsForDeceleration(double deceleration, int closest_waypoint, int obstacle_waypoint)
{
  int extra = 4; // for safety

  // decelerate with constant deceleration
  for (int index = obstacle_waypoint + extra; index >= closest_waypoint; index--)
  {
    if (!checkWaypoint(index, __FUNCTION__))
      continue;

    // v = sqrt( (v0)^2 + 2ax )
    std::array range = {index, obstacle_waypoint};
    double changed_vel = calcChangedVelocity(decelerate_vel_min_, deceleration, range);

    double prev_vel = prev_waypoints_.waypoints[index].twist.twist.linear.x;
    const int sgn = (prev_vel < 0) ? -1 : 1;
    new_waypoints_.waypoints[index].twist.twist.linear.x = sgn * std::min(std::abs(prev_vel), changed_vel);
  }

}

void VelocitySetPath::avoidSuddenAcceleration(double deceleration, int closest_waypoint)
{
  for (int i = 0;; i++)
  {
    if (!checkWaypoint(closest_waypoint + i, __FUNCTION__))
      return;

    // accelerate with constant acceleration
    // v = root((v0)^2 + 2ax)
    std::array range = {closest_waypoint, closest_waypoint + i};
    double changed_vel = calcChangedVelocity(current_vel_, deceleration, range) + velocity_offset_;

    const double& target_vel = new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x;
    // Don't exceed original velocity
    if (changed_vel > std::abs(target_vel))
      return;

    const int sgn = (target_vel < 0) ? -1 : 1;
    new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x = sgn * changed_vel;
  }

  return;
}

void VelocitySetPath::avoidSuddenDeceleration(double velocity_change_limit, double deceleration, int closest_waypoint)
{
  if (closest_waypoint < 0)
    return;

  const double& closest_vel = new_waypoints_.waypoints[closest_waypoint].twist.twist.linear.x;

  // if accelerating, do not modify the speed profile.
  if ((current_vel_ >= 0.0 && current_vel_ <= closest_vel) || (current_vel_ < 0.0 && current_vel_ > closest_vel))
    return;

  // not avoid braking
  if (std::abs(current_vel_ - closest_vel) < velocity_change_limit)
    return;

  //std::cout << "avoid sudden braking!" << std::endl;
  for (int i = 0;; i++)
  {
    if (!checkWaypoint(closest_waypoint + i, __FUNCTION__))
      return;

    // sqrt(v^2 - 2ax)
    std::array range = {closest_waypoint, closest_waypoint + i};
    double changed_vel = calcChangedVelocity(std::abs(current_vel_) - velocity_change_limit, -deceleration, range);
    const double& target_vel = new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x;

    if (std::isnan(changed_vel))
    {
      break;
    }
    const int sgn = (target_vel < 0) ? -1 : 1;
    new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x = sgn * changed_vel;
  }

}

void VelocitySetPath::changeWaypointsForStopping(int stop_waypoint, int obstacle_waypoint, int closest_waypoint, double deceleration)
{
  if (closest_waypoint < 0)
    return;

  // decelerate with constant deceleration
  for (int index = stop_waypoint; index >= closest_waypoint; index--)
  {
    if (!checkWaypoint(index, __FUNCTION__))
      continue;

    // v = (v0)^2 + 2ax, and v0 = 0
    std::array range = {index, stop_waypoint};
    double changed_vel = calcChangedVelocity(0.0, deceleration, range);

    double prev_vel = prev_waypoints_.waypoints[index].twist.twist.linear.x;
    const int sgn = (prev_vel < 0) ? -1 : 1;
    new_waypoints_.waypoints[index].twist.twist.linear.x = sgn * std::min(std::abs(prev_vel), changed_vel);
  }

  // fill velocity with 0 for stopping
  for (int i = stop_waypoint; i <= obstacle_waypoint; i++)
  {
    new_waypoints_.waypoints[i].twist.twist.linear.x = 0;
  }

}

void VelocitySetPath::initializeNewWaypoints()
{
  new_waypoints_ = prev_waypoints_;
}

double VelocitySetPath::calcInterval(const int begin, const int end) const
{
  // check index
  if (begin < 0 || begin >= getPrevWaypointsSize() || end < 0 || end >= getPrevWaypointsSize())
  {
    ROS_WARN("Invalid index");
    return 0;
  }

  // Calculate the inteval of waypoints
  double dist_sum = 0;
  for (int i = begin; i < end; i++)
  {
    tf::Vector3 v1(prev_waypoints_.waypoints[i].pose.pose.position.x,
                   prev_waypoints_.waypoints[i].pose.pose.position.y, 0);

    tf::Vector3 v2(prev_waypoints_.waypoints[i + 1].pose.pose.position.x,
                   prev_waypoints_.waypoints[i + 1].pose.pose.position.y, 0);

    dist_sum += tf::tfDistance(v1, v2);
  }

  return dist_sum;
}

void VelocitySetPath::resetFlag()
{
  set_path_ = false;
}


/*
waypointsCallback函数更新 VelocitySetPath对象vs_path内的 autoware_msgs::Lane类型变量
prev_waypoints_和new_waypoints_，以及bool类型变量set_path__ .
*/
void VelocitySetPath::waypointsCallback(const autoware_msgs::LaneConstPtr& msg)
{
  prev_waypoints_ = *msg;
  // temporary, edit waypoints velocity later  临时性设置,稍后处理轨迹点速度
  new_waypoints_ = *msg;

  set_path_ = true;
}

//currentVelocityCallback函数更新vs_path 内的double类型变量current_vel_。
void VelocitySetPath::currentVelocityCallback(const geometry_msgs::TwistStampedConstPtr& msg)
{
  current_vel_ = msg->twist.linear.x;
}

4.velocity_set

#include 
#include 
#include 
#include 

#include 
#include 
#include 

namespace
{
constexpr int LOOP_RATE = 10;
constexpr double DECELERATION_SEARCH_DISTANCE = 30;
constexpr double STOP_SEARCH_DISTANCE = 60;

void obstacleColorByKind(const EControl kind, std_msgs::ColorRGBA &color, const double alpha=0.5)
{
  if (kind == EControl::STOP)
  {
    color.r = 1.0; color.g = 0.0; color.b = 0.0; color.a = alpha;  // red
  }
  else if (kind == EControl::STOPLINE)
  {
    color.r = 0.0; color.g = 0.0; color.b = 1.0; color.a = alpha;  // blue
  }
  else if (kind == EControl::DECELERATE)
  {
    color.r = 1.0; color.g = 1.0; color.b = 0.0; color.a = alpha;  // yellow
  }
  else
  {
    color.r = 1.0; color.g = 1.0; color.b = 1.0; color.a = alpha;  // white
  }
}

// Display a detected obstacle
void displayObstacle(const EControl& kind, const ObstaclePoints& obstacle_points, const ros::Publisher& obstacle_pub)
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "/map";
  marker.header.stamp = ros::Time();
  marker.ns = "my_namespace";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;

  static geometry_msgs::Point prev_obstacle_point;
  if (kind == EControl::STOP || kind == EControl::STOPLINE || kind == EControl::DECELERATE)
  {
    marker.pose.position = obstacle_points.getObstaclePoint(kind);
    prev_obstacle_point = marker.pose.position;
  }
  else  // kind == OTHERS
  {
    marker.pose.position = prev_obstacle_point;
  }
  geometry_msgs::Quaternion quat;
  marker.pose.orientation = quat;

  marker.scale.x = 1.0;
  marker.scale.y = 1.0;
  marker.scale.z = 2.0;
  marker.lifetime = ros::Duration(0.1);
  marker.frame_locked = true;
  obstacleColorByKind(kind, marker.color, 0.7);

  obstacle_pub.publish(marker);
}

void displayDetectionRange(const autoware_msgs::Lane& lane, const CrossWalk& crosswalk, const int closest_waypoint,
                           const EControl& kind, const int obstacle_waypoint, const double stop_range,
                           const double deceleration_range, const ros::Publisher& detection_range_pub)
{
  // set up for marker array
  visualization_msgs::MarkerArray marker_array;
  visualization_msgs::Marker crosswalk_marker;
  visualization_msgs::Marker waypoint_marker_stop;
  visualization_msgs::Marker waypoint_marker_decelerate;
  visualization_msgs::Marker stop_line;
  crosswalk_marker.header.frame_id = "/map";
  crosswalk_marker.header.stamp = ros::Time();
  crosswalk_marker.id = 0;
  crosswalk_marker.type = visualization_msgs::Marker::SPHERE_LIST;
  crosswalk_marker.action = visualization_msgs::Marker::ADD;
  waypoint_marker_stop = crosswalk_marker;
  waypoint_marker_decelerate = crosswalk_marker;
  stop_line = crosswalk_marker;
  stop_line.type = visualization_msgs::Marker::CUBE;

  // set each namespace
  crosswalk_marker.ns = "Crosswalk Detection";
  waypoint_marker_stop.ns = "Stop Detection";
  waypoint_marker_decelerate.ns = "Decelerate Detection";
  stop_line.ns = "Stop Line";

  // set scale and color
  double scale = 2 * stop_range;
  waypoint_marker_stop.scale.x = scale;
  waypoint_marker_stop.scale.y = scale;
  waypoint_marker_stop.scale.z = scale;
  waypoint_marker_stop.color.a = 0.2;
  waypoint_marker_stop.color.r = 0.0;
  waypoint_marker_stop.color.g = 1.0;
  waypoint_marker_stop.color.b = 0.0;
  waypoint_marker_stop.frame_locked = true;

  scale = 2 * (stop_range + deceleration_range);
  waypoint_marker_decelerate.scale.x = scale;
  waypoint_marker_decelerate.scale.y = scale;
  waypoint_marker_decelerate.scale.z = scale;
  waypoint_marker_decelerate.color.a = 0.15;
  waypoint_marker_decelerate.color.r = 1.0;
  waypoint_marker_decelerate.color.g = 1.0;
  waypoint_marker_decelerate.color.b = 0.0;
  waypoint_marker_decelerate.frame_locked = true;

  if (obstacle_waypoint > -1)
  {
    stop_line.pose.position = lane.waypoints[obstacle_waypoint].pose.pose.position;
    stop_line.pose.orientation = lane.waypoints[obstacle_waypoint].pose.pose.orientation;
  }
  stop_line.pose.position.z += 1.0;
  stop_line.scale.x = 0.1;
  stop_line.scale.y = 15.0;
  stop_line.scale.z = 2.0;
  stop_line.lifetime = ros::Duration(0.1);
  stop_line.frame_locked = true;
  obstacleColorByKind(kind, stop_line.color, 0.3);

  int crosswalk_id = crosswalk.getDetectionCrossWalkID();
  if (crosswalk_id > 0)
    scale = crosswalk.getDetectionPoints(crosswalk_id).width;
  crosswalk_marker.scale.x = scale;
  crosswalk_marker.scale.y = scale;
  crosswalk_marker.scale.z = scale;
  crosswalk_marker.color.a = 0.5;
  crosswalk_marker.color.r = 0.0;
  crosswalk_marker.color.g = 1.0;
  crosswalk_marker.color.b = 0.0;
  crosswalk_marker.frame_locked = true;

  // set marker points coordinate
  for (int i = 0; i < STOP_SEARCH_DISTANCE; i++)
  {
    if (closest_waypoint < 0 || i + closest_waypoint > static_cast(lane.waypoints.size()) - 1)
      break;

    geometry_msgs::Point point;
    point = lane.waypoints[closest_waypoint + i].pose.pose.position;

    waypoint_marker_stop.points.push_back(point);

    if (i > DECELERATION_SEARCH_DISTANCE)
      continue;
    waypoint_marker_decelerate.points.push_back(point);
  }

  if (crosswalk_id > 0)
  {
    if (!crosswalk.isMultipleDetection())
    {
      for (const auto& p : crosswalk.getDetectionPoints(crosswalk_id).points)
        crosswalk_marker.points.push_back(p);
    }
    else
    {
      for (const auto& c_id : crosswalk.getDetectionCrossWalkIDs())
      {
        for (const auto& p : crosswalk.getDetectionPoints(c_id).points)
        {
          scale = crosswalk.getDetectionPoints(c_id).width;
          crosswalk_marker.points.push_back(p);
        }
      }
    }
  }
  // publish marker
  marker_array.markers.push_back(crosswalk_marker);
  marker_array.markers.push_back(waypoint_marker_stop);
  marker_array.markers.push_back(waypoint_marker_decelerate);
  if (kind != EControl::KEEP)
    marker_array.markers.push_back(stop_line);
  detection_range_pub.publish(marker_array);
  marker_array.markers.clear();
}

// obstacle detection for crosswalk
EControl crossWalkDetection(const pcl::PointCloud& points, const CrossWalk& crosswalk,
                            const geometry_msgs::PoseStamped& localizer_pose, const int points_threshold,
                            ObstaclePoints* obstacle_points)
{
  int crosswalk_id = crosswalk.getDetectionCrossWalkID();
  double search_radius = crosswalk.getDetectionPoints(crosswalk_id).width / 2;
  // std::vector crosswalk_ids crosswalk.getDetectionCrossWalkIDs();

  // Search each calculated points in the crosswalk
  for (const auto& c_id : crosswalk.getDetectionCrossWalkIDs())
  {
    for (const auto& p : crosswalk.getDetectionPoints(c_id).points)
    {
      geometry_msgs::Point detection_point = calcRelativeCoordinate(p, localizer_pose.pose);
      tf::Vector3 detection_vector = point2vector(detection_point);
      detection_vector.setZ(0.0);

      int stop_count = 0;  // the number of points in the detection area
      for (const auto& p : points)
      {
        tf::Vector3 point_vector(p.x, p.y, 0.0);
        double distance = tf::tfDistance(point_vector, detection_vector);
        if (distance < search_radius)
        {
          stop_count++;
          geometry_msgs::Point point_temp;
          point_temp.x = p.x;
          point_temp.y = p.y;
          point_temp.z = p.z;
          obstacle_points->setStopPoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
        }
        if (stop_count > points_threshold)
          return EControl::STOP;
      }
    }

    obstacle_points->clearStopPoints();
    if (!crosswalk.isMultipleDetection())
      break;
  }
  return EControl::KEEP;  // find no obstacles
}

int detectStopObstacle(const pcl::PointCloud& points, const int closest_waypoint,
                       const autoware_msgs::Lane& lane, const CrossWalk& crosswalk, double stop_range,
                       double points_threshold, const geometry_msgs::PoseStamped& localizer_pose,
                       ObstaclePoints* obstacle_points, EObstacleType* obstacle_type,
                       const int wpidx_detection_result_by_other_nodes)
{
  int stop_obstacle_waypoint = -1;
  *obstacle_type = EObstacleType::NONE;
  // start search from the closest waypoint
  for (int i = closest_waypoint; i < closest_waypoint + STOP_SEARCH_DISTANCE; i++)
  {
    // reach the end of waypoints
    if (i >= static_cast(lane.waypoints.size()))
      break;

    // detection another nodes
    if (wpidx_detection_result_by_other_nodes >= 0 &&
        lane.waypoints.at(i).gid == wpidx_detection_result_by_other_nodes)
    {
      stop_obstacle_waypoint = i;
      *obstacle_type = EObstacleType::STOPLINE;
      obstacle_points->setStopPoint(lane.waypoints.at(i).pose.pose.position); // for vizuialization
      break;
    }

    // Detection for cross walk
    if (i == crosswalk.getDetectionWaypoint())
    {
      // found an obstacle in the cross walk
      if (crossWalkDetection(points, crosswalk, localizer_pose, points_threshold, obstacle_points) == EControl::STOP)
      {
        stop_obstacle_waypoint = i;
        *obstacle_type = EObstacleType::ON_CROSSWALK;
        break;
      }
    }

    // waypoint seen by localizer
    geometry_msgs::Point waypoint = calcRelativeCoordinate(lane.waypoints[i].pose.pose.position, localizer_pose.pose);
    tf::Vector3 tf_waypoint = point2vector(waypoint);
    tf_waypoint.setZ(0);

    int stop_point_count = 0;
    for (const auto& p : points)
    {
      tf::Vector3 point_vector(p.x, p.y, 0);

      // 2D distance between waypoint and points (obstacle)
      double dt = tf::tfDistance(point_vector, tf_waypoint);
      if (dt < stop_range)
      {
        stop_point_count++;
        geometry_msgs::Point point_temp;
        point_temp.x = p.x;
        point_temp.y = p.y;
        point_temp.z = p.z;
        obstacle_points->setStopPoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
      }
    }

    // there is an obstacle if the number of points exceeded the threshold
    if (stop_point_count > points_threshold)
    {
      stop_obstacle_waypoint = i;
      *obstacle_type = EObstacleType::ON_WAYPOINTS;
      break;
    }

    obstacle_points->clearStopPoints();

    // check next waypoint...
  }

  return stop_obstacle_waypoint;
}

int detectDecelerateObstacle(const pcl::PointCloud& points, const int closest_waypoint,
                             const autoware_msgs::Lane& lane, const double stop_range, const double deceleration_range,
                             const double points_threshold, const geometry_msgs::PoseStamped& localizer_pose,
                             ObstaclePoints* obstacle_points)
{
  int decelerate_obstacle_waypoint = -1;
  // start search from the closest waypoint
  for (int i = closest_waypoint; i < closest_waypoint + DECELERATION_SEARCH_DISTANCE; i++)
  {
    // reach the end of waypoints
    if (i >= static_cast(lane.waypoints.size()))
      break;

    // waypoint seen by localizer
    geometry_msgs::Point waypoint = calcRelativeCoordinate(lane.waypoints[i].pose.pose.position, localizer_pose.pose);
    tf::Vector3 tf_waypoint = point2vector(waypoint);
    tf_waypoint.setZ(0);

    int decelerate_point_count = 0;
    for (const auto& p : points)
    {
      tf::Vector3 point_vector(p.x, p.y, 0);

      // 2D distance between waypoint and points (obstacle)
      double dt = tf::tfDistance(point_vector, tf_waypoint);
      if (dt > stop_range && dt < stop_range + deceleration_range)
      {
        decelerate_point_count++;
        geometry_msgs::Point point_temp;
        point_temp.x = p.x;
        point_temp.y = p.y;
        point_temp.z = p.z;
        obstacle_points->setDeceleratePoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
      }
    }

    // there is an obstacle if the number of points exceeded the threshold
    if (decelerate_point_count > points_threshold)
    {
      decelerate_obstacle_waypoint = i;
      break;
    }

    obstacle_points->clearDeceleratePoints();

    // check next waypoint...
  }

  return decelerate_obstacle_waypoint;
}

// Detect an obstacle by using pointcloud
EControl pointsDetection(const pcl::PointCloud& points, const int closest_waypoint,
                         const autoware_msgs::Lane& lane, const CrossWalk& crosswalk, const VelocitySetInfo& vs_info,
                         int* obstacle_waypoint, ObstaclePoints* obstacle_points)
{
  // no input for detection || no closest waypoint
  if ((points.empty() == true && vs_info.getDetectionResultByOtherNodes() == -1) || closest_waypoint < 0)
    return EControl::KEEP;

  EObstacleType obstacle_type = EObstacleType::NONE;
  int stop_obstacle_waypoint =
      detectStopObstacle(points, closest_waypoint, lane, crosswalk, vs_info.getStopRange(),
                         vs_info.getPointsThreshold(), vs_info.getLocalizerPose(),
                         obstacle_points, &obstacle_type, vs_info.getDetectionResultByOtherNodes());

  // skip searching deceleration range
  if (vs_info.getDecelerationRange() < 0.01)
  {
    *obstacle_waypoint = stop_obstacle_waypoint;
    if (stop_obstacle_waypoint < 0)
      return EControl::KEEP;
    else if (obstacle_type == EObstacleType::ON_WAYPOINTS || obstacle_type == EObstacleType::ON_CROSSWALK)
      return EControl::STOP;
    else if (obstacle_type == EObstacleType::STOPLINE)
      return EControl::STOPLINE;
    else
      return EControl::OTHERS;
  }

  int decelerate_obstacle_waypoint =
      detectDecelerateObstacle(points, closest_waypoint, lane, vs_info.getStopRange(), vs_info.getDecelerationRange(),
                               vs_info.getPointsThreshold(), vs_info.getLocalizerPose(), obstacle_points);

  // stop obstacle was not found
  if (stop_obstacle_waypoint < 0)
  {
    *obstacle_waypoint = decelerate_obstacle_waypoint;
    return decelerate_obstacle_waypoint < 0 ? EControl::KEEP : EControl::DECELERATE;
  }

  // stop obstacle was found but decelerate obstacle was not found
  if (decelerate_obstacle_waypoint < 0)
  {
    *obstacle_waypoint = stop_obstacle_waypoint;
    return EControl::STOP;
  }

  // about 5.0 meter
  double waypoint_interval =
      getPlaneDistance(lane.waypoints[0].pose.pose.position, lane.waypoints[1].pose.pose.position);
  int stop_decelerate_threshold = 5 / waypoint_interval;

  // both were found
  if (stop_obstacle_waypoint - decelerate_obstacle_waypoint > stop_decelerate_threshold)
  {
    *obstacle_waypoint = decelerate_obstacle_waypoint;
    return EControl::DECELERATE;
  }
  else
  {
    *obstacle_waypoint = stop_obstacle_waypoint;
    return EControl::STOP;
  }
}

//障碍物检测并可视化障碍物。
EControl obstacleDetection(int closest_waypoint, const autoware_msgs::Lane& lane, const CrossWalk& crosswalk,
                           const VelocitySetInfo vs_info, const ros::Publisher& detection_range_pub,
                           const ros::Publisher& obstacle_pub, int* obstacle_waypoint)
{
  ObstaclePoints obstacle_points;
  EControl detection_result = pointsDetection(vs_info.getPoints(), closest_waypoint, lane, crosswalk, vs_info,
                                              obstacle_waypoint, &obstacle_points);
  displayDetectionRange(lane, crosswalk, closest_waypoint, detection_result, *obstacle_waypoint, vs_info.getStopRange(),
                        vs_info.getDecelerationRange(), detection_range_pub);

  static int false_count = 0;
  static EControl prev_detection = EControl::KEEP;
  static int prev_obstacle_waypoint = -1;

  // stop or decelerate because we found obstacles
  if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE || detection_result == EControl::DECELERATE)
  {
    displayObstacle(detection_result, obstacle_points, obstacle_pub);
    prev_detection = detection_result;
    false_count = 0;
    prev_obstacle_waypoint = *obstacle_waypoint;
    return detection_result;
  }

  // there are no obstacles, but wait a little for safety
  if (prev_detection == EControl::STOP || prev_detection == EControl::STOPLINE || prev_detection == EControl::DECELERATE)
  {
    false_count++;

    if (false_count < LOOP_RATE / 2)
    {
      *obstacle_waypoint = prev_obstacle_waypoint;
      displayObstacle(EControl::OTHERS, obstacle_points, obstacle_pub);
      return prev_detection;
    }
  }

  // there are no obstacles, so we move forward
  *obstacle_waypoint = -1;
  false_count = 0;
  prev_detection = EControl::KEEP;
  return detection_result;
}


/*
根据前方障碍物/停车线情况，更新vs_path 内的new_waypoints_中的轨迹点速度，
并从 new_waypoints_中截取部分轨迹点发布到话题"final_waypoints"。
new_waypoints_在前面VelocitySetPath类中的 waypointsCallback 函数中被赋值，
且跟prev_waypoints_一致。
*/
void changeWaypoints(const VelocitySetInfo& vs_info, const EControl& detection_result, int closest_waypoint,
                     int obstacle_waypoint, const ros::Publisher& final_waypoints_pub, VelocitySetPath* vs_path)
{
  if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE)
  {  // STOP for obstacle/stopline  在障碍物/停止线前停车
    // stop_waypoint is about stop_distance meter away from obstacles/stoplines
    //stop_waypoint大约距离障碍物/停车线stop_distance米远
    int stop_distance = (detection_result == EControl::STOP)
      ? vs_info.getStopDistanceObstacle() : vs_info.getStopDistanceStopline();
    double deceleration = (detection_result == EControl::STOP)
      ? vs_info.getDecelerationObstacle() : vs_info.getDecelerationStopline();

      //确定与obstacle _waypoint 的距离为stop_distance相对应轨迹点下标
    int stop_waypoint =
    //calcWaypointIndexReverse函数确定与begin_waypoint的距离为distance相对应的轨迹点下标。
        calcWaypointIndexReverse(vs_path->getPrevWaypoints(), obstacle_waypoint, stop_distance);
    
    // change waypoints to stop by the stop_waypoint 基于stop waypoint更改轨迹点速度实现停车
    //changeWaypointsForStopping函数:在到达停车点前以恒定加速度减速，并以此更新new_waypoints_中轨迹点的速度。
    vs_path->changeWaypointsForStopping(stop_waypoint, obstacle_waypoint, closest_waypoint, deceleration);

    //avoidSuddenAcceleration函数:保证加速过程中的加速度恒定，据此更新轨迹点速度。
    vs_path->avoidSuddenAcceleration(deceleration, closest_waypoint);

    //avoidSuddenDeceleration函数:保证减速过程中的加速度恒定，据此更新轨迹点速度。
    vs_path->avoidSuddenDeceleration(vs_info.getVelocityChangeLimit(), deceleration, closest_waypoint);

    //setTemporalWaypoints函数:从最近的轨迹点开始从new_waypoints_中抽取大约“_temporal_waypoints_size”个轨迹点到 temporal_waypoints_.
    vs_path->setTemporalWaypoints(vs_info.getTemporalWaypointsSize(), closest_waypoint, vs_info.getControlPose());
    final_waypoints_pub.publish(vs_path->getTemporalWaypoints());
  }
  else if (detection_result == EControl::DECELERATE)
  {  // DECELERATE for obstacles 在障碍物前减速
    //initializeNewWaypoints函数:重置new_waypoints_。
    vs_path->initializeNewWaypoints();

    //changeWaypointsForDeceleration函数:作用和实现流程与changeWaypoints-ForStopping函数类似，只是并非减速至0，而是减速至某一速度。
    vs_path->changeWaypointsForDeceleration(vs_info.getDecelerationObstacle(), closest_waypoint, obstacle_waypoint);
    vs_path->avoidSuddenDeceleration(vs_info.getVelocityChangeLimit(), vs_info.getDecelerationObstacle(), closest_waypoint);
    vs_path->avoidSuddenAcceleration(vs_info.getDecelerationObstacle(), closest_waypoint);
    vs_path->setTemporalWaypoints(vs_info.getTemporalWaypointsSize(), closest_waypoint, vs_info.getControlPose());
   
   //getTemporalWaypoints函数:返回vs_path内的 temporal_waypoints_.
    final_waypoints_pub.publish(vs_path->getTemporalWaypoints());
  }
  else
  {  // ACCELERATE or KEEP 加速或保持
    vs_path->initializeNewWaypoints();
    vs_path->avoidSuddenAcceleration(vs_info.getDecelerationObstacle(), closest_waypoint);
    vs_path->avoidSuddenDeceleration(vs_info.getVelocityChangeLimit(), vs_info.getDecelerationObstacle(), closest_waypoint);
    vs_path->setTemporalWaypoints(vs_info.getTemporalWaypointsSize(), closest_waypoint, vs_info.getControlPose());
    final_waypoints_pub.publish(vs_path->getTemporalWaypoints());
  }
}

}  // end namespace

//主函数
int main(int argc, char** argv)
{
  ros::init(argc, argv, "velocity_set");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  bool use_crosswalk_detection;
  bool enable_multiple_crosswalk_detection;

  std::string points_topic;
  private_nh.param("use_crosswalk_detection", use_crosswalk_detection, true);
  private_nh.param("enable_multiple_crosswalk_detection", enable_multiple_crosswalk_detection, true);


  private_nh.param("points_topic", points_topic, "points_lanes");

  // class 类对象
  CrossWalk crosswalk;
  VelocitySetPath vs_path;
  VelocitySetInfo vs_info;

  // velocity set subscriber “速度设定”订阅
  ros::Subscriber waypoints_sub = nh.subscribe("safety_waypoints", 1, &VelocitySetPath::waypointsCallback, &vs_path);
  ros::Subscriber current_vel_sub =
      nh.subscribe("current_velocity", 1, &VelocitySetPath::currentVelocityCallback, &vs_path);



  // velocity set info subscriber “速度设定信息”订阅
  ros::Subscriber config_sub = nh.subscribe("config/velocity_set", 1, &VelocitySetInfo::configCallback, &vs_info);
  ros::Subscriber points_sub = nh.subscribe(points_topic, 1, &VelocitySetInfo::pointsCallback, &vs_info);
  ros::Subscriber localizer_sub = nh.subscribe("localizer_pose", 1, &VelocitySetInfo::localizerPoseCallback, &vs_info);
  ros::Subscriber control_pose_sub = nh.subscribe("current_pose", 1, &VelocitySetInfo::controlPoseCallback, &vs_info);
  ros::Subscriber detectionresult_sub = nh.subscribe("/state/stopline_wpidx", 1, &VelocitySetInfo::detectionCallback, &vs_info);

  // vector map subscriber “矢量地图”订阅
  ros::Subscriber sub_dtlane = nh.subscribe("vector_map_info/cross_walk", 1, &CrossWalk::crossWalkCallback, &crosswalk);
  ros::Subscriber sub_area = nh.subscribe("vector_map_info/area", 1, &CrossWalk::areaCallback, &crosswalk);
  ros::Subscriber sub_line = nh.subscribe("vector_map_info/line", 1, &CrossWalk::lineCallback, &crosswalk);
  ros::Subscriber sub_point = nh.subscribe("vector_map_info/point", 1, &CrossWalk::pointCallback, &crosswalk);

  // publisher  发布器
  ros::Publisher detection_range_pub = nh.advertise("detection_range", 1);
  ros::Publisher obstacle_pub = nh.advertise("obstacle", 1);
  ros::Publisher obstacle_waypoint_pub = nh.advertise("obstacle_waypoint", 1, true);
  ros::Publisher stopline_waypoint_pub = nh.advertise("stopline_waypoint", 1, true);

  ros::Publisher final_waypoints_pub;
  final_waypoints_pub = nh.advertise("final_waypoints", 1, true);

  ros::Rate loop_rate(LOOP_RATE);
  while (ros::ok())
  {
    ros::spinOnce();

    int closest_waypoint = 0;

    if (crosswalk.loaded_all && !crosswalk.set_points)

    ///更新crosswalk内的 bdID_, detection points_和 set _points
      crosswalk.setCrossWalkPoints();

    if (!vs_info.getSetPose() || !vs_path.getSetPath() || vs_path.getPrevWaypointsSize() == 0)
    {
      loop_rate.sleep();
      continue;
    }


    //设置crosswalk内的变量enable_multiple_crosswalk_detection_
    crosswalk.setMultipleDetectionFlag(enable_multiple_crosswalk_detection);

    if (use_crosswalk_detection)

    /*
     setDetectionwaypoint设置crosswalk中变量detection_waypoint_,
     而传入函数的是findclosestCrosswalk函数获得的无人车处于
     当前位置时在搜索范围内最靠近人行横道的轨迹点下标
    */
      crosswalk.setDetectionWaypoint(
          crosswalk.findClosestCrosswalk(closest_waypoint, vs_path.getPrevWaypoints(), STOP_SEARCH_DISTANCE));

    int obstacle_waypoint = -1;

    //障碍物检测并可视化障碍物
    EControl detection_result = obstacleDetection(closest_waypoint, vs_path.getPrevWaypoints(), crosswalk, vs_info,
                                                  detection_range_pub, obstacle_pub, &obstacle_waypoint);

/*
  根据前方障碍物/停车线情况，
  更新vs path内的new_waypoints_中的轨迹点速度，
  并从new_waypoints_中截取部分轨迹点发布到话题"final_waypoints"
*/
    changeWaypoints(vs_info, detection_result, closest_waypoint,
                    obstacle_waypoint, final_waypoints_pub, &vs_path);


    //清空vs_info中的points
    vs_info.clearPoints();

    // publish obstacle waypoint index  发布obstacle_waypoint_index和 stopline_waypoint_index
    std_msgs::Int32 obstacle_waypoint_index;
    std_msgs::Int32 stopline_waypoint_index;
    if (detection_result == EControl::STOP)
    {
      obstacle_waypoint_index.data = obstacle_waypoint;
      stopline_waypoint_index.data = -1;
    }
    else if (detection_result == EControl::STOPLINE)
    {
      obstacle_waypoint_index.data = -1;
      stopline_waypoint_index.data = obstacle_waypoint;
    }
    else
    {
      obstacle_waypoint_index.data = -1;
      stopline_waypoint_index.data = -1;
    }
    obstacle_waypoint_pub.publish(obstacle_waypoint_index);
    stopline_waypoint_pub.publish(stopline_waypoint_index);

    //重置vs_path 内的set_path_
    vs_path.resetFlag();

    loop_rate.sleep();
  }

  return 0;
}

参考书目《Autoware与自动驾驶技术》

注：可以下载Visual Studio Coded代码编辑软件，展示性好点。