CARLA
TrackTraffic.cpp
Go to the documentation of this file.
1 
3 
5 
6 namespace carla {
7 namespace traffic_manager {
8 
11 
13 
15  const std::vector<SimpleWaypointPtr> waypoints) {
16 
17  DeleteActor(actor_id);
18 
19  std::unordered_set<GeoGridId> current_grids;
20  // Step through waypoints and update grid list for actor and actor list for grids.
21  for (auto &waypoint : waypoints) {
22  UpdatePassingVehicle(waypoint->GetId(), actor_id);
23 
24  GeoGridId ggid = waypoint->GetGeodesicGridId();
25  current_grids.insert(ggid);
26 
27  if (grid_to_actors.find(ggid) != grid_to_actors.end()) {
28  ActorIdSet &actor_ids = grid_to_actors.at(ggid);
29  if (actor_ids.find(actor_id) == actor_ids.end()) {
30  actor_ids.insert(actor_id);
31  }
32  } else {
33  grid_to_actors.insert({ggid, {actor_id}});
34  }
35  }
36 
37  actor_to_grids.insert({actor_id, current_grids});
38 }
39 
40 void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buffer) {
41  if (!buffer.empty()) {
42 
43  // Clear current actor from all grids containing itself.
44  if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
45  std::unordered_set<GeoGridId> &current_grids = actor_to_grids.at(actor_id);
46  for (auto &grid_id : current_grids) {
47  if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
48  ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
49  actor_ids.erase(actor_id);
50  }
51  }
52 
53  actor_to_grids.erase(actor_id);
54  }
55 
56  // Step through buffer and update grid list for actor and actor list for grids.
57  std::unordered_set<GeoGridId> current_grids;
58  uint64_t buffer_size = buffer.size();
59  uint64_t step_size = static_cast<uint64_t>(static_cast<float>(buffer_size) * INV_BUFFER_STEP_THROUGH);
60  for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) {
61  GeoGridId ggid = buffer.at(std::min(i * step_size, buffer_size - 1u))->GetGeodesicGridId();
62  current_grids.insert(ggid);
63 
64  // Add grid entry if not present.
65  if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
66  grid_to_actors.insert({ggid, {}});
67  }
68 
69  ActorIdSet &actor_ids = grid_to_actors.at(ggid);
70  if (actor_ids.find(actor_id) == actor_ids.end()) {
71  actor_ids.insert(actor_id);
72  }
73  }
74 
75  actor_to_grids.insert({actor_id, current_grids});
76  }
77 }
78 
80  ActorIdSet actor_id_set;
81 
82  if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
83  const std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
84  for (auto &grid_id : grid_ids) {
85  if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
86  const ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
87  actor_id_set.insert(actor_ids.begin(), actor_ids.end());
88  }
89  }
90  }
91 
92  return actor_id_set;
93 }
94 
96  if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
97  std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
98  for (auto &grid_id : grid_ids) {
99  if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
100  ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
101  actor_ids.erase(actor_id);
102  }
103  }
104  actor_to_grids.erase(actor_id);
105  }
106 
107  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
108  WaypointIdSet waypoint_id_set = waypoint_occupied.at(actor_id);
109  for (const uint64_t &waypoint_id : waypoint_id_set) {
110  RemovePassingVehicle(waypoint_id, actor_id);
111  }
112  }
113 }
114 
115 void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
116  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
117  ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
118  if (actor_id_set.find(actor_id) == actor_id_set.end()) {
119  actor_id_set.insert(actor_id);
120  }
121  } else {
122  waypoint_overlap_tracker.insert({waypoint_id, {actor_id}});
123  }
124 
125  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
126  WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
127  if (waypoint_id_set.find(waypoint_id) == waypoint_id_set.end()) {
128  waypoint_id_set.insert(waypoint_id);
129  }
130  } else {
131  waypoint_occupied.insert({actor_id, {waypoint_id}});
132  }
133 }
134 
135 void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
136  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
137  ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
138  actor_id_set.erase(actor_id);
139 
140  if (actor_id_set.size() == 0) {
141  waypoint_overlap_tracker.erase(waypoint_id);
142  }
143  }
144 
145  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
146  WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
147  waypoint_id_set.erase(waypoint_id);
148 
149  if (waypoint_id_set.size() == 0) {
150  waypoint_occupied.erase(actor_id);
151  }
152  }
153 }
154 
155 ActorIdSet TrackTraffic::GetPassingVehicles(uint64_t waypoint_id) const {
156 
157  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
158  return waypoint_overlap_tracker.at(waypoint_id);
159  } else {
160  return ActorIdSet();
161  }
162 }
163 
165  waypoint_overlap_tracker.clear();
166  waypoint_occupied.clear();
167  actor_to_grids.clear();
168  grid_to_actors.clear();
169 }
170 
171 } // namespace traffic_manager
172 } // namespace carla
std::unordered_set< uint64_t > WaypointIdSet
Structure to keep track of waypoints occupied by vehicles;.
Definition: TrackTraffic.h:27
void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id)
Methods to update, remove and retrieve vehicles passing through a waypoint.
void UpdateGridPosition(const ActorId actor_id, const Buffer &buffer)
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
std::unordered_set< ActorId > ActorIdSet
std::deque< std::shared_ptr< SimpleWaypoint > > Buffer
void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id)
std::unordered_map< GeoGridId, ActorIdSet > grid_to_actors
Actors currently passing through grids.
Definition: TrackTraffic.h:34
void UpdateUnregisteredGridPosition(const ActorId actor_id, const std::vector< SimpleWaypointPtr > waypoints)
carla::ActorId ActorId
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const
crd::JuncId GeoGridId
Definition: InMemoryMap.h:38
void DeleteActor(ActorId actor_id)
Method to delete actor data from tracking.
WaypointOccupancyMap waypoint_occupied
Definition: TrackTraffic.h:29
ActorIdSet GetPassingVehicles(uint64_t waypoint_id) const
std::unordered_map< ActorId, std::unordered_set< GeoGridId > > actor_to_grids
Geodesic grids occupied by actors&#39;s paths.
Definition: TrackTraffic.h:32