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 <memory>
10 #include <unordered_map>
11 #include <unordered_set>
12 
13 #include "boost/geometry.hpp"
14 #include "boost/geometry/geometries/point.hpp"
15 #include "boost/geometry/index/rtree.hpp"
16 
17 #include "carla/client/Map.h"
18 #include "carla/client/Waypoint.h"
19 #include "carla/geom/Location.h"
20 #include "carla/geom/Math.h"
21 #include "carla/Memory.h"
22 #include "carla/road/RoadTypes.h"
23 
25 
26 namespace carla {
27 namespace traffic_manager {
28 
29 namespace cg = carla::geom;
30 namespace cc = carla::client;
31 namespace crd = carla::road;
32 namespace bg = boost::geometry;
33 namespace bgi = boost::geometry::index;
34 
36  using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
37  using NodeList = std::vector<SimpleWaypointPtr>;
40 
41  using Point3D = bg::model::point<float, 3, bg::cs::cartesian>;
42  using SpatialTreeEntry = std::pair<Point3D, SimpleWaypointPtr>;
43 
44  using SegmentId = std::tuple<crd::RoadId, crd::LaneId, crd::SectionId>;
45  using SegmentTopology = std::map<SegmentId, std::pair<std::vector<SegmentId>, std::vector<SegmentId>>>;
46  using SegmentMap = std::map<SegmentId, std::vector<SimpleWaypointPtr>>;
47 
48  /// This class builds a discretized local map-cache.
49  /// Instantiate the class with the world and run SetUp() to construct the
50  /// local map.
51  class InMemoryMap {
52 
53  private:
54 
55  /// Object to hold the world map received by the constructor.
57  /// Structure to hold all custom waypoint objects after interpolation of
58  /// sparse topology.
60  /// Spatial quadratic R-tree for indexing and querying waypoints.
61  bgi::rtree<SpatialTreeEntry, bgi::quadratic<32>> rtree;
62 
63  /// Geodesic grid topology.
64  std::unordered_map<GeoGridId, cg::Location> geodesic_grid_center;
65 
66  public:
67 
68  InMemoryMap(WorldMap world_map);
69  ~InMemoryMap();
70 
71  /// This method constructs the local map with a resolution of
72  /// sampling_resolution.
73  void SetUp();
74 
75  /// This method returns the closest waypoint to a given location on the map.
77 
78  /// This method returns the full list of discrete samples of the map in the
79  /// local cache.
80  NodeList GetDenseTopology() const;
81 
82  std::string GetMapName();
83 
84  private:
85 
86  /// This method is used to find and place lane change links.
87  void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);
88 
89  std::vector<SimpleWaypointPtr> GetSuccessors(const SegmentId segment_id,
90  const SegmentTopology &segment_topology, const SegmentMap &segment_map);
91  std::vector<SimpleWaypointPtr> GetPredecessors(const SegmentId segment_id,
92  const SegmentTopology &segment_topology, const SegmentMap &segment_map);
93 
94  /// Computes the segment id of a given waypoint.
95  /// The Id takes into account OpenDrive's road Id, lane Id and Section Id.
96  SegmentId GetSegmentId(const WaypointPtr &wp) const;
97  SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const;
98  };
99 
100 } // namespace traffic_manager
101 } // namespace carla
SegmentId GetSegmentId(const WaypointPtr &wp) const
Computes the segment id of a given waypoint.
Definition: InMemoryMap.cpp:23
std::vector< SimpleWaypointPtr > GetPredecessors(const SegmentId segment_id, const SegmentTopology &segment_topology, const SegmentMap &segment_map)
Definition: InMemoryMap.cpp:49
bg::model::point< float, 3, bg::cs::cartesian > Point3D
Definition: InMemoryMap.h:41
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:99
void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint)
This method is used to find and place lane change links.
carla::SharedPtr< cc::Waypoint > WaypointPtr
Definition: InMemoryMap.h:35
std::map< SegmentId, std::vector< SimpleWaypointPtr > > SegmentMap
Definition: InMemoryMap.h:46
NodeList GetDenseTopology() const
This method returns the full list of discrete samples of the map in the local cache.
std::map< SegmentId, std::pair< std::vector< SegmentId >, std::vector< SegmentId > >> SegmentTopology
Definition: InMemoryMap.h:45
std::vector< SimpleWaypointPtr > GetSuccessors(const SegmentId segment_id, const SegmentTopology &segment_topology, const SegmentMap &segment_map)
Definition: InMemoryMap.cpp:31
std::vector< SimpleWaypointPtr > NodeList
Definition: InMemoryMap.h:37
crd::JuncId GeoGridId
Definition: InMemoryMap.h:38
carla::SharedPtr< cc::Map > WorldMap
Definition: InMemoryMap.h:39
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
std::pair< Point3D, SimpleWaypointPtr > SpatialTreeEntry
Definition: InMemoryMap.h:42
NodeList dense_topology
Structure to hold all custom waypoint objects after interpolation of sparse topology.
Definition: InMemoryMap.h:59
std::unordered_map< GeoGridId, cg::Location > geodesic_grid_center
Geodesic grid topology.
Definition: InMemoryMap.h:64
This class builds a discretized local map-cache.
Definition: InMemoryMap.h:51
void SetUp()
This method constructs the local map with a resolution of sampling_resolution.
Definition: InMemoryMap.cpp:67
WorldMap _world_map
Object to hold the world map received by the constructor.
Definition: InMemoryMap.h:56
std::tuple< crd::RoadId, crd::LaneId, crd::SectionId > SegmentId
Definition: InMemoryMap.h:44
bgi::rtree< SpatialTreeEntry, bgi::quadratic< 32 > > rtree
Spatial quadratic R-tree for indexing and querying waypoints.
Definition: InMemoryMap.h:61