（七）学习笔记autoware源码core_planning（lane_rule)

#ifdef DEBUG
#include <sstream>
#endif  // DEBUG

#include <ros/console.h>

#include <vector_map/vector_map.h>
#include "autoware_config_msgs/ConfigLaneRule.h"
#include "autoware_msgs/LaneArray.h"

#include <lane_planner/lane_planner_vmap.hpp>

namespace
{
double config_acceleration = 1;            // m/s^2
double config_stopline_search_radius = 1;  // meter
int config_number_of_zeros_ahead = 0;
int config_number_of_zeros_behind = 0;
int config_number_of_smoothing_count = 0;

int waypoint_max;
double search_radius;  // meter
double curve_weight;
double crossroad_weight;
double clothoid_weight;
std::string frame_id;

ros::Publisher traffic_pub;
ros::Publisher red_pub;
ros::Publisher green_pub;

lane_planner::vmap::VectorMap all_vmap;
lane_planner::vmap::VectorMap lane_vmap;
double curve_radius_min;
double crossroad_radius_min;
double clothoid_radius_min;
autoware_msgs::LaneArray cached_waypoint;

#ifdef DEBUG
visualization_msgs::Marker debug_marker;
ros::Publisher marker_pub;
int marker_cnt;
#endif  // DEBUG

autoware_msgs::Lane create_new_lane(const autoware_msgs::Lane& lane, const std_msgs::Header& header)
{
  autoware_msgs::Lane l = lane;
  l.header = header;

  for (autoware_msgs::Waypoint& w : l.waypoints)
  {
    w.pose.header = header;
    w.twist.header = header;
  }

  return l;
}


/*
修正 lane中轨迹点的速度，首先固定lane 内下标在[start_index，start_index+fixed_cnt)范围内的轨迹点速度都为
fixed_vel，随后根据运动学公式v = sqrt(square_vel + 2 * acceleration * distance)(加速情况)修正后面轨迹点的速度。
*/
autoware_msgs::Lane apply_acceleration(const autoware_msgs::Lane& lane, double acceleration, size_t start_index,
                                       size_t fixed_cnt, double fixed_vel)
{
  autoware_msgs::Lane l = lane;

  if (fixed_cnt == 0)
    return l;

  double square_vel = fixed_vel * fixed_vel;
  double distance = 0;

  //从下标为start_index的轨迹点开始遍历l.waypoints
  for (size_t i = start_index; i < l.waypoints.size(); ++i)
  {
    if (i - start_index < fixed_cnt)
    {
      //令下标范围在[start_index,start_index+fixed_cnt范围内的轨迹点速度都为fixed_vel
      l.waypoints[i].twist.twist.linear.x = fixed_vel;
      continue;
    }

//以下代码对下标在[start_index+fixed_cnt，l.waypoints.size()]范围内的轨迹点实施
    geometry_msgs::Point a = l.waypoints[i - 1].pose.pose.position;
    geometry_msgs::Point b = l.waypoints[i].pose.pose.position;
    distance += hypot(b.x - a.x, b.y - a.y);

    const int sgn = (l.waypoints[i].twist.twist.linear.x < 0.0) ? -1 : 1;
    double v = sqrt(square_vel + 2 * acceleration * distance);
    if (v < l.waypoints[i].twist.twist.linear.x)
    {
      l.waypoints[i].twist.twist.linear.x = sgn * v;
    }
    else
    {
      //此函数适用于加速情况，而且默认储存的轨迹点速度为匀速,因此只要v大于某一轨迹点速度,后面的便不用再修正
      break;
    }
  }

  return l;
}

/*
apply_crossroad_acceleration函数的作用与实现流程大致与前面的apply_stopline_acceleration函数一样,
只不过这里速度修正的中心点由前面的停止点换成了交叉口的起始点或末尾点。
*/
autoware_msgs::Lane apply_crossroad_acceleration(const autoware_msgs::Lane& lane, double acceleration)
{
  autoware_msgs::Lane l = lane;

  bool crossroad = false;
  std::vector<size_t> start_indexes;
  std::vector<size_t> end_indexes;
  for (size_t i = 0; i < l.waypoints.size(); ++i)
  {
    vector_map::DTLane dtlane = lane_planner::vmap::create_vector_map_dtlane(l.waypoints[i].dtlane);
    if (i == 0)
    {
      crossroad = lane_planner::vmap::is_crossroad_dtlane(dtlane);
      continue;
    }
    if (crossroad)
    {
      if (!lane_planner::vmap::is_crossroad_dtlane(dtlane))
      {
        end_indexes.push_back(i - 1);
        crossroad = false;
      }
    }
    else
    {
      if (lane_planner::vmap::is_crossroad_dtlane(dtlane))
      {
        start_indexes.push_back(i);
        crossroad = true;
      }
    }
  }
  if (start_indexes.empty() && end_indexes.empty())
    return l;

  for (const size_t i : end_indexes)
    l = apply_acceleration(l, acceleration, i, 1, l.waypoints[i].twist.twist.linear.x);

  std::reverse(l.waypoints.begin(), l.waypoints.end());

  std::vector<size_t> reverse_start_indexes;
  for (const size_t i : start_indexes)
    reverse_start_indexes.push_back(l.waypoints.size() - i - 1);
  std::reverse(reverse_start_indexes.begin(), reverse_start_indexes.end());

  for (const size_t i : reverse_start_indexes)
    l = apply_acceleration(l, acceleration, i, 1, l.waypoints[i].twist.twist.linear.x);

  std::reverse(l.waypoints.begin(), l.waypoints.end());

  return l;
}


/*
apply_stopline_acceleration函数与前面 
apply_stopline_acceleration(const autoware_msgs::Lanc&lane，double acceleration,double stopline_search_radius，size_t ahead_cnt,size _tbehind_cnt)
函数的作用是一样的:对车速进行修正，以停车点为中心修正附近轨迹点车速为0，接着运用 匀减速/匀加速 运动学方程修正
匀减速接近/匀加速离开 停车点过程中的车速。但是中间流程相较于前一个同名函数更简单些，其可以直接查询行车路径上的停止点，
而不必去全局矢量地图lane_vmap中遍历搜索。
*/
autoware_msgs::Lane apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration,