CARLA
InMemoryMap.cpp
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 
9 
10 namespace carla {
11 namespace traffic_manager {
12 
13 
14  namespace cg = carla::geom;
15  using namespace constants::Map;
16 
17  using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
18  using RawNodeList = std::vector<WaypointPtr>;
19 
20  InMemoryMap::InMemoryMap(WorldMap world_map) : _world_map(world_map) {}
22 
24  return std::make_tuple(wp->GetRoadId(), wp->GetLaneId(), wp->GetSectionId());
25  }
26 
28  return GetSegmentId(swp->GetWaypoint());
29  }
30 
31  std::vector<SimpleWaypointPtr> InMemoryMap::GetSuccessors(const SegmentId segment_id,
32  const SegmentTopology &segment_topology, const SegmentMap &segment_map) {
33  std::vector<SimpleWaypointPtr> result;
34  if (segment_topology.find(segment_id) == segment_topology.end()) {
35  return result;
36  }
37 
38  for (const auto &successor_segment_id : segment_topology.at(segment_id).second) {
39  if (segment_map.find(successor_segment_id) == segment_map.end()) {
40  auto successors = GetSuccessors(successor_segment_id, segment_topology, segment_map);
41  result.insert(result.end(), successors.begin(), successors.end());
42  } else {
43  result.emplace_back(segment_map.at(successor_segment_id).front());
44  }
45  }
46  return result;
47  }
48 
49  std::vector<SimpleWaypointPtr> InMemoryMap::GetPredecessors(const SegmentId segment_id,
50  const SegmentTopology &segment_topology, const SegmentMap &segment_map) {
51  std::vector<SimpleWaypointPtr> result;
52  if (segment_topology.find(segment_id) == segment_topology.end()) {
53  return result;
54  }
55 
56  for (const auto &predecessor_segment_id : segment_topology.at(segment_id).first) {
57  if (segment_map.find(predecessor_segment_id) == segment_map.end()) {
58  auto predecessors = GetPredecessors(predecessor_segment_id, segment_topology, segment_map);
59  result.insert(result.end(), predecessors.begin(), predecessors.end());
60  } else {
61  result.emplace_back(segment_map.at(predecessor_segment_id).back());
62  }
63  }
64  return result;
65  }
66 
68 
69  // 1. Building segment topology (i.e., defining set of segment predecessors and successors)
70  assert(_world_map != nullptr && "No map reference found.");
71  auto waypoint_topology = _world_map->GetTopology();
72 
73  SegmentTopology segment_topology;
74  std::unordered_map<int64_t, std::pair<std::set<crd::RoadId>, std::set<crd::RoadId>>> std_road_connectivity;
75  std::unordered_map<crd::RoadId, bool> is_real_junction;
76 
77  for (auto &connection : waypoint_topology) {
78  auto &waypoint = connection.first;
79  auto &successor = connection.second;
80 
81  // Setting segment predecessors and successors.
82  SegmentId waypoint_segment_id = GetSegmentId(connection.first);
83  SegmentId successor_segment_id = GetSegmentId(connection.second);
84  using SegIdVectorPair = std::pair<std::vector<SegmentId>, std::vector<SegmentId>>;
85  SegIdVectorPair &connection_first = segment_topology[waypoint_segment_id];
86  SegIdVectorPair &connection_second = segment_topology[successor_segment_id];
87  connection_first.second.push_back(successor_segment_id);
88  connection_second.first.push_back(waypoint_segment_id);
89 
90  // From path to standard road.
91  bool waypoint_is_junction = waypoint->IsJunction();
92  bool successor_is_junction = successor->IsJunction();
93  if (waypoint_is_junction && !successor_is_junction) {
94  crd::RoadId path_id = waypoint->GetRoadId();
95  int64_t std_road_id = static_cast<int64_t>(successor->GetRoadId());
96  std_road_id = (successor->GetLaneId() < 0) ? -1 * std_road_id : std_road_id;
97 
98  std::set<crd::RoadId> &in_paths = std_road_connectivity[std_road_id].first;
99  in_paths.insert(path_id);
100 
101  if (in_paths.size() >= 2) {
102  for (auto &in_path_id: in_paths) {
103  is_real_junction[in_path_id] = true;
104  }
105  }
106  }
107 
108  // From standard road to path.
109  if (!waypoint_is_junction && successor_is_junction) {
110  crd::RoadId path_id = successor->GetRoadId();
111  int64_t std_road_id = static_cast<int64_t>(waypoint->GetRoadId());
112  std_road_id = (waypoint->GetLaneId() < 0) ? -1 * std_road_id : std_road_id;
113 
114  std::set<crd::RoadId> &out_paths = std_road_connectivity[std_road_id].second;
115  out_paths.insert(path_id);
116 
117  if (out_paths.size() >= 2) {
118  for (auto &out_path_id: out_paths) {
119  is_real_junction[out_path_id] = true;
120  }
121  }
122  }
123  }
124 
125  // 2. Consuming the raw dense topology from cc::Map into SimpleWaypoints.
126  SegmentMap segment_map;
127  assert(_world_map != nullptr && "No map reference found.");
128  auto raw_dense_topology = _world_map->GenerateWaypoints(MAP_RESOLUTION);
129  for (auto &waypoint_ptr: raw_dense_topology) {
130  segment_map[GetSegmentId(waypoint_ptr)].emplace_back(std::make_shared<SimpleWaypoint>(waypoint_ptr));
131  }
132 
133  // 3. Processing waypoints.
134  auto distance_squared =
135  [](cg::Location l1, cg::Location l2) {
136  return cg::Math::DistanceSquared(l1, l2);
137  };
138  auto square = [](float input) {return std::pow(input, 2);};
139  auto compare_s = [](const SimpleWaypointPtr &swp1, const SimpleWaypointPtr &swp2) {
140  return (swp1->GetWaypoint()->GetDistance() < swp2->GetWaypoint()->GetDistance());
141  };
142 
143  GeoGridId geodesic_grid_id_counter = -1;
144  for (auto &segment: segment_map) {
145  auto &segment_waypoints = segment.second;
146 
147  // Generating geodesic grid ids.
148  ++geodesic_grid_id_counter;
149 
150  // Ordering waypoints according to road direction.
151  std::sort(segment_waypoints.begin(), segment_waypoints.end(), compare_s);
152  auto lane_id = segment_waypoints.front()->GetWaypoint()->GetLaneId();
153  if (lane_id > 0) {
154  std::reverse(segment_waypoints.begin(), segment_waypoints.end());
155  }
156 
157  // Placing intra-segment connections.
158  cg::Location grid_edge_location = segment_waypoints.front()->GetLocation();
159  for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
160 
161  // Assigning grid id.
162  if (distance_squared(grid_edge_location, segment_waypoints.at(i)->GetLocation()) >
163  square(MAX_GEODESIC_GRID_LENGTH)) {
164  ++geodesic_grid_id_counter;
165  grid_edge_location = segment_waypoints.at(i)->GetLocation();
166  }
167  segment_waypoints.at(i)->SetGeodesicGridId(geodesic_grid_id_counter);
168 
169  segment_waypoints.at(i)->SetNextWaypoint({segment_waypoints.at(i + 1)});
170  segment_waypoints.at(i + 1)->SetPreviousWaypoint({segment_waypoints.at(i)});
171  }
172  segment_waypoints.back()->SetGeodesicGridId(geodesic_grid_id_counter);
173 
174  // Adding simple waypoints to processed dense topology.
175  for (auto swp: segment_waypoints) {
176  // Checking whether the waypoint is a real junction.
177  auto road_id = swp->GetWaypoint()->GetRoadId();
178  if (swp->GetWaypoint()->IsJunction() && !is_real_junction.count(road_id)) {
179  swp->SetIsJunction(false);
180  } else {
181  swp->SetIsJunction(swp->GetWaypoint()->IsJunction());
182  }
183 
184  dense_topology.push_back(swp);
185  }
186  }
187 
188  // Localizing waypoints into grids.
189  for (auto &simple_waypoint: dense_topology) {
190  if (simple_waypoint != nullptr) {
191  const cg::Location loc = simple_waypoint->GetLocation();
192  Point3D point(loc.x, loc.y, loc.z);
193  rtree.insert(std::make_pair(point, simple_waypoint));
194  }
195  }
196 
197  // Placing inter-segment connections.
198  for (auto &segment : segment_map) {
199  SegmentId segment_id = segment.first;
200  auto &segment_waypoints = segment.second;
201 
202  auto successors = GetSuccessors(segment_id, segment_topology, segment_map);
203  auto predecessors = GetPredecessors(segment_id, segment_topology, segment_map);
204 
205  segment_waypoints.front()->SetPreviousWaypoint(predecessors);
206  segment_waypoints.back()->SetNextWaypoint(successors);
207  }
208 
209  // Linking lane change connections.
210  for (auto &simple_waypoint:dense_topology) {
211  if (!simple_waypoint->CheckJunction()) {
212  FindAndLinkLaneChange(simple_waypoint);
213  }
214  }
215 
216  // Linking any unconnected segments.
217  for (auto &swp: dense_topology) {
218  if (swp->GetNextWaypoint().empty()) {
219  auto neighbour = swp->GetRightWaypoint();
220  if (!neighbour) {
221  neighbour = swp->GetLeftWaypoint();
222  }
223 
224  if (neighbour) {
225  swp->SetNextWaypoint(neighbour->GetNextWaypoint());
226  for (auto next_waypoint : neighbour->GetNextWaypoint()) {
227  next_waypoint->SetPreviousWaypoint({swp});
228  }
229  }
230  }
231  }
232  }
233 
235 
236  Point3D query_point(loc.x, loc.y, loc.z);
237  std::vector<SpatialTreeEntry> result_1;
238 
239  rtree.query(bgi::nearest(query_point, 1), std::back_inserter(result_1));
240  SpatialTreeEntry &closest_entry = result_1.front();
241  SimpleWaypointPtr &closest_point = closest_entry.second;
242 
243  return closest_point;
244  }
245 
246  std::vector<SimpleWaypointPtr> InMemoryMap::GetDenseTopology() const {
247  return dense_topology;
248  }
249 
251 
252  const WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
253  const crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange();
254 
255  /// Cheack for transits
256  switch(lane_change)
257  {
258  /// Left transit way point present only
259  case crd::element::LaneMarking::LaneChange::Left:
260  {
261  const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
262  if (left_waypoint != nullptr &&
263  left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
264  (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
265 
266  SimpleWaypointPtr closest_simple_waypoint = GetWaypoint(left_waypoint->GetTransform().location);
267  reference_waypoint->SetLeftWaypoint(closest_simple_waypoint);
268  }
269  }
270  break;
271 
272  /// Right transit way point present only
273  case crd::element::LaneMarking::LaneChange::Right:
274  {
275  const WaypointPtr right_waypoint = raw_waypoint->GetRight();
276  if(right_waypoint != nullptr &&
277  right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
278  (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
279 
280  SimpleWaypointPtr closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location);
281  reference_waypoint->SetRightWaypoint(closest_simple_waypoint);
282  }
283  }
284  break;
285 
286  /// Both left and right transit present
288  {
289  /// Right transit way point
290  const WaypointPtr right_waypoint = raw_waypoint->GetRight();
291  if (right_waypoint != nullptr &&
292  right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
293  (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
294 
295  SimpleWaypointPtr closest_simple_waypointR = GetWaypoint(right_waypoint->GetTransform().location);
296  reference_waypoint->SetRightWaypoint(closest_simple_waypointR);
297  }
298 
299  /// Left transit way point
300  const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
301  if (left_waypoint != nullptr &&
302  left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
303  (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
304 
305  SimpleWaypointPtr closest_simple_waypointL = GetWaypoint(left_waypoint->GetTransform().location);
306  reference_waypoint->SetLeftWaypoint(closest_simple_waypointL);
307  }
308  }
309  break;
310 
311  /// For no transit waypoint (left or right)
312  default: break;
313  }
314  }
315 
316  std::string InMemoryMap::GetMapName() {
317  assert(_world_map != nullptr && "No map reference found.");
318  return _world_map->GetName();
319  }
320 
321 } // namespace traffic_manager
322 } // namespace carla
SegmentId GetSegmentId(const WaypointPtr &wp) const
Computes the segment id of a given waypoint.
Definition: InMemoryMap.cpp:23
uint32_t RoadId
Definition: RoadTypes.h:15
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
void reverse(I begin, I end)
Definition: pugixml.cpp:7358
SimpleWaypointPtr GetWaypoint(const cg::Location loc) const
This method returns the closest waypoint to a given location on the map.
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.
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:66
std::vector< std::pair< WaypointPtr, WaypointPtr > > TopologyList
Definition: InMemoryMap.cpp:17
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.
static const float MAX_GEODESIC_GRID_LENGTH
Definition: Constants.h:96
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
crd::JuncId GeoGridId
Definition: InMemoryMap.h:38
carla::SharedPtr< cc::Map > WorldMap
Definition: InMemoryMap.h:39
static const double MAP_RESOLUTION
Definition: Constants.h:97
std::vector< WaypointPtr > RawNodeList
Definition: InMemoryMap.cpp:18
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
void sort(I begin, I end, const Pred &pred)
Definition: pugixml.cpp:7444
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