CARLA
InMemoryMap.h
Go to the documentation of this file.
1 // Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #pragma once
8 
9 #include <chrono>
10 #include <memory>
11 #include <unordered_map>
12 #include <unordered_set>
13 
14 #include "boost/geometry.hpp"
15 #include "boost/geometry/geometries/point.hpp"
16 #include "boost/geometry/index/rtree.hpp"
17 
18 #include "carla/client/Map.h"
19 #include "carla/client/Waypoint.h"
20 #include "carla/geom/Location.h"
21 #include "carla/geom/Math.h"
22 #include "carla/Memory.h"
23 #include "carla/road/RoadTypes.h"
24 
28 
29 namespace carla {
30 namespace traffic_manager {
31 
32 namespace cg = carla::geom;
33 namespace cc = carla::client;
34 namespace crd = carla::road;
35 namespace bg = boost::geometry;
36 namespace bgi = boost::geometry::index;
37 
39  using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
40  using NodeList = std::vector<SimpleWaypointPtr>;
43 
44  using Point3D = bg::model::point<float, 3, bg::cs::cartesian>;
45  using Box = bg::model::box<Point3D>;
46  using SpatialTreeEntry = std::pair<Point3D, SimpleWaypointPtr>;
47 
48  using SegmentId = std::tuple<crd::RoadId, crd::LaneId, crd::SectionId>;
49  using SegmentTopology = std::map<SegmentId, std::pair<std::vector<SegmentId>, std::vector<SegmentId>>>;
50  using SegmentMap = std::map<SegmentId, std::vector<SimpleWaypointPtr>>;
51  using Rtree = bgi::rtree<SpatialTreeEntry, bgi::rstar<16>>;
52 
53  /// This class builds a discretized local map-cache.
54  /// Instantiate the class with the world and run SetUp() to construct the
55  /// local map.
56  class InMemoryMap {
57 
58  private:
59 
60  /// Object to hold the world map received by the constructor.
62  /// Structure to hold all custom waypoint objects after interpolation of
63  /// sparse topology.
65  /// Spatial quadratic R-tree for indexing and querying waypoints.
67 
68  public:
69 
70  InMemoryMap(WorldMap world_map);
71  ~InMemoryMap();
72 
73  static void Cook(WorldMap world_map, const std::string& path);
74 
75  //bool Load(const std::string& filename);
76  bool Load(const std::vector<uint8_t>& content);
77 
78  /// This method constructs the local map with a resolution of sampling_resolution.
79  void SetUp();
80 
81  /// This method returns the closest waypoint to a given location on the map.
83 
84  /// This method returns n waypoints in an delta area with a certain distance from the ego vehicle.
85  NodeList GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const;
86 
87  /// This method returns the full list of discrete samples of the map in the local cache.
88  NodeList GetDenseTopology() const;
89 
90  std::string GetMapName();
91 
92  const cc::Map& GetMap() const;
93 
94  private:
95  void Save(const std::string& path);
96 
97  void SetUpDenseTopology();
98  void SetUpSpatialTree();
99  void SetUpRoadOption();
100 
101  /// This method is used to find and place lane change links.
102  void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);
103 
104  NodeList GetSuccessors(const SegmentId segment_id,
105  const SegmentTopology &segment_topology,
106  const SegmentMap &segment_map);
107  NodeList GetPredecessors(const SegmentId segment_id,
108  const SegmentTopology &segment_topology,
109  const SegmentMap &segment_map);
110 
111  /// Computes the segment id of a given waypoint.
112  /// The Id takes into account OpenDrive's road Id, lane Id and Section Id.
113  SegmentId GetSegmentId(const WaypointPtr &wp) const;
114  SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const;
115  };
116 
117 } // namespace traffic_manager
118 } // namespace carla
SegmentId GetSegmentId(const WaypointPtr &wp) const
Computes the segment id of a given waypoint.
Definition: InMemoryMap.cpp:25
NodeList GetPredecessors(const SegmentId segment_id, const SegmentTopology &segment_topology, const SegmentMap &segment_map)
Definition: InMemoryMap.cpp:52
NodeList GetDenseTopology() const
This method returns the full list of discrete samples of the map in the local cache.
Rtree rtree
Spatial quadratic R-tree for indexing and querying waypoints.
Definition: InMemoryMap.h:66
bg::model::point< float, 3, bg::cs::cartesian > Point3D
Definition: InMemoryMap.h:44
bgi::rtree< SpatialTreeEntry, bgi::rstar< 16 > > Rtree
Definition: InMemoryMap.h:51
SimpleWaypointPtr GetWaypoint(const cg::Location loc) const
This method returns the closest waypoint to a given location on the map.
boost::shared_ptr< T > SharedPtr
Use this SharedPtr (boost::shared_ptr) to keep compatibility with boost::python, but it would be nice...
Definition: Memory.h:20
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
static void Cook(WorldMap world_map, const std::string &path)
Definition: InMemoryMap.cpp:71
NodeList GetSuccessors(const SegmentId segment_id, const SegmentTopology &segment_topology, const SegmentMap &segment_map)
Definition: InMemoryMap.cpp:33
void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint)
This method is used to find and place lane change links.
bg::model::box< Point3D > Box
Definition: InMemoryMap.h:45
void Save(const std::string &path)
Definition: InMemoryMap.cpp:77
std::map< SegmentId, std::vector< SimpleWaypointPtr > > SegmentMap
Definition: InMemoryMap.h:50
std::map< SegmentId, std::pair< std::vector< SegmentId >, std::vector< SegmentId > >> SegmentTopology
Definition: InMemoryMap.h:49
carla::SharedPtr< cc::Waypoint > WaypointPtr
Definition: InMemoryMap.h:38
NodeList GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const
This method returns n waypoints in an delta area with a certain distance from the ego vehicle...
std::vector< SimpleWaypointPtr > NodeList
Definition: InMemoryMap.h:40
crd::JuncId GeoGridId
Definition: InMemoryMap.h:41
bool Load(const std::vector< uint8_t > &content)
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
std::pair< Point3D, SimpleWaypointPtr > SpatialTreeEntry
Definition: InMemoryMap.h:46
NodeList dense_topology
Structure to hold all custom waypoint objects after interpolation of sparse topology.
Definition: InMemoryMap.h:64
const cc::Map & GetMap() const
carla::SharedPtr< const cc::Map > WorldMap
Definition: InMemoryMap.h:42
This class builds a discretized local map-cache.
Definition: InMemoryMap.h:56
void SetUp()
This method constructs the local map with a resolution of sampling_resolution.
WorldMap _world_map
Object to hold the world map received by the constructor.
Definition: InMemoryMap.h:61
std::tuple< crd::RoadId, crd::LaneId, crd::SectionId > SegmentId
Definition: InMemoryMap.h:48