InMemoryMap.h

浏览该文件的文档.

// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#pragma once
 
#include <chrono>
#include <memory>
#include <unordered_map>
#include <unordered_set>
 
#if defined(__clang__)
#  pragma clang diagnostic push
#  pragma clang diagnostic ignored "-Wshadow"
#endif
#include "boost/geometry.hpp"
#include "boost/geometry/geometries/point.hpp"
#include "boost/geometry/index/rtree.hpp"
#if defined(__clang__)
#  pragma clang diagnostic pop
#endif
 
#include "carla/client/Map.h"
#include "carla/client/Waypoint.h"
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/Memory.h"
#include "carla/road/RoadTypes.h"
 
#include "carla/trafficmanager/RandomGenerator.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
#include "carla/trafficmanager/CachedSimpleWaypoint.h"
 
namespace carla {
namespace traffic_manager {
 
namespace cg = carla::geom;
namespace cc = carla::client;
namespace crd = carla::road;
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
 
  using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
  using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
  using NodeList = std::vector<SimpleWaypointPtr>;
  using GeoGridId = crd::JuncId;
  using WorldMap = carla::SharedPtr<const cc::Map>;
 
  using Point3D = bg::model::point<float, 3, bg::cs::cartesian>;
  using Box = bg::model::box<Point3D>;
  using SpatialTreeEntry = std::pair<Point3D, SimpleWaypointPtr>;
 
  using SegmentId = std::tuple<crd::RoadId, crd::LaneId, crd::SectionId>;
  using SegmentTopology = std::map<SegmentId, std::pair<std::vector<SegmentId>, std::vector<SegmentId>>>;
  using SegmentMap = std::map<SegmentId, std::vector<SimpleWaypointPtr>>;
  using Rtree = bgi::rtree<SpatialTreeEntry, bgi::rstar<16>>;
 
  /// This class builds a discretized local map-cache.
  /// Instantiate the class with the world and run SetUp() to construct the
  /// local map.
  class InMemoryMap {
 
  private:
 
    /// Object to hold the world map received by the constructor.
    WorldMap _world_map;
    /// Structure to hold all custom waypoint objects after interpolation of
    /// sparse topology.
    NodeList dense_topology;
    /// Spatial quadratic R-tree for indexing and querying waypoints.
    Rtree rtree;
 
  public:
 
    InMemoryMap(WorldMap world_map);
    ~InMemoryMap();
 
    static void Cook(WorldMap world_map, const std::string& path);
 
    //bool Load(const std::string& filename);
    bool Load(const std::vector<uint8_t>& content);
 
    /// This method constructs the local map with a resolution of sampling_resolution.
    void SetUp();
 
    /// This method returns the closest waypoint to a given location on the map.
    SimpleWaypointPtr GetWaypoint(const cg::Location loc) const;
 
    /// This method returns n waypoints in an delta area with a certain distance from the ego vehicle.
    NodeList GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const;
 
    /// This method returns the full list of discrete samples of the map in the local cache.
    NodeList GetDenseTopology() const;
 
    std::string GetMapName();
 
    const cc::Map& GetMap() const;
 
  private:
    void Save(const std::string& path);
 
    void SetUpDenseTopology();
    void SetUpSpatialTree();
    void SetUpRoadOption();
 
    /// This method is used to find and place lane change links.
    void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);
 
    NodeList GetSuccessors(const SegmentId segment_id,
                          const SegmentTopology &segment_topology,
                          const SegmentMap &segment_map);
    NodeList GetPredecessors(const SegmentId segment_id,
                            const SegmentTopology &segment_topology,
                            const SegmentMap &segment_map);
 
    /// Computes the segment id of a given waypoint.
    /// The Id takes into account OpenDrive's road Id, lane Id and Section Id.
    SegmentId GetSegmentId(const WaypointPtr &wp) const;
    SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const;
  };
 
} // namespace traffic_manager
} // namespace carla