CARLA
WalkerManager.cpp
Go to the documentation of this file.
1 // Copyright (c) 2019 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 
8 #include "carla/nav/Navigation.h"
9 
10 namespace carla {
11 namespace nav {
12 
14  _nav = nullptr;
15  }
16 
18  }
19 
20  // create a new walker route
22  WalkerInfo info;
23  info.state = WALKER_IDLE;
24 
25  // save it
26  _walkers[id] = std::move(info);
27 
28  return true;
29  }
30 
31  // remove a walker route
33  // search
34  auto it = _walkers.find(id);
35  if (it == _walkers.end())
36  return false;
37  _walkers.erase(it);
38 
39  return true;
40  }
41 
42  // update all routes
43  bool WalkerManager::Update(double delta) {
44 
45  // check all walkers
46  for (auto &it : _walkers) {
47 
48  // get the elements
49  WalkerInfo &info = it.second;
50 
51  // in function of the state
52  switch (info.state) {
53  case WALKER_IDLE:
54  break;
55 
56  case WALKER_WALKING:
57  {
58  // get the target point
59  carla::geom::Location &target = info.route[info.currentIndex].location;
60  // get current position
61  carla::geom::Location current;
62  _nav->GetWalkerPosition(it.first, current);
63  // check distance to the target point
64  carla::geom::Vector3D dist(target.x - current.x, target.z - current.z, target.y - current.y);
65  if (dist.SquaredLength() <= 4) {
66  info.state = WALKER_IN_EVENT;
67  _nav->PauseAgent(it.first, true);
68  }
69  }
70  break;
71 
72  case WALKER_IN_EVENT:
73  switch (ExecuteEvent(it.first, it.second, delta)) {
75  break;
76  case EventResult::End:
77  // next point in route
78  SetWalkerNextPoint(it.first);
79  break;
81  // unblock changing the route
82  SetWalkerRoute(it.first);
83  break;
84  }
85  break;
86 
87  case WALKER_STOP:
88  info.state = WALKER_IDLE;
89  break;
90  }
91  }
92 
93  return true;
94  }
95 
96  // set a new route from its current position
98  // check
99  if (_nav == nullptr)
100  return false;
101 
102  // set a new random target
103  carla::geom::Location location;
104  _nav->GetRandomLocation(location, nullptr);
105 
106  // set the route
107  return SetWalkerRoute(id, location);
108  }
109 
110  // set a new route from its current position
112  // check
113  if (_nav == nullptr)
114  return false;
115 
116  // search
117  auto it = _walkers.find(id);
118  if (it == _walkers.end())
119  return false;
120 
121  // get it
122  WalkerInfo &info = it->second;
123  std::vector<carla::geom::Location> path;
124  std::vector<unsigned char> area;
125 
126  // save both points for the route
127  _nav->GetWalkerPosition(id, info.from);
128  info.to = to;
129  info.currentIndex = 0;
130  info.state = WALKER_IDLE;
131 
132  // get a route from navigation
133  _nav->GetAgentRoute(id, info.from, to, path, area);
134 
135  // create each point of the route
136  info.route.clear();
137  info.route.reserve(path.size());
138  unsigned char previous_area = CARLA_AREA_SIDEWALK;
139  for (unsigned int i=0; i<path.size(); ++i) {
140  // get the type
141  switch (area[i]) {
142  // do nothing
143  case CARLA_AREA_SIDEWALK:
144  info.route.emplace_back(WalkerEventIgnore(), std::move(path[i]), area[i]);
145  break;
146 
147  // stop and check
148  case CARLA_AREA_ROAD:
150  // only if we come from a safe area (sidewalks, grass or crosswalk)
151  if (previous_area != CARLA_AREA_CROSSWALK && previous_area != CARLA_AREA_ROAD)
152  info.route.emplace_back(WalkerEventStopAndCheck(5), std::move(path[i]), area[i]);
153  break;
154 
155  default:
156  info.route.emplace_back(WalkerEventIgnore(), std::move(path[i]), area[i]);
157  }
158  previous_area = area[i];
159  }
160 
161  // assign the first point to go (second in the list)
162  SetWalkerNextPoint(id);
163  return true;
164  }
165 
166  // set the next point in the route
168  // check
169  if (_nav == nullptr)
170  return false;
171 
172  // search
173  auto it = _walkers.find(id);
174  if (it == _walkers.end())
175  return false;
176 
177  // get it
178  WalkerInfo &info = it->second;
179 
180  // advance next point
181  ++info.currentIndex;
182 
183  // check the end
184  if (info.currentIndex < info.route.size()) {
185  // change the state
186  info.state = WALKER_WALKING;
187  // assign the point to go
188  _nav->PauseAgent(id, false);
189  _nav->SetWalkerDirectTarget(id, info.route[info.currentIndex].location);
190  } else {
191  // change the state
192  info.state = WALKER_STOP;
193  _nav->PauseAgent(id, true);
194  // we need a new route from here
195  SetWalkerRoute(id);
196  }
197 
198  return true;
199  }
200 
201  // get the next point in the route
203  // check
204  if (_nav == nullptr)
205  return false;
206 
207  // search
208  auto it = _walkers.find(id);
209  if (it == _walkers.end())
210  return false;
211 
212  // get it
213  WalkerInfo &info = it->second;
214 
215  // check the end
216  if (info.currentIndex < info.route.size()) {
217  location = info.route[info.currentIndex].location;
218  return true;
219  } else {
220  return false;
221  }
222  }
223 
225  // check
226  if (_nav == nullptr)
227  return false;
228 
229  // search
230  auto it = _walkers.find(id);
231  if (it == _walkers.end())
232  return false;
233 
234  // get it
235  WalkerInfo &info = it->second;
236 
237  // check the end of current crosswalk
238  unsigned int pos = info.currentIndex;
239  while (pos < info.route.size()) {
240  if (info.route[pos].areaType != CARLA_AREA_CROSSWALK) {
241  location = info.route[pos].location;
242  return true;
243  }
244  ++pos;
245  }
246 
247  return false;
248  }
249 
251  // go to the event
252  WalkerRoutePoint &rp = info.route[info.currentIndex];
253 
254  // build the visitor structure
255  WalkerEventVisitor visitor(this, id, delta);
256  // run the event
257  return boost::apply_visitor(visitor, rp.event);
258  }
259 
260 
261 } // namespace nav
262 } // namespace carla
std::vector< WalkerRoutePoint > route
Definition: WalkerManager.h:39
bool GetWalkerNextPoint(ActorId id, carla::geom::Location &location)
get the next point in the route
bool GetWalkerPosition(ActorId id, carla::geom::Location &location)
get the walker current location
Definition: Navigation.cpp:974
rpc::ActorId ActorId
Definition: ActorId.h:18
float SquaredLength() const
Definition: geom/Vector3D.h:45
void PauseAgent(ActorId id, bool pause)
set an agent as paused for the crowd
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
EventResult
result of an event
Definition: WalkerEvent.h:20
bool RemoveWalker(ActorId id)
remove a walker route
bool SetWalkerNextPoint(ActorId id)
set the next point in the route
carla::geom::Location from
Definition: WalkerManager.h:35
event to pause and check for near vehicles
Definition: WalkerEvent.h:37
bool SetWalkerDirectTarget(ActorId id, carla::geom::Location to)
Definition: Navigation.cpp:770
std::unordered_map< ActorId, WalkerInfo > _walkers
Definition: WalkerManager.h:81
EventResult ExecuteEvent(ActorId id, WalkerInfo &info, double delta)
bool GetWalkerCrosswalkEnd(ActorId id, carla::geom::Location &location)
get the point in the route that end current crosswalk
bool GetAgentRoute(ActorId id, carla::geom::Location from, carla::geom::Location to, std::vector< carla::geom::Location > &path, std::vector< unsigned char > &area)
Definition: Navigation.cpp:360
bool GetRandomLocation(carla::geom::Location &location, dtQueryFilter *filter=nullptr) const
get a random location for navigation
bool Update(double delta)
update all routes
bool SetWalkerRoute(ActorId id)
set a new route from its current position
carla::geom::Location to
Definition: WalkerManager.h:36
empty event that just ignores
Definition: WalkerEvent.h:27
bool AddWalker(ActorId id)
create a new walker route
unsigned int currentIndex
Definition: WalkerManager.h:37