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  auto waypoint = buffer.at(std::min(i * step_size, buffer_size - 1u));
62  GeoGridId ggid = waypoint->GetGeodesicGridId();
63  current_grids.insert(ggid);
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 
79 
80 bool TrackTraffic::IsGeoGridFree(const GeoGridId geogrid_id) const {
81  if (grid_to_actors.find(geogrid_id) != grid_to_actors.end()) {
82  return grid_to_actors.at(geogrid_id).empty();
83  }
84  return true;
85 }
86 
87 void TrackTraffic::AddTakenGrid(const GeoGridId geogrid_id, const ActorId actor_id) {
88  if (grid_to_actors.find(geogrid_id) == grid_to_actors.end()) {
89  grid_to_actors.insert({geogrid_id, {actor_id}});
90  }
91 }
92 
93 
95  hero_location = _location;
96 }
97 
99  return hero_location;
100 }
101 
103  ActorIdSet actor_id_set;
104 
105  if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
106  const std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
107  for (auto &grid_id : grid_ids) {
108  if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
109  const ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
110  actor_id_set.insert(actor_ids.begin(), actor_ids.end());
111  }
112  }
113  }
114 
115  return actor_id_set;
116 }
117 
119  if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
120  std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
121  for (auto &grid_id : grid_ids) {
122  if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
123  ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
124  actor_ids.erase(actor_id);
125  }
126  }
127  actor_to_grids.erase(actor_id);
128  }
129 
130  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
131  WaypointIdSet waypoint_id_set = waypoint_occupied.at(actor_id);
132  for (const uint64_t &waypoint_id : waypoint_id_set) {
133  RemovePassingVehicle(waypoint_id, actor_id);
134  }
135  }
136 }
137 
138 void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
139  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
140  ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
141  if (actor_id_set.find(actor_id) == actor_id_set.end()) {
142  actor_id_set.insert(actor_id);
143  }
144  } else {
145  waypoint_overlap_tracker.insert({waypoint_id, {actor_id}});
146  }
147 
148  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
149  WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
150  if (waypoint_id_set.find(waypoint_id) == waypoint_id_set.end()) {
151  waypoint_id_set.insert(waypoint_id);
152  }
153  } else {
154  waypoint_occupied.insert({actor_id, {waypoint_id}});
155  }
156 }
157 
158 void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
159  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
160  ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
161  actor_id_set.erase(actor_id);
162 
163  if (actor_id_set.size() == 0) {
164  waypoint_overlap_tracker.erase(waypoint_id);
165  }
166  }
167 
168  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
169  WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
170  waypoint_id_set.erase(waypoint_id);
171 
172  if (waypoint_id_set.size() == 0) {
173  waypoint_occupied.erase(actor_id);
174  }
175  }
176 }
177 
178 ActorIdSet TrackTraffic::GetPassingVehicles(uint64_t waypoint_id) const {
179 
180  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
181  return waypoint_overlap_tracker.at(waypoint_id);
182  } else {
183  return ActorIdSet();
184  }
185 }
186 
188  waypoint_overlap_tracker.clear();
189  waypoint_occupied.clear();
190  actor_to_grids.clear();
191  grid_to_actors.clear();
192 }
193 
194 } // namespace traffic_manager
195 } // namespace carla
cg::Location hero_location
Current hero location.
Definition: TrackTraffic.h:36
std::unordered_set< uint64_t > WaypointIdSet
Structure to keep track of waypoints occupied by vehicles;.
Definition: TrackTraffic.h:27
void SetHeroLocation(const cg::Location location)
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:133
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:41
void DeleteActor(ActorId actor_id)
Method to delete actor data from tracking.
WaypointOccupancyMap waypoint_occupied
Definition: TrackTraffic.h:29
void AddTakenGrid(const GeoGridId geogrid_id, const ActorId actor_id)
cg::Location GetHeroLocation() const
bool IsGeoGridFree(const GeoGridId geogrid_id) const
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