Relative_map模块

1.relative_map模块功能

相对地图模块是在车身坐标系—前-左-天坐标（The Vehicle Frame —Front-Left-Up，FLU）下，相对地图模块是连接HDMap / Perception模块和计划模块的中间层。 该模块在身体坐标系（FLU）中生成实时相对地图和用于规划的参考线。 相对地图模块的输入包含两个部分：离线和在线。 离线部分是导航线（人类驾驶路径）以及导航线及其附近的HDMap信息。 在线部分是来自感知模块的交通标志相关信息，例如车道标记，人行横道，交通信号灯等。相关地图的生成可以利用在线和离线部分。 它也仅适用于在线或离线部分。

输入
*来自Dreamview模块的NavigationInfo
*来自preciption模块的LaneMarker
*来自localization模块的localization
*来自canbus模块的chassis

输出
*相对地图遵循在modules/map/proto/map.proto中定义的地图格式

相对地图有如下两个特点：

1. 首先，产生的地图数据是相对于车的车身坐标系，这也是为什么该模块被命名为相对地图；在车身坐标系下，车辆坐标永远在原点，车头方向永远为0。
2. 其次，地图数据会根据车的位置和朝向的变化实时更新，更新频率是10Hz。

相对地图基于navigation path录制模式，依赖于提前录制的navigation path与实时摄像头信息，同时依靠GPS定位。在基于纯摄像头模式时，并不进行自身定位和建造增量式地图，只依赖实时摄像头生成的车道线信息行驶。

2. relative_map的数据接口

相对地图模块接收导航，感知，定位，底盘的数据消息，生成相对地图发布出去给planning使用，MapMsg消息是对外发布的消息，其消息格式如下：

MapMsg地图消息格式。

变量名	说明
header	消息的头
hdmap apollo.hdmap.Map	modules / map / proto / map.proto中定义的地图格式
navigation_path	map<string, NavigationPath> key: type string; the lane id in Map value: Navigation path; the reference line of the lane
lane_marker	lane marker info from perception
localization	定位

NavigationInfo是接收的导航消息（这里是给定的离线路径信息？有待确认，但是在代码的测试代码中读取类型看感觉像离线路径）
NavigationInfo消息格式。

变量名	说明
header	消息的头
navigation_path	NavigationPath

NavigationPath消息。

变量名	说明
header	消息的头
navigation_path

path消息格式。

变量名	说明
name	路径名
path_point	路径点

path消息格式。

变量名	说明
x	坐标x
y	坐标y
z	坐标z
theta	航向角
kappa	曲率
s
dkappa	曲率的导数
ddkappa	曲率的二阶导数
lane_id	车道id
x_derivative	X的导数
y_derivative	Y的导数

3. relative_map模块框架

相对地图的生成及调用过程如下：

从流程中可知初始化及开始的过程中主要是对相对地图中的参数进行获取并初始化，然后相对地图以周期为10hz不断更新接收导航线信息、感知信息、定位信息、底盘信息生成相对地图，最后将生成的地图信息发布出去，其相对地图的数据格式见上面表格。
在planning的参考线生成模块中，在导航模式下，接收到相对地图后，得到车辆当前的车道信息，获取优先级高于当前通道的优先级通道信息列表，并获得最高优先级的通道作为目标通道，获取当前车道与目标车道最近的邻居车道，并确保其位置在当前车道的左侧或右侧。遍历相对地图中导航路径的每一条，并添加路径点及相关属性，生成参考线。
相对地图的生成过程如下：
1.首先获取导航模式下的配置参数。
获取配置参数主要是在初始化函数中读取配置文件的各参数值，其过程如下：

Status RelativeMap::Init() {
……
  config_.Clear();
  if (!cyber::common::GetProtoFromFile(FLAGS_relative_map_config_filename,
                                       &config_)) {
    return Status(ErrorCode::RELATIVE_MAP_ERROR,
                  "Unable to load relative map conf file: " +
                      FLAGS_relative_map_config_filename);
  }
  //设置导航车道参数
  navigation_lane_.SetConfig(config_.navigation_lane());
  const auto& map_param = config_.map_param();
  //设置地图的参数
  navigation_lane_.SetDefaultWidth(map_param.default_left_width(),
                                   map_param.default_right_width());

  return Status::OK();
}

其中参数主要包括地图参数和车道参数，其详细信息如下

//地图参数，左右宽度1.875,限速65mph
map_param {
  default_left_width: 1.875
  default_right_width: 1.875
  default_speed_limit : 29.06 # 65 mph
}
//导航车道
navigation_lane {
  min_lane_marker_quality: 0.49
  lane_source: OFFLINE_GENERATED  //车道来源，离线生成
  max_len_from_navigation_line: 250.0 //最大的导航线的长度 250m
  min_len_for_navigation_lane: 150.0   //最小导航车道长度150m 
  max_len_for_navigation_lane: 250.0  //导航车道最大长度250m
  ratio_navigation_lane_len_to_speed: 8.0  //8s
  max_distance_to_navigation_line: 15.0  //到导航线的最大距离15m
  min_view_range_to_use_lane_marker: 0.5  //使用车道线最小视角范围0.5m
  min_lane_half_width: 1.5  //最小车道一半宽度1.5m
  max_lane_half_width: 2.0   //最大车道一半宽度 2m
  lane_marker_weight: 0.1   //车道线权重0.1
}

2.相对地图以10hz的频率运行，独立的进程来实时创建导航地图

//开启状态的进程
bool RelativeMap::Process(MapMsg* const map_msg) {
  {
    //互斥锁
    std::lock_guard<std::mutex> lock(navigation_lane_mutex_);
    //从导航的车道中创建地图
    CreateMapFromNavigationLane(map_msg);
  }
  return true;
}

3.创建导航地图
创建导航地图一共分三步，1）首先更新导航、感知、定位、底盘的状态信息，2）然后通过离线地图或者感知车道线生成路径，3）最后将导航信息更新到相对地图的信息中。其中生成路径也是最核心的部分，其详细过程如下：

3.1更新状态信息

//从导航车道中创建地图
bool RelativeMap::CreateMapFromNavigationLane(MapMsg* map_msg) {
  CHECK_NOTNULL(map_msg);
  // update vehicle state from localization and chassis
  //通过定位与地盘更新车辆状态
  LocalizationEstimate const& localization = localization_;
  Chassis const& chassis = chassis_;
  VehicleStateProvider::Instance()->Update(localization, chassis);
  //在map中添加定位数据
  map_msg->mutable_localization()->CopyFrom(localization_);

  // update navigation_lane from perception_obstacles (lane marker)
  //通过感知障碍物中（车道线）更新导航车道
  PerceptionObstacles const& perception = perception_obstacles_;
  navigation_lane_.UpdatePerception(perception);
  //然后把感知障碍物车道线加入到map中
  map_msg->mutable_lane_marker()->CopyFrom(perception_obstacles_.lane_marker());
  ………………
}

3.2生成路径

//从导航车道中创建地图
bool RelativeMap::CreateMapFromNavigationLane(MapMsg* map_msg) {
  ………………
  //生成路径
  if (!navigation_lane_.GeneratePath()) {
    LogErrorStatus(map_msg, "Failed to generate a navigation path.");
    return false;
  }  
……
}

生成导航路径主要包括两种方式：第一中是离线生成，另外一种是通过感知的车道线实时生成。通过感知的车道线生成路径方式如下：

其详细过程如下

 //构建导航路径的四元组
  current_navi_path_tuple_ = std::make_tuple(-1, -1.0, -1.0, nullptr);
  // original_pose is in world coordination: ENU
  //东北天坐标系下的车辆姿态
  original_pose_ = VehicleStateProvider::Instance()->original_pose();
  //导航线的数量
  int navigation_line_num = navigation_info_.navigation_path_size();
  //得到感知车道线
  const auto &lane_marker = perception_obstacles_.lane_marker();
  //生成路径在感知功能下
  auto generate_path_on_perception_func = [this, &lane_marker]() {
    //创建当前的路径
    auto current_navi_path = std::make_shared<NavigationPath>();
    auto *path = current_navi_path->mutable_path();
    //将lanemarker转换成path
    ConvertLaneMarkerToPath(lane_marker, path);
    //设置优先级，0表示直接到达的
    current_navi_path->set_path_priority(0);
    //得到左右的宽度
    double left_width = perceived_left_width_ > 0.0 ? perceived_left_width_
                                                    : default_left_width_;
    double right_width = perceived_right_width_ > 0.0 ? perceived_right_width_
                                                      : default_right_width_;
    //构建当前导航路径的四元组
    current_navi_path_tuple_ =
        std::make_tuple(0, left_width, right_width, current_navi_path);
  };

其中关键的地方就是将车道线转换成路径，其过程主要分为这样几步：

具体过程如下：

//转换车道线成路径
void NavigationLane::ConvertLaneMarkerToPath(
    const perception::LaneMarkers &lane_marker, common::Path *const path) {
  CHECK_NOTNULL(path);

  //设置路径名为车道线生成路径
  path->set_name("Path from lane markers.");
  //得到左边车道线和右边车道线
  const auto &left_lane = lane_marker.left_lane_marker();
  const auto &right_lane = lane_marker.right_lane_marker();
  //左边车道线生成的c0为左右两边
  double path_c0 = (left_lane.c0_position() + right_lane.c0_position()) / 2.0;
  //左边的质量等于左边车道线的质量+0.001,右边车道的质量为右边车道线的质量+0.001
  double left_quality = left_lane.quality() + 0.001;
  double right_quality = right_lane.quality() + 0.001;
  //权重划分
  double quality_divider = left_quality + right_quality;

  //得到系数
  double path_c1 = (left_lane.c1_heading_angle() * left_quality +
                    right_lane.c1_heading_angle() * right_quality) /
                   quality_divider;

  double path_c2 = (left_lane.c2_curvature() * left_quality +
                    right_lane.c2_curvature() * right_quality) /
                   quality_divider;

  double path_c3 = (left_lane.c3_curvature_derivative() * left_quality +
                    right_lane.c3_curvature_derivative() * right_quality) /
                   quality_divider;
  //车辆当前的线速度
  const double current_speed =
      VehicleStateProvider::Instance()->vehicle_state().linear_velocity();
  //路径范围，8秒范围
  double path_range =
      current_speed * config_.ratio_navigation_lane_len_to_speed();
  if (path_range <= config_.min_len_for_navigation_lane()) {
    path_range = config_.min_len_for_navigation_lane();
  } else {
    path_range = config_.max_len_for_navigation_lane();
  }
  //单位长度为1m，start_s = -2
  const double unit_z = 1.0;
  const double start_s = -2.0;
  double accumulated_s = start_s;
  for (double z = start_s; z <= path_range; z += unit_z) {
    double x1 = z;
    double y1 = 0;
    //如果左右车道线有一个大于0.5m，则进行拟合
    if (left_lane.view_range() > config_.min_view_range_to_use_lane_marker() ||
        right_lane.view_range() > config_.min_view_range_to_use_lane_marker()) {
      y1 = EvaluateCubicPolynomial(path_c0, path_c1, path_c2, path_c3, z);
    }
    auto *point = path->add_path_point();
    point->set_x(x1);
    point->set_y(y1);
    //点大于1时，进行采样
    if (path->path_point_size() > 1) {
      auto &pre_point = path->path_point(path->path_point_size() - 2);
      accumulated_s += std::hypot(x1 - pre_point.x(), y1 - pre_point.y());
    }
    point->set_s(accumulated_s);
    point->set_theta(
        std::atan2(3 * path_c3 * x1 * x1 + 2 * path_c2 * x1 + path_c1, 1));
    point->set_kappa(GetKappa(path_c1, path_c2, path_c3, x1));

    const double k1 = GetKappa(path_c1, path_c2, path_c3, x1 - 0.0001);
    const double k2 = GetKappa(path_c1, path_c2, path_c3, x1 + 0.0001);
    point->set_dkappa((k2 - k1) / 0.0002);
  }
  //得到车道宽度
  perceived_left_width_ = std::fabs(left_lane.c0_position());
  perceived_right_width_ = std::fabs(right_lane.c0_position());
  // If the perceived lane width is incorrect, use the default lane width
  // directly.
  double perceived_lane_width = perceived_left_width_ + perceived_right_width_;
  if (perceived_lane_width < 2.0 * config_.min_lane_half_width() ||
      perceived_lane_width > 2.0 * config_.max_lane_half_width()) {
    perceived_left_width_ = default_left_width_;
    perceived_right_width_ = default_right_width_;
  }
}

然后判断是否有离线生成的导航线，如果有进行合并，如果没有就考虑通过感知的数据实时生成。
离线生成导航线的方法为：

//config_.lane_source ：1表示感知，2表示离线生成
  if (config_.lane_source() == NavigationLaneConfig::OFFLINE_GENERATED &&
      navigation_line_num > 0) {
    // Generate multiple navigation paths based on navigation lines.
    // Don't worry about efficiency because the total number of navigation lines
    // will not exceed 10 at most.
    //基于导航线生成多个导航路径。不用担心效率，因为总航路不会超过10条。
    for (int i = 0; i < navigation_line_num; ++i) {
      auto current_navi_path = std::make_shared<NavigationPath>();
      auto *path = current_navi_path->mutable_path();
      //转换导航线到path
      if (ConvertNavigationLineToPath(i, path)) {
        current_navi_path->set_path_priority(
            navigation_info_.navigation_path(i).path_priority());
        navigation_path_list_.emplace_back(
            i, default_left_width_, default_right_width_, current_navi_path);
      }
    }
    //如果没有基于导航线生成的导航路径，则根据感知的车道标记生成车辆所在位置的导航路径。
    if (navigation_path_list_.empty()) {
      generate_path_on_perception_func();
      return true;
    }
    //根据车辆的方向将导航路径从左到右排序。在FLU车辆坐标系统中，车辆左侧y坐标为正，
    //右侧y坐标为负。因此，可以根据其y坐标从左到右对导航路径进行排序。
    navigation_path_list_.sort(
        [](const NaviPathTuple &left, const NaviPathTuple &right) {
          double left_y = std::get<3>(left)->path().path_point(0).y();
          double right_y =变量名 std::get<3>(right)->path().path_point(0).y();
          return left_y > right_y;
        });
    //获取车辆当前所在的导航路径。
    double min_d = std::numeric_limits<double>::max();
    for (const auto &navi_path_tuple : navigation_path_list_) {
      int current_line_index = std::get<0>(navi_path_tuple);
      ADEBUG << "Current navigation path index is: " << current_line_index;
      double current_d = std::numeric_limits<double>::max();
      auto item_iter = last_project_index_map_.find(current_line_index);
      if (item_iter != last_project_index_map_.end()) {
        current_d = item_iter->second.second;
      }
      if (current_d < min_d) {
        min_d = current_d;
        current_navi_path_tuple_ = navi_path_tuple;
      }
    }
    //将车辆所在的当前导航路径与感知的车道标记合并。
    auto *path = std::get<3>(current_navi_path_tuple_)->mutable_path();
    MergeNavigationLineAndLaneMarker(std::get<0>(current_navi_path_tuple_),
                                     path);

    //设置车辆当前所在的导航路径的宽度。
    double left_width = perceived_left_width_ > 0.0 ? perceived_left_width_
                                                    : default_left_width_;
    double right_width = perceived_right_width_ > 0.0 ? perceived_right_width_
                                                      : default_right_width_;
    if (!IsFloatEqual(left_width, default_left_width_) &&
        !IsFloatEqual(right_width, default_right_width_)) {
      left_width = left_width > default_left_width_ ? left_width - min_d
                                                    : left_width + min_d;
      right_width = right_width > default_right_width_ ? right_width - min_d
                                                       : right_width + min_d;
    }

    ADEBUG << "The left width of current lane is: " << left_width
           << " and the right width of current lane is: " << right_width;

    std::get<1>(current_navi_path_tuple_) = left_width;
    std::get<2>(current_navi_path_tuple_) = right_width;
    auto curr_navi_path_iter = std::find_if(
        std::begin(navigation_path_list_), std::end(navigation_path_list_),
        [this](const NaviPathTuple &item) {
          return std::get<0>(item) == std::get<0>(current_navi_path_tuple_);
        });
    if (curr_navi_path_iter != std::end(navigation_path_list_)) {
      std::get<1>(*curr_navi_path_iter) = left_width;
      std::get<2>(*curr_navi_path_iter) = right_width;
    }

  //设置每个导航路径与其相邻路径之间的宽度。使用多点平均的原因是为了防止来自奇异点的过多干扰。
    //如果当前导航路径是车辆当前所在的路径，则当前车道宽度使用感知宽度。
    int average_point_size = 5;
    for (auto iter = navigation_path_list_.begin();
         iter != navigation_path_list_.end(); ++iter) {
      const auto &curr_path = std::get<3>(*iter)->path();

      // Left neighbor
      auto prev_iter = std::prev(iter);
      if (prev_iter != navigation_path_list_.end()) {
        const auto &prev_path = std::get<3>(*prev_iter)->path();
        average_point_size = std::min(
            average_point_size,
            std::min(curr_path.path_point_size(), prev_path.path_point_size()));
        double lateral_distance_sum = 0.0;
        for (int i = 0; i < average_point_size; ++i) {
          lateral_distance_sum +=
              fabs(curr_path.path_point(i).y() - prev_path.path_point(i).y());
        }
        double width = lateral_distance_sum /
                       static_cast<double>(average_point_size) / 2.0;
        width = common::math::Clamp(width, config_.min_lane_half_width(),
                                    config_.max_lane_half_width());

        auto &curr_left_width = std::get<1>(*iter);
        auto &prev_right_width = std::get<2>(*prev_iter);
        if (std::get<0>(*iter) == std::get<0>(current_navi_path_tuple_)) {
          prev_right_width = 2.0 * width - curr_left_width;
        } else {
          curr_left_width = width;
          prev_right_width = width;
        }
      }
      // Right neighbor
      auto next_iter = std::next(iter);
      if (next_iter != navigation_path_list_.end()) {
        const auto &next_path = std::get<3>(*next_iter)->path();
        average_point_size = std::min(
            average_point_size,
            std::min(curr_path.path_point_size(), next_path.path_point_size()));
        double lateral_distance_sum = 0.0;
        for (int i = 0; i < average_point_size; ++i) {
          lateral_distance_sum +=
              fabs(curr_path.path_point(i).y() - next_path.path_point(i).y());
        }
        double width = lateral_distance_sum /
                       static_cast<double>(average_point_size) / 2.0;
        width = common::math::Clamp(width, config_.min_lane_half_width(),
                                    config_.max_lane_half_width());

        auto &curr_right_width = std::get<2>(*iter);
        auto &next_left_width = std::get<1>(*next_iter);
        if (std::get<0>(*iter) == std::get<0>(current_navi_path_tuple_)) {
          next_left_width = 2.0 * width - curr_right_width;
        } else {
          next_left_width = width;
          curr_right_width = width;
        }
      }
    }
    return true;
  }

4.生成导航线后，添加到相对地图中

//从导航车道中创建地图
bool RelativeMap::CreateMapFromNavigationLane(MapMsg* map_msg) {
  ………………
  //通过navigation_path创建map proto的地图
  if (!navigation_lane_.CreateMap(config_.map_param(), map_msg)) {
    LogErrorStatus(map_msg,
                   "Failed to create map from current navigation path.");
    AERROR << "Failed to create map from navigation path.";
    return false;
  } 
……
}