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 
9 #include "carla/Logging.h"
11 #include "carla/client/Waypoint.h"
12 #include "carla/client/World.h"
14 #include "carla/nav/Navigation.h"
15 #include "carla/rpc/Actor.h"
16 
17 namespace carla {
18 namespace nav {
19 
21  }
22 
24  }
25 
26  // assign the navigation module
28  _nav = nav;
29  }
30 
31  // reference to the simulator to access API functions
32  void WalkerManager::SetSimulator(std::weak_ptr<carla::client::detail::Simulator> simulator) {
33  _simulator = simulator;
34  }
35 
36  // create a new walker route
39 
40  WalkerInfo info;
41  info.state = WALKER_IDLE;
42 
43  // save it
44  _walkers[id] = std::move(info);
45 
46  return true;
47  }
48 
49  // remove a walker route
51  // search
52  auto it = _walkers.find(id);
53  if (it == _walkers.end())
54  return false;
55  _walkers.erase(it);
56 
57  return true;
58  }
59 
60  // update all routes
61  bool WalkerManager::Update(double delta) {
62 
63  // check all walkers
64  for (auto &it : _walkers) {
65 
66  // get the elements
67  WalkerInfo &info = it.second;
68 
69  // in function of the state
70  switch (info.state) {
71  case WALKER_IDLE:
72  break;
73 
74  case WALKER_WALKING:
75  {
76  // get the target point
77  carla::geom::Location &target = info.route[info.currentIndex].location;
78  // get current position
79  carla::geom::Location current;
80  _nav->GetWalkerPosition(it.first, current);
81  // check distance to the target point
82  carla::geom::Vector3D dist(target.x - current.x, target.z - current.z, target.y - current.y);
83  if (dist.SquaredLength() <= 1) {
84  info.state = WALKER_IN_EVENT;
85  }
86  }
87  break;
88 
89  case WALKER_IN_EVENT:
90  switch (ExecuteEvent(it.first, it.second, delta)) {
92  break;
93  case EventResult::End:
94  // next point in route
95  SetWalkerNextPoint(it.first);
96  break;
98  // unblock changing the route
99  SetWalkerRoute(it.first);
100  break;
101  }
102  break;
103 
104  case WALKER_STOP:
105  info.state = WALKER_IDLE;
106  break;
107  }
108  }
109 
110  return true;
111  }
112 
113  // set a new route from its current position
115  // check
116  if (_nav == nullptr)
117  return false;
118 
119  // set a new random target
120  carla::geom::Location location;
121  _nav->GetRandomLocation(location, nullptr);
122 
123  // set the route
124  return SetWalkerRoute(id, location);
125  }
126 
127  // set a new route from its current position
129  // check
130  if (_nav == nullptr)
131  return false;
132 
133  // search
134  auto it = _walkers.find(id);
135  if (it == _walkers.end())
136  return false;
137 
138  // get it
139  WalkerInfo &info = it->second;
140  std::vector<carla::geom::Location> path;
141  std::vector<unsigned char> area;
142 
143  // save both points for the route
144  _nav->GetWalkerPosition(id, info.from);
145  info.to = to;
146  info.currentIndex = 0;
147  info.state = WALKER_IDLE;
148 
149  // get a route from navigation
150  _nav->GetAgentRoute(id, info.from, to, path, area);
151 
152  // create each point of the route
153  info.route.clear();
154  info.route.reserve(path.size());
155  unsigned char previous_area = CARLA_AREA_SIDEWALK;
156  for (unsigned int i=0; i<path.size(); ++i) {
157  // get the type
158  switch (area[i]) {
159  // do nothing
160  case CARLA_AREA_SIDEWALK:
161  info.route.emplace_back(WalkerEventIgnore(), std::move(path[i]), area[i]);
162  break;
163 
164  // stop and check
165  case CARLA_AREA_ROAD:
167  // only if we come from a safe area (sidewalks, grass or crosswalk)
168  if (previous_area != CARLA_AREA_CROSSWALK && previous_area != CARLA_AREA_ROAD)
169  info.route.emplace_back(WalkerEventStopAndCheck(60), std::move(path[i]), area[i]);
170  break;
171 
172  default:
173  info.route.emplace_back(WalkerEventIgnore(), std::move(path[i]), area[i]);
174  }
175  previous_area = area[i];
176  }
177 
178  // assign the first point to go (second in the list)
179  SetWalkerNextPoint(id);
180  return true;
181  }
182 
183  // set the next point in the route
185  // check
186  if (_nav == nullptr)
187  return false;
188 
189  // search
190  auto it = _walkers.find(id);
191  if (it == _walkers.end())
192  return false;
193 
194  // get it
195  WalkerInfo &info = it->second;
196 
197  // advance next point
198  ++info.currentIndex;
199 
200  // check the end
201  if (info.currentIndex < info.route.size()) {
202  // change the state
203  info.state = WALKER_WALKING;
204  // assign the point to go
205  _nav->PauseAgent(id, false);
206  _nav->SetWalkerDirectTarget(id, info.route[info.currentIndex].location);
207  } else {
208  // change the state
209  info.state = WALKER_STOP;
210  _nav->PauseAgent(id, true);
211  // we need a new route from here
212  SetWalkerRoute(id);
213  }
214 
215  return true;
216  }
217 
218  // get the next point in the route
220  // check
221  if (_nav == nullptr)
222  return false;
223 
224  // search
225  auto it = _walkers.find(id);
226  if (it == _walkers.end())
227  return false;
228 
229  // get it
230  WalkerInfo &info = it->second;
231 
232  // check the end
233  if (info.currentIndex < info.route.size()) {
234  location = info.route[info.currentIndex].location;
235  return true;
236  } else {
237  return false;
238  }
239  }
240 
242  // check
243  if (_nav == nullptr)
244  return false;
245 
246  // search
247  auto it = _walkers.find(id);
248  if (it == _walkers.end())
249  return false;
250 
251  // get it
252  WalkerInfo &info = it->second;
253 
254  // check the end of current crosswalk
255  unsigned int pos = info.currentIndex;
256  while (pos < info.route.size()) {
257  if (info.route[pos].areaType != CARLA_AREA_CROSSWALK) {
258  location = info.route[pos].location;
259  return true;
260  }
261  ++pos;
262  }
263 
264  return false;
265  }
266 
268  // go to the event
269  WalkerRoutePoint &rp = info.route[info.currentIndex];
270 
271  // build the visitor structure
272  WalkerEventVisitor visitor(this, id, delta);
273  // run the event
274  return boost::variant2::visit(visitor, rp.event);
275  }
276 
278  static bool AlreadyCalculated = false;
279  if (AlreadyCalculated) return;
280 
281  // the world
282  carla::client::World world = _simulator.lock()->GetWorld();
283 
284  _traffic_lights.clear();
285  std::vector<carla::rpc::Actor> actors = _simulator.lock()->GetAllTheActorsInTheEpisode();
286  for (auto actor : actors) {
287  carla::client::ActorSnapshot snapshot = _simulator.lock()->GetActorSnapshot(actor.id);
288  // check traffic lights only
289  if (actor.description.id == "traffic.traffic_light") {
290  // get the TL actor
292  boost::static_pointer_cast<carla::client::TrafficLight>(world.GetActor(actor.id));
293  // get the waypoints where the TL affects
294  std::vector<SharedPtr<carla::client::Waypoint>> list = tl->GetStopWaypoints();
295  for (auto &way : list) {
296  _traffic_lights.emplace_back(tl, way->GetTransform().location);
297  }
298  }
299  }
300 
301  AlreadyCalculated = true;
302  }
303 
304 
305  // return the trafficlight affecting that position
307  carla::geom::Location UnrealPos,
308  float max_distance) {
309  float min_dist = std::numeric_limits<float>::infinity();
311  for (auto &&item : _traffic_lights) {
312  float dist = UnrealPos.DistanceSquared(item.second);
313  if (dist < min_dist) {
314  min_dist = dist;
315  actor = item.first;
316  }
317  }
318  // if distance is not in the limit, then reject the trafficlight
319  if (max_distance < 0.0f || min_dist <= max_distance * max_distance) {
320  return actor;
321  } else {
323  }
324  }
325 
326 
327 } // namespace nav
328 } // namespace carla
std::vector< WalkerRoutePoint > route
Definition: WalkerManager.h:42
void SetNav(Navigation *nav)
assign the navigation module
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:987
rpc::ActorId ActorId
Definition: ActorId.h:18
std::weak_ptr< carla::client::detail::Simulator > _simulator
Definition: WalkerManager.h:95
float SquaredLength() const
Definition: geom/Vector3D.h:45
void PauseAgent(ActorId id, bool pause)
set an agent as paused for the crowd
boost::shared_ptr< T > SharedPtr
Use this SharedPtr (boost::shared_ptr) to keep compatibility with boost::python, but it would be nice...
Definition: Memory.h:20
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
EventResult
result of an event
Definition: WalkerEvent.h:28
std::vector< SharedPtr< Waypoint > > GetStopWaypoints() const
bool RemoveWalker(ActorId id)
remove a walker route
std::vector< std::pair< SharedPtr< carla::client::TrafficLight >, carla::geom::Location > > _traffic_lights
Definition: WalkerManager.h:93
bool SetWalkerNextPoint(ActorId id)
set the next point in the route
carla::geom::Location from
Definition: WalkerManager.h:38
SharedPtr< carla::client::TrafficLight > GetTrafficLightAffecting(carla::geom::Location UnrealPos, float max_distance=-1.0f)
return the trafficlight affecting that position
event to pause and check for near vehicles
Definition: WalkerEvent.h:45
bool SetWalkerDirectTarget(ActorId id, carla::geom::Location to)
Definition: Navigation.cpp:782
std::unordered_map< ActorId, WalkerInfo > _walkers
Definition: WalkerManager.h:92
EventResult ExecuteEvent(ActorId id, WalkerInfo &info, double delta)
SharedPtr< Actor > GetActor(ActorId id) const
Find actor by id, return nullptr if not found.
Definition: World.cpp:98
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:372
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
auto DistanceSquared(const Location &loc) const
Definition: geom/Location.h:44
carla::geom::Location to
Definition: WalkerManager.h:39
empty event that just ignores
Definition: WalkerEvent.h:35
bool AddWalker(ActorId id)
create a new walker route
Manage the pedestrians navigation, using the Recast & Detour library for low level calculations...
Definition: Navigation.h:57
void SetSimulator(std::weak_ptr< carla::client::detail::Simulator > simulator)
reference to the simulator to access API functions
unsigned int currentIndex
Definition: WalkerManager.h:40