Apollo Map模块解析

1. hdmap文件夹

1.1 adapter文件夹功能介绍

        入口为opendrive_adapter.cc（严格来说不叫入口，这部分代码只是API,供其他模块调用）。opendrive_adapter调用xml_parser下的各种xxx_xml_parser类的方法parse()将高精地图（.xml格式文件）解析为xxxInternal格式。后调用proto_origanizer类中的方法将xxxInternal格式的数据解析为ProtoData格式的数据。        

        xxxInternal数据格式在common_define.h中定义，以RoadInternal为例子：

using PbRoad = apollo::hdmap::Road;

...

struct RoadInternal {
  std::string id;
  PbRoad road;

  bool in_junction;
  std::string junction_id;

  std::string type;

  std::vector<RoadSectionInternal> sections;

  std::vector<TrafficLightInternal> traffic_lights;
  // std::vector<RSUInternal> rsus;
  std::vector<StopSignInternal> stop_signs;
  std::vector<YieldSignInternal> yield_signs;
  std::vector<PbCrosswalk> crosswalks;
  std::vector<PbClearArea> clear_areas;
  std::vector<PbSpeedBump> speed_bumps;
  std::vector<StopLineInternal> stop_lines;
  std::vector<PbParkingSpace> parking_spaces;
  std::vector<PbPNCJunction> pnc_junctions;

  RoadInternal() : in_junction(false) { junction_id = ""; }
};

         ProtoData数据格式定义在proto_origanizer.h中：

struct ProtoData {
  PbHeader header;
  std::unordered_map<std::string, PbLane> pb_lanes;
  std::unordered_map<std::string, PbRoad> pb_roads;
  std::unordered_map<std::string, PbCrosswalk> pb_crosswalks;
  std::unordered_map<std::string, PbClearArea> pb_clear_areas;
  std::unordered_map<std::string, PbSpeedBump> pb_speed_bumps;
  std::unordered_map<std::string, PbJunction> pb_junction;
  std::unordered_map<std::string, PbSignal> pb_signals;
  std::unordered_map<std::string, PbStopSign> pb_stop_signs;
  std::unordered_map<std::string, PbYieldSign> pb_yield_signs;
  std::unordered_map<std::string, PbOverlap> pb_overlaps;
  std::unordered_map<std::string, PbJunction> pb_junctions;
  std::unordered_map<std::string, StopLineInternal> pb_stop_lines;
  std::unordered_map<std::string, PbParkingSpace> pb_parking_spaces;
  std::unordered_map<std::string, PbPNCJunction> pb_pnc_junctions;
  std::unordered_map<std::string, PbRSU> pb_rsus;
};

         入口函数为OpendriveAdapter::LoadData()，通过调用RoadsXmlParser::Parse()来将xml文件中与road相关的标签信息存入RoadInternal结构中，涉及RoadInternal的id、road的id、junction_id、type等信息。通过顺便调用LanesXmlParser::Parse（）来填入sections的相关信息。最后调用Parse_road_objects（）和Parse_road_signals（）来将RoadInternal中包含的traffic_lights、...pnc_junctions信息进行填充。

std::vector<RoadInternal> roads;
  status = RoadsXmlParser::Parse(*root_node, &roads);
  if (!status.ok()) {
    AERROR << "fail to parse opendrive road, " << status.error_message();
    return false;
  }

...
Status RoadsXmlParser::Parse(const tinyxml2::XMLElement& xml_node,
                             std::vector<RoadInternal>* roads) {
  CHECK_NOTNULL(roads);

  auto road_node = xml_node.FirstChildElement("road");
  while (road_node) {
    // road attributes
    std::string id;
    std::string junction_id;
    int checker = UtilXmlParser::QueryStringAttribute(*road_node, "id", &id);
    checker += UtilXmlParser::QueryStringAttribute(*road_node, "junction",
                                                   &junction_id);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parsing road attributes";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

    RoadInternal road_internal;
    road_internal.id = id;
    road_internal.road.mutable_id()->set_id(id);
    if (IsRoadBelongToJunction(junction_id)) {
      road_internal.road.mutable_junction_id()->set_id(junction_id);
    }

    std::string type;
    checker = UtilXmlParser::QueryStringAttribute(*road_node, "type", &type);
    if (checker != tinyxml2::XML_SUCCESS) {
      // forward compatibility with old data
      type = "CITYROAD";
    }
    PbRoadType pb_road_type;
    RETURN_IF_ERROR(to_pb_road_type(type, &pb_road_type));
    road_internal.road.set_type(pb_road_type);

    // lanes
    RETURN_IF_ERROR(LanesXmlParser::Parse(*road_node, road_internal.id,
                                          &road_internal.sections));

    // objects
    Parse_road_objects(*road_node, &road_internal);
    // signals
    Parse_road_signals(*road_node, &road_internal);

    roads->push_back(road_internal);
    road_node = road_node->NextSiblingElement("road");
  }

  return Status::OK();
}

...
void RoadsXmlParser::Parse_road_objects(const tinyxml2::XMLElement& xml_node,
                                        RoadInternal* road_info) {
  CHECK_NOTNULL(road_info);

  // objects
  auto sub_node = xml_node.FirstChildElement("objects");
  if (sub_node != nullptr) {
    // stop line
    ObjectsXmlParser::ParseStopLines(*sub_node, &road_info->stop_lines);
    // crosswalks
    ObjectsXmlParser::ParseCrosswalks(*sub_node, &road_info->crosswalks);
    // clearareas
    ObjectsXmlParser::ParseClearAreas(*sub_node, &road_info->clear_areas);
    // speed_bumps
    ObjectsXmlParser::ParseSpeedBumps(*sub_node, &road_info->speed_bumps);
    // parking_spaces
    ObjectsXmlParser::ParseParkingSpaces(*sub_node, &road_info->parking_spaces);
    // pnc_junctions
    ObjectsXmlParser::ParsePNCJunctions(*sub_node, &road_info->pnc_junctions);
  }
}

         在成功将xml中的road相关标签信息存入RoadInternal数据结构后，opendrive_adapter::LoadData()在最后调用proto_organizer.GetRoadElements(&roads);来将RoadInternal的数据解析为ProtoData数据格式，用哈希表的方式来对lane、road、crossroad等地图元素进行管理。如下面代码所示，通过遍历每个RoadInternal对pb_lanes、pb_roads、pb_crosswalks等ProtoData中定义的unordered_map对象进行赋值。

void ProtoOrganizer::GetRoadElements(std::vector<RoadInternal>* roads) {
  for (auto& road_internal : *roads) {
    // lanes
    for (auto& section_internal : road_internal.sections) {
      for (auto& lane_internal : section_internal.lanes) {
        std::string lane_id = lane_internal.lane.id().id();
        proto_data_.pb_lanes[lane_id] = lane_internal.lane;
        section_internal.section.add_lane_id()->set_id(lane_id);
      }
      (*road_internal.road.add_section()) = section_internal.section;
      proto_data_.pb_roads[road_internal.id] = road_internal.road;
    }
    // crosswalks
    for (auto& crosswalk : road_internal.crosswalks) {
      proto_data_.pb_crosswalks[crosswalk.id().id()] = crosswalk;
    }
    // parking_spaces
    for (auto& parking_space : road_internal.parking_spaces) {
      proto_data_.pb_parking_spaces[parking_space.id().id()] = parking_space;
    }
    // clear areas
    for (auto& clear_area : road_internal.clear_areas) {
      proto_data_.pb_clear_areas[clear_area.id().id()] = clear_area;
    }
    // speed_bump
    for (auto& speed_bump : road_internal.speed_bumps) {
      proto_data_.pb_speed_bumps[speed_bump.id().id()] = speed_bump;
    }
    // stop lines
    for (auto& stop_line_internal : road_internal.stop_lines) {
      proto_data_.pb_stop_lines[stop_line_internal.id] = stop_line_internal;
    }
    // traffic_lights
    for (auto& traffic_light_internal : road_internal.traffic_lights) {
      auto& traffic_light = traffic_light_internal.traffic_light;
      for (auto stop_line_id : traffic_light_internal.stop_line_ids) {
        CHECK_GT(proto_data_.pb_stop_lines.count(stop_line_id), 0U);
        auto& stop_line_curve = proto_data_.pb_stop_lines[stop_line_id].curve;
        (*traffic_light.add_stop_line()) = stop_line_curve;
      }
      proto_data_.pb_signals[traffic_light.id().id()] = traffic_light;
    }
    // stop signs
    for (auto& stop_sign_internal : road_internal.stop_signs) {
      auto& stop_sign = stop_sign_internal.stop_sign;
      for (auto stop_line_id : stop_sign_internal.stop_line_ids) {
        CHECK_GT(proto_data_.pb_stop_lines.count(stop_line_id), 0U);
        auto& stop_line_curve = proto_data_.pb_stop_lines[stop_line_id].curve;
        (*stop_sign.add_stop_line()) = stop_line_curve;
      }
      proto_data_.pb_stop_signs[stop_sign.id().id()] = stop_sign;
    }
    // yield signs
    for (auto& yield_sign_internal : road_internal.yield_signs) {
      auto& yield_sign = yield_sign_internal.yield_sign;
      for (auto stop_line_id : yield_sign_internal.stop_line_ids) {
        CHECK_GT(proto_data_.pb_stop_lines.count(stop_line_id), 0U);
        auto& stop_line_curve = proto_data_.pb_stop_lines[stop_line_id].curve;
        (*yield_sign.add_stop_line()) = stop_line_curve;
      }
      proto_data_.pb_yield_signs[yield_sign.id().id()] = yield_sign;
    }
    // pnc junctions
    for (auto& pnc_junction : road_internal.pnc_junctions) {
      proto_data_.pb_pnc_junctions[pnc_junction.id().id()] = pnc_junction;
    }
  }
}

        最后调用proto_organizer.OutputData(pb_map)来可以打印出各个道路元素的个数。pb_map为apollo::hdmap::Map类型的对象。

总结：

        上面以从xml获取road相关信息举例，总体思路为将xml信息解析为xxxInternal格式，然后再将xxxInternal数据格式转化为ProtoData格式，统一用哈希表来对所有map元素进行管理。为啥经过这个中间步骤不是很懂，可能是方便代码管理吧，一下子将xml元素直接全部转化为ProtoData结构的信息可能代码会很混乱。

1.2 剩余文件功能介绍（hdmap_common、 hdmap_impl、 hdmap_util和hdmap）

        上图中的类基本都是地图元素管理的API，由其他模块调用。其中， hdmap_util和hdmap两个文件基本都是接口函数，真正干活的代码在hdmap_impl和hdmap_common中。

        hdmap_common.h和hdmap_common.c中，主要为每种道路元素所具备API，以道路元素Lane为例，在hdmap_common.h中定义了LaneInfo类，在此类中定义了某个lane所具备的相关成员函数，例如获取该lane的heading、segments、该lane上所有的singal 以及其他重叠区域等方法。

class LaneInfo {
 public:
  explicit LaneInfo(const Lane &lane);

  const Id &id() const { return lane_.id(); }
  const Id &road_id() const { return road_id_; }
  const Id &section_id() const { return section_id_; }
  const Lane &lane() const { return lane_; }
  const std::vector<apollo::common::math::Vec2d> &points() const {
    return points_;
  }
  const std::vector<apollo::common::math::Vec2d> &unit_directions() const {
    return unit_directions_;
  }
  double Heading(const double s) const;
  double Curvature(const double s) const;
  const std::vector<double> &headings() const { return headings_; }
  const std::vector<apollo::common::math::LineSegment2d> &segments() const {
    return segments_;
  }
  const std::vector<double> &accumulate_s() const { return accumulated_s_; }
  const std::vector<OverlapInfoConstPtr> &overlaps() const { return overlaps_; }
  const std::vector<OverlapInfoConstPtr> &cross_lanes() const {
    return cross_lanes_;
  }
  const std::vector<OverlapInfoConstPtr> &signals() const { return signals_; }
  const std::vector<OverlapInfoConstPtr> &yield_signs() const {
    return yield_signs_;
  }
  const std::vector<OverlapInfoConstPtr> &stop_signs() const {
    return stop_signs_;
  }
  const std::vector<OverlapInfoConstPtr> &crosswalks() const {
    return crosswalks_;
  }
  const std::vector<OverlapInfoConstPtr> &junctions() const {
    return junctions_;
  }
  const std::vector<OverlapInfoConstPtr> &clear_areas() const {
    return clear_areas_;
  }
  const std::vector<OverlapInfoConstPtr> &speed_bumps() const {
    return speed_bumps_;
  }
  const std::vector<OverlapInfoConstPtr> &parking_spaces() const {
    return parking_spaces_;
  }
  const std::vector<OverlapInfoConstPtr> &pnc_junctions() const {
    return pnc_junctions_;
  }
  double total_length() const { return total_length_; }
  using SampledWidth = std::pair<double, double>;
  const std::vector<SampledWidth> &sampled_left_width() const {
    return sampled_left_width_;
  }
  const std::vector<SampledWidth> &sampled_right_width() const {
    return sampled_right_width_;
  }
  void GetWidth(const double s, double *left_width, double *right_width) const;
  double GetWidth(const double s) const;
  double GetEffectiveWidth(const double s) const;

  const std::vector<SampledWidth> &sampled_left_road_width() const {
    return sampled_left_road_width_;
  }
  const std::vector<SampledWidth> &sampled_right_road_width() const {
    return sampled_right_road_width_;
  }
  void GetRoadWidth(const double s, double *left_width,
                    double *right_width) const;
  double GetRoadWidth(const double s) const;

  bool IsOnLane(const apollo::common::math::Vec2d &point) const;
  bool IsOnLane(const apollo::common::math::Box2d &box) const;

  apollo::common::PointENU GetSmoothPoint(double s) const;
  double DistanceTo(const apollo::common::math::Vec2d &point) const;
  double DistanceTo(const apollo::common::math::Vec2d &point,
                    apollo::common::math::Vec2d *map_point, double *s_offset,
                    int *s_offset_index) const;
  apollo::common::PointENU GetNearestPoint(
      const apollo::common::math::Vec2d &point, double *distance) const;
  bool GetProjection(const apollo::common::math::Vec2d &point,
                     double *accumulate_s, double *lateral) const;

 private:
  friend class HDMapImpl;
  friend class RoadInfo;
  void Init();
  void PostProcess(const HDMapImpl &map_instance);
  void UpdateOverlaps(const HDMapImpl &map_instance);
  double GetWidthFromSample(const std::vector<LaneInfo::SampledWidth> &samples,
                            const double s) const;
  void CreateKDTree();
  void set_road_id(const Id &road_id) { road_id_ = road_id; }
  void set_section_id(const Id &section_id) { section_id_ = section_id; }

 private:
  const Lane &lane_;
  std::vector<apollo::common::math::Vec2d> points_;
  std::vector<apollo::common::math::Vec2d> unit_directions_;
  std::vector<double> headings_;
  std::vector<apollo::common::math::LineSegment2d> segments_;
  std::vector<double> accumulated_s_;
  std::vector<std::string> overlap_ids_;
  std::vector<OverlapInfoConstPtr> overlaps_;
  std::vector<OverlapInfoConstPtr> cross_lanes_;
  std::vector<OverlapInfoConstPtr> signals_;
  std::vector<OverlapInfoConstPtr> yield_signs_;
  std::vector<OverlapInfoConstPtr> stop_signs_;
  std::vector<OverlapInfoConstPtr> crosswalks_;
  std::vector<OverlapInfoConstPtr> junctions_;
  std::vector<OverlapInfoConstPtr> clear_areas_;
  std::vector<OverlapInfoConstPtr> speed_bumps_;
  std::vector<OverlapInfoConstPtr> parking_spaces_;
  std::vector<OverlapInfoConstPtr> pnc_junctions_;
  double total_length_ = 0.0;
  std::vector<SampledWidth> sampled_left_width_;
  std::vector<SampledWidth> sampled_right_width_;

  std::vector<SampledWidth> sampled_left_road_width_;
  std::vector<SampledWidth> sampled_right_road_width_;

  std::vector<LaneSegmentBox> segment_box_list_;
  std::unique_ptr<LaneSegmentKDTree> lane_segment_kdtree_;

  Id road_id_;
  Id section_id_;
};
using LaneSegmentBox =
    ObjectWithAABox<LaneInfo, apollo::common::math::LineSegment2d>;
using LaneSegmentKDTree = apollo::common::math::AABoxKDTree2d<LaneSegmentBox>;

               在hdmap_impl.h中则实现了HDMapImpl类，该类主要通过哈希表实现了对各种各种道路元素的管理，大多数成员函数为获取某个位置，一定范围内的道路元素有哪些。