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 
7 #include "carla/Logging.h"
8 
11 #include <boost/geometry/geometries/box.hpp>
12 
13 namespace carla {
14 namespace traffic_manager {
15 
16  namespace cg = carla::geom;
17  using namespace constants::Map;
18 
19  using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
20  using RawNodeList = std::vector<WaypointPtr>;
21 
22  InMemoryMap::InMemoryMap(WorldMap world_map) : _world_map(world_map) {}
24 
26  return std::make_tuple(wp->GetRoadId(), wp->GetLaneId(), wp->GetSectionId());
27  }
28 
30  return GetSegmentId(swp->GetWaypoint());
31  }
32 
34  const SegmentTopology &segment_topology,
35  const SegmentMap &segment_map) {
36  NodeList result;
37  if (segment_topology.find(segment_id) == segment_topology.end()) {
38  return result;
39  }
40 
41  for (const auto &successor_segment_id : segment_topology.at(segment_id).second) {
42  if (segment_map.find(successor_segment_id) == segment_map.end()) {
43  auto successors = GetSuccessors(successor_segment_id, segment_topology, segment_map);
44  result.insert(result.end(), successors.begin(), successors.end());
45  } else {
46  result.emplace_back(segment_map.at(successor_segment_id).front());
47  }
48  }
49  return result;
50  }
51 
53  const SegmentTopology &segment_topology,
54  const SegmentMap &segment_map) {
55  NodeList result;
56  if (segment_topology.find(segment_id) == segment_topology.end()) {
57  return result;
58  }
59 
60  for (const auto &predecessor_segment_id : segment_topology.at(segment_id).first) {
61  if (segment_map.find(predecessor_segment_id) == segment_map.end()) {
62  auto predecessors = GetPredecessors(predecessor_segment_id, segment_topology, segment_map);
63  result.insert(result.end(), predecessors.begin(), predecessors.end());
64  } else {
65  result.emplace_back(segment_map.at(predecessor_segment_id).back());
66  }
67  }
68  return result;
69  }
70 
71  void InMemoryMap::Cook(WorldMap world_map, const std::string& path) {
72  InMemoryMap local_map(world_map);
73  local_map.SetUp();
74  local_map.Save(path);
75  }
76 
77  void InMemoryMap::Save(const std::string& path) {
78  std::string filename;
79  if (path.empty()) {
80  filename = this->GetMapName() + ".bin";
81  } else {
82  filename = path;
83  }
84 
85  std::ofstream out_file;
86  out_file.open(filename, std::ios::binary);
87  if (!out_file.is_open()) {
88  log_error("Could not open binary file");
89  return;
90  }
91 
92  // write total records
93  uint32_t total = static_cast<uint32_t>(dense_topology.size());
94  out_file.write(reinterpret_cast<const char *>(&total), sizeof(uint32_t));
95 
96  // write simple waypoints
97  std::unordered_set<uint64_t> used_ids;
98  for (auto& wp: dense_topology) {
99  if (used_ids.find(wp->GetId()) != used_ids.end()) {
100  log_error("Could not generate the binary file. There are repeated waypoints");
101  }
102  CachedSimpleWaypoint cached_wp(wp);
103  cached_wp.Write(out_file);
104 
105  used_ids.insert(wp->GetId());
106  }
107 
108  out_file.close();
109  return;
110  }
111 
112  bool InMemoryMap::Load(const std::vector<uint8_t>& content) {
113  unsigned long pos = 0;
114  std::vector<CachedSimpleWaypoint> cached_waypoints;
115  std::unordered_map<uint64_t, uint32_t> id2index;
116 
117  // read total records
118  uint32_t total;
119  memcpy(&total, &content[pos], sizeof(total));
120  pos += sizeof(total);
121 
122  // read simple waypoints
123  for (uint32_t i=0; i < total; i++) {
124  CachedSimpleWaypoint cached_wp;
125  cached_wp.Read(content, pos);
126  cached_waypoints.push_back(cached_wp);
127  id2index.insert({cached_wp.waypoint_id, i});
128 
129  WaypointPtr waypoint_ptr = _world_map->GetWaypointXODR(cached_wp.road_id, cached_wp.lane_id, cached_wp.s);
130  SimpleWaypointPtr wp = std::make_shared<SimpleWaypoint>(waypoint_ptr);
131  wp->SetGeodesicGridId(cached_wp.geodesic_grid_id);
132  wp->SetIsJunction(cached_wp.is_junction);
133  wp->SetRoadOption(static_cast<RoadOption>(cached_wp.road_option));
134  dense_topology.push_back(wp);
135  }
136 
137  // connect waypoints
138  for (uint32_t i=0; i < dense_topology.size(); i++) {
139  auto wp = dense_topology.at(i);
140  auto cached_wp = cached_waypoints.at(i);
141 
142  std::vector<SimpleWaypointPtr> next_waypoints;
143  for (auto id : cached_wp.next_waypoints) {
144  next_waypoints.push_back(dense_topology.at(id2index.at(id)));
145  }
146  std::vector<SimpleWaypointPtr> previous_waypoints;
147  for (auto id : cached_wp.previous_waypoints) {
148  previous_waypoints.push_back(dense_topology.at(id2index.at(id)));
149  }
150  wp->SetNextWaypoint(next_waypoints);
151  wp->SetPreviousWaypoint(previous_waypoints);
152  if (cached_wp.next_left_waypoint > 0) {
153  wp->SetLeftWaypoint(dense_topology.at(id2index.at(cached_wp.next_left_waypoint)));
154  }
155  if (cached_wp.next_right_waypoint > 0) {
156  wp->SetRightWaypoint(dense_topology.at(id2index.at(cached_wp.next_right_waypoint)));
157  }
158  }
159 
160  // create spatial tree
162 
163  return true;
164  }
165 
167 
168  // 1. Building segment topology (i.e., defining set of segment predecessors and successors)
169  assert(_world_map != nullptr && "No map reference found.");
170  auto waypoint_topology = _world_map->GetTopology();
171 
172  SegmentTopology segment_topology;
173  std::unordered_map<int64_t, std::pair<std::set<crd::RoadId>, std::set<crd::RoadId>>> std_road_connectivity;
174  std::unordered_map<crd::RoadId, bool> is_real_junction;
175 
176  for (auto &connection : waypoint_topology) {
177  auto &waypoint = connection.first;
178  auto &successor = connection.second;
179 
180  // Setting segment predecessors and successors.
181  SegmentId waypoint_segment_id = GetSegmentId(connection.first);
182  SegmentId successor_segment_id = GetSegmentId(connection.second);
183  using SegIdVectorPair = std::pair<std::vector<SegmentId>, std::vector<SegmentId>>;
184  SegIdVectorPair &connection_first = segment_topology[waypoint_segment_id];
185  SegIdVectorPair &connection_second = segment_topology[successor_segment_id];
186  connection_first.second.push_back(successor_segment_id);
187  connection_second.first.push_back(waypoint_segment_id);
188 
189  // From path to standard road.
190  bool waypoint_is_junction = waypoint->IsJunction();
191  bool successor_is_junction = successor->IsJunction();
192  if (waypoint_is_junction && !successor_is_junction) {
193  crd::RoadId path_id = waypoint->GetRoadId();
194  int64_t std_road_id = static_cast<int64_t>(successor->GetRoadId());
195  std_road_id = (successor->GetLaneId() < 0) ? -1 * std_road_id : std_road_id;
196 
197  std::set<crd::RoadId> &in_paths = std_road_connectivity[std_road_id].first;
198  in_paths.insert(path_id);
199 
200  if (in_paths.size() >= 2) {
201  for (auto &in_path_id: in_paths) {
202  is_real_junction[in_path_id] = true;
203  }
204  }
205  }
206 
207  // From standard road to path.
208  if (!waypoint_is_junction && successor_is_junction) {
209  crd::RoadId path_id = successor->GetRoadId();
210  int64_t std_road_id = static_cast<int64_t>(waypoint->GetRoadId());
211  std_road_id = (waypoint->GetLaneId() < 0) ? -1 * std_road_id : std_road_id;
212 
213  std::set<crd::RoadId> &out_paths = std_road_connectivity[std_road_id].second;
214  out_paths.insert(path_id);
215 
216  if (out_paths.size() >= 2) {
217  for (auto &out_path_id: out_paths) {
218  is_real_junction[out_path_id] = true;
219  }
220  }
221  }
222  }
223 
224  // 2. Consuming the raw dense topology from cc::Map into SimpleWaypoints.
225  SegmentMap segment_map;
226  assert(_world_map != nullptr && "No map reference found.");
227  auto raw_dense_topology = _world_map->GenerateWaypoints(MAP_RESOLUTION);
228  for (auto &waypoint_ptr: raw_dense_topology) {
229  segment_map[GetSegmentId(waypoint_ptr)].emplace_back(std::make_shared<SimpleWaypoint>(waypoint_ptr));
230  }
231 
232  // 3. Processing waypoints.
233  auto distance_squared = [](cg::Location l1, cg::Location l2) {
234  return cg::Math::DistanceSquared(l1, l2);
235  };
236  auto square = [](float input) {return std::pow(input, 2);};
237  auto compare_s = [](const SimpleWaypointPtr &swp1, const SimpleWaypointPtr &swp2) {
238  return (swp1->GetWaypoint()->GetDistance() < swp2->GetWaypoint()->GetDistance());
239  };
240  auto wpt_angle = [](cg::Vector3D l1, cg::Vector3D l2) {
241  return cg::Math::GetVectorAngle(l1, l2);
242  };
243  auto max = [](int16_t x, int16_t y) {
244  return x ^ ((x ^ y) & -(x < y));
245  };
246 
247  GeoGridId geodesic_grid_id_counter = -1;
248  for (auto &segment: segment_map) {
249  auto &segment_waypoints = segment.second;
250 
251  // Generating geodesic grid ids.
252  ++geodesic_grid_id_counter;
253 
254  // Ordering waypoints according to road direction.
255  std::sort(segment_waypoints.begin(), segment_waypoints.end(), compare_s);
256  auto lane_id = segment_waypoints.front()->GetWaypoint()->GetLaneId();
257  if (lane_id > 0) {
258  std::reverse(segment_waypoints.begin(), segment_waypoints.end());
259  }
260 
261  // Adding more waypoints if the angle is too tight or if they are too distant.
262  for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
263  double distance = std::abs(segment_waypoints.at(i)->GetWaypoint()->GetDistance() - segment_waypoints.at(i+1)->GetWaypoint()->GetDistance());
264  double angle = wpt_angle(segment_waypoints.at(i)->GetTransform().GetForwardVector(), segment_waypoints.at(i+1)->GetTransform().GetForwardVector());
265  int16_t angle_splits = static_cast<int16_t>(angle/MAX_WPT_RADIANS);
266  int16_t distance_splits = static_cast<int16_t>((distance*distance)/MAX_WPT_DISTANCE);
267  auto max_splits = max(angle_splits, distance_splits);
268  if (max_splits >= 1) {
269  // Compute how many waypoints do we need to generate.
270  for (uint16_t j = 0; j < max_splits; ++j) {
271  auto next_waypoints = segment_waypoints.at(i)->GetWaypoint()->GetNext(distance/(max_splits+1));
272  if (next_waypoints.size() != 0) {
273  auto new_waypoint = next_waypoints.front();
274  i++;
275  segment_waypoints.insert(segment_waypoints.begin()+static_cast<int64_t>(i), std::make_shared<SimpleWaypoint>(new_waypoint));
276  } else {
277  // Reached end of the road.
278  break;
279  }
280  }
281  }
282  }
283 
284  // Placing intra-segment connections.
285  cg::Location grid_edge_location = segment_waypoints.front()->GetLocation();
286  for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
287  SimpleWaypointPtr current_waypoint = segment_waypoints.at(i);
288  SimpleWaypointPtr next_waypoint = segment_waypoints.at(i+1);
289  // Assigning grid id.
290  if (distance_squared(grid_edge_location, current_waypoint->GetLocation()) >
291  square(MAX_GEODESIC_GRID_LENGTH)) {
292  ++geodesic_grid_id_counter;
293  grid_edge_location = current_waypoint->GetLocation();
294  }
295  current_waypoint->SetGeodesicGridId(geodesic_grid_id_counter);
296 
297  current_waypoint->SetNextWaypoint({next_waypoint});
298  next_waypoint->SetPreviousWaypoint({current_waypoint});
299 
300  }
301  segment_waypoints.back()->SetGeodesicGridId(geodesic_grid_id_counter);
302 
303  // Adding simple waypoints to processed dense topology.
304  for (auto swp: segment_waypoints) {
305  // Checking whether the waypoint is in a real junction.
306  auto wpt = swp->GetWaypoint();
307  auto road_id = wpt->GetRoadId();
308  if (wpt->IsJunction() && !is_real_junction.count(road_id)) {
309  swp->SetIsJunction(false);
310  } else {
311  swp->SetIsJunction(swp->GetWaypoint()->IsJunction());
312  }
313 
314  dense_topology.push_back(swp);
315  }
316  }
317 
319 
320  // Placing inter-segment connections.
321  for (auto &segment : segment_map) {
322  SegmentId segment_id = segment.first;
323  auto &segment_waypoints = segment.second;
324 
325  auto successors = GetSuccessors(segment_id, segment_topology, segment_map);
326  auto predecessors = GetPredecessors(segment_id, segment_topology, segment_map);
327 
328  segment_waypoints.front()->SetPreviousWaypoint(predecessors);
329  segment_waypoints.back()->SetNextWaypoint(successors);
330  }
331 
332  // Linking lane change connections.
333  for (auto &swp : dense_topology) {
334  if (!swp->CheckJunction()) {
336  }
337  }
338 
339  // Linking any unconnected segments.
340  for (auto &swp : dense_topology) {
341  if (swp->GetNextWaypoint().empty()) {
342  auto neighbour = swp->GetRightWaypoint();
343  if (!neighbour) {
344  neighbour = swp->GetLeftWaypoint();
345  }
346 
347  if (neighbour) {
348  swp->SetNextWaypoint(neighbour->GetNextWaypoint());
349  for (auto next_waypoint : neighbour->GetNextWaypoint()) {
350  next_waypoint->SetPreviousWaypoint({swp});
351  }
352  }
353  }
354  }
355 
356  // Specifying a RoadOption for each SimpleWaypoint
357  SetUpRoadOption();
358  }
359 
361  for (auto &simple_waypoint: dense_topology) {
362  if (simple_waypoint != nullptr) {
363  const cg::Location loc = simple_waypoint->GetLocation();
364  Point3D point(loc.x, loc.y, loc.z);
365  rtree.insert(std::make_pair(point, simple_waypoint));
366  }
367  }
368  }
369 
371  for (auto &swp : dense_topology) {
372  std::vector<SimpleWaypointPtr> next_waypoints = swp->GetNextWaypoint();
373  std::size_t next_swp_size = next_waypoints.size();
374 
375  if (next_swp_size == 0) {
376  // No next waypoint means that this is an end of the road.
377  swp->SetRoadOption(RoadOption::RoadEnd);
378  }
379 
380  else if (next_swp_size > 1 || (!swp->CheckJunction() && next_waypoints.front()->CheckJunction())) {
381  // To check if we are in an actual junction, and not on an highway, we try to see
382  // if there's a landmark nearby of type Traffic Light, Stop Sign or Yield Sign.
383 
384  bool found_landmark= false;
385  if (next_swp_size <= 1) {
386 
387  auto all_landmarks = swp->GetWaypoint()->GetAllLandmarksInDistance(15.0);
388 
389  if (all_landmarks.empty()) {
390  // Landmark hasn't been found, this isn't a junction.
391  swp->SetRoadOption(RoadOption::LaneFollow);
392  } else {
393  for (auto &landmark : all_landmarks) {
394  auto landmark_type = landmark->GetType();
395  if (landmark_type == "1000001" || landmark_type == "206" || landmark_type == "205") {
396  // We found a landmark.
397  found_landmark= true;
398  break;
399  }
400  }
401  if (!found_landmark) {
402  swp->SetRoadOption(RoadOption::LaneFollow);
403  }
404  }
405  }
406 
407  // If we did find a landmark, or we are in the other case, find all waypoints
408  // in the junction and assign the correct RoadOption.
409  if (found_landmark || next_swp_size > 1) {
410  swp->SetRoadOption(RoadOption::LaneFollow);
411  for (auto &next_swp : next_waypoints) {
412  std::vector<SimpleWaypointPtr> traversed_waypoints;
413  SimpleWaypointPtr junction_end_waypoint;
414 
415  if (next_swp_size > 1) {
416  junction_end_waypoint = next_swp;
417  } else {
418  junction_end_waypoint = next_waypoints.front();
419  }
420 
421  while (junction_end_waypoint->CheckJunction()){
422  traversed_waypoints.push_back(junction_end_waypoint);
423  std::vector<SimpleWaypointPtr> temp = junction_end_waypoint->GetNextWaypoint();
424  if (temp.empty()) {
425  break;
426  }
427  junction_end_waypoint = temp.front();
428  }
429 
430  // Calculate the angle between the first and the last point of the junction.
431  int16_t current_angle = static_cast<int16_t>(traversed_waypoints.front()->GetTransform().rotation.yaw);
432  int16_t junction_end_angle = static_cast<int16_t>(traversed_waypoints.back()->GetTransform().rotation.yaw);
433  int16_t diff_angle = (junction_end_angle - current_angle) % 360;
434  bool straight = (diff_angle < STRAIGHT_DEG && diff_angle > -STRAIGHT_DEG) ||
435  (diff_angle > 360-STRAIGHT_DEG && diff_angle <= 360) ||
436  (diff_angle < -360+STRAIGHT_DEG && diff_angle >= -360);
437  bool right = (diff_angle >= STRAIGHT_DEG && diff_angle <= 180) ||
438  (diff_angle <= -180 && diff_angle >= -360+STRAIGHT_DEG);
439 
440  auto assign_option = [](RoadOption ro, std::vector<SimpleWaypointPtr> traversed_waypoints) {
441  for (auto &twp : traversed_waypoints) {
442  twp->SetRoadOption(ro);
443  }
444  };
445 
446  // Assign RoadOption according to the angle.
447  if (straight) assign_option(RoadOption::Straight, traversed_waypoints);
448  else if (right) assign_option(RoadOption::Right, traversed_waypoints);
449  else assign_option(RoadOption::Left, traversed_waypoints);
450  }
451  }
452  }
453  else if (next_swp_size == 1 && swp->GetRoadOption() == RoadOption::Void) {
454  swp->SetRoadOption(RoadOption::LaneFollow);
455  }
456  }
457  }
458 
460 
461  Point3D query_point(loc.x, loc.y, loc.z);
462  std::vector<SpatialTreeEntry> result_1;
463 
464  rtree.query(bgi::nearest(query_point, 1), std::back_inserter(result_1));
465 
466  SpatialTreeEntry &closest_entry = result_1.front();
467  SimpleWaypointPtr &closest_point = closest_entry.second;
468 
469  return closest_point;
470  }
471 
472  NodeList InMemoryMap::GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const {
473  Point3D query_point(loc.x, loc.y, loc.z);
474 
475  Point3D lower_p1(loc.x + random_sample, loc.y + random_sample, loc.z + Z_DELTA);
476  Point3D lower_p2(loc.x - random_sample, loc.y - random_sample, loc.z - Z_DELTA);
477  Point3D upper_p1(loc.x + random_sample + DELTA, loc.y + random_sample + DELTA, loc.z + Z_DELTA);
478  Point3D upper_p2(loc.x - random_sample - DELTA, loc.y - random_sample - DELTA, loc.z - Z_DELTA);
479 
480  Box lower_query_box(lower_p2, lower_p1);
481  Box upper_query_box(upper_p2, upper_p1);
482 
483  NodeList result;
484  uint8_t x = 0;
485  for (Rtree::const_query_iterator
486  it = rtree.qbegin(bgi::within(upper_query_box)
487  && !bgi::within(lower_query_box)
488  && bgi::satisfies([&](SpatialTreeEntry const& v) { return !v.second->CheckJunction();}));
489  it != rtree.qend();
490  ++it) {
491  x++;
492  result.push_back(it->second);
493  if (x >= n_points)
494  break;
495  }
496 
497  return result;
498  }
499 
501  return dense_topology;
502  }
503 
505 
506  const WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
507  const crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange();
508 
509  /// Cheack for transits
510  switch(lane_change)
511  {
512  /// Left transit way point present only
513  case crd::element::LaneMarking::LaneChange::Left:
514  {
515  const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
516  if (left_waypoint != nullptr &&
517  left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
518  (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
519 
520  SimpleWaypointPtr closest_simple_waypoint = GetWaypoint(left_waypoint->GetTransform().location);
521  reference_waypoint->SetLeftWaypoint(closest_simple_waypoint);
522  }
523  }
524  break;
525 
526  /// Right transit way point present only
527  case crd::element::LaneMarking::LaneChange::Right:
528  {
529  const WaypointPtr right_waypoint = raw_waypoint->GetRight();
530  if(right_waypoint != nullptr &&
531  right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
532  (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
533 
534  SimpleWaypointPtr closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location);
535  reference_waypoint->SetRightWaypoint(closest_simple_waypoint);
536  }
537  }
538  break;
539 
540  /// Both left and right transit present
542  {
543  /// Right transit way point
544  const WaypointPtr right_waypoint = raw_waypoint->GetRight();
545  if (right_waypoint != nullptr &&
546  right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
547  (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
548 
549  SimpleWaypointPtr closest_simple_waypointR = GetWaypoint(right_waypoint->GetTransform().location);
550  reference_waypoint->SetRightWaypoint(closest_simple_waypointR);
551  }
552 
553  /// Left transit way point
554  const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
555  if (left_waypoint != nullptr &&
556  left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
557  (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
558 
559  SimpleWaypointPtr closest_simple_waypointL = GetWaypoint(left_waypoint->GetTransform().location);
560  reference_waypoint->SetLeftWaypoint(closest_simple_waypointL);
561  }
562  }
563  break;
564 
565  /// For no transit waypoint (left or right)
566  default: break;
567  }
568  }
569 
570  std::string InMemoryMap::GetMapName() {
571  assert(_world_map != nullptr && "No map reference found.");
572  return _world_map->GetName();
573  }
574 
575  const cc::Map& InMemoryMap::GetMap() const {
576  return *_world_map;
577  }
578 
579 
580 } // namespace traffic_manager
581 } // 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.
uint32_t RoadId
Definition: RoadTypes.h:15
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
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.
static void log_error(Args &&... args)
Definition: Logging.h:110
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.
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:70
std::vector< std::pair< WaypointPtr, WaypointPtr > > TopologyList
Definition: InMemoryMap.cpp:19
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
static const float MAX_GEODESIC_GRID_LENGTH
Definition: Constants.h:100
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
void Read(const std::vector< uint8_t > &content, unsigned long &start)
bool Load(const std::vector< uint8_t > &content)
static double GetVectorAngle(const Vector3D &a, const Vector3D &b)
Returns the angle between 2 vectors in radians.
Definition: Math.cpp:14
std::vector< WaypointPtr > RawNodeList
Definition: InMemoryMap.cpp:20
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
void sort(I begin, I end, const Pred &pred)
Definition: pugixml.cpp:7444
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