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  for (uint64_t i = 0u; i <= buffer_size - 1u; ++i) {
60  auto waypoint = buffer.at(i);
61  GeoGridId ggid = waypoint->GetGeodesicGridId();
62  current_grids.insert(ggid);
63  // Add grid entry if not present.
64  if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
65  grid_to_actors.insert({ggid, {}});
66  }
67 
68  ActorIdSet &actor_ids = grid_to_actors.at(ggid);
69  if (actor_ids.find(actor_id) == actor_ids.end()) {
70  actor_ids.insert(actor_id);
71  }
72  }
73 
74  actor_to_grids.insert({actor_id, current_grids});
75  }
76 }
77 
78 
79 bool TrackTraffic::IsGeoGridFree(const GeoGridId geogrid_id) const {
80  if (grid_to_actors.find(geogrid_id) != grid_to_actors.end()) {
81  return grid_to_actors.at(geogrid_id).empty();
82  }
83  return true;
84 }
85 
86 void TrackTraffic::AddTakenGrid(const GeoGridId geogrid_id, const ActorId actor_id) {
87  if (grid_to_actors.find(geogrid_id) == grid_to_actors.end()) {
88  grid_to_actors.insert({geogrid_id, {actor_id}});
89  }
90 }
91 
92 
94  hero_location = _location;
95 }
96 
98  return hero_location;
99 }
100 
102  ActorIdSet actor_id_set;
103 
104  if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
105  const std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
106  for (auto &grid_id : grid_ids) {
107  if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
108  const ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
109  actor_id_set.insert(actor_ids.begin(), actor_ids.end());
110  }
111  }
112  }
113 
114  return actor_id_set;
115 }
116 
118  if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
119  std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
120  for (auto &grid_id : grid_ids) {
121  if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
122  ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
123  actor_ids.erase(actor_id);
124  }
125  }
126  actor_to_grids.erase(actor_id);
127  }
128 
129  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
130  WaypointIdSet waypoint_id_set = waypoint_occupied.at(actor_id);
131  for (const uint64_t &waypoint_id : waypoint_id_set) {
132  RemovePassingVehicle(waypoint_id, actor_id);
133  }
134  }
135 }
136 
137 void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
138  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
139  ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
140  if (actor_id_set.find(actor_id) == actor_id_set.end()) {
141  actor_id_set.insert(actor_id);
142  }
143  } else {
144  waypoint_overlap_tracker.insert({waypoint_id, {actor_id}});
145  }
146 
147  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
148  WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
149  if (waypoint_id_set.find(waypoint_id) == waypoint_id_set.end()) {
150  waypoint_id_set.insert(waypoint_id);
151  }
152  } else {
153  waypoint_occupied.insert({actor_id, {waypoint_id}});
154  }
155 }
156 
157 void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
158  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
159  ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
160  actor_id_set.erase(actor_id);
161 
162  if (actor_id_set.size() == 0) {
163  waypoint_overlap_tracker.erase(waypoint_id);
164  }
165  }
166 
167  if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
168  WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
169  waypoint_id_set.erase(waypoint_id);
170 
171  if (waypoint_id_set.size() == 0) {
172  waypoint_occupied.erase(actor_id);
173  }
174  }
175 }
176 
177 ActorIdSet TrackTraffic::GetPassingVehicles(uint64_t waypoint_id) const {
178 
179  if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
180  return waypoint_overlap_tracker.at(waypoint_id);
181  } else {
182  return ActorIdSet();
183  }
184 }
185 
187  waypoint_overlap_tracker.clear();
188  waypoint_occupied.clear();
189  actor_to_grids.clear();
190  grid_to_actors.clear();
191 }
192 
193 } // namespace traffic_manager
194 } // 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