CARLA
Navigation.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 
7 #define _USE_MATH_DEFINES // to avoid undefined error of M_PI (bug in Visual
8  // Studio 2015 and 2017)
9 #include <cmath>
10 
11 #include "carla/Logging.h"
12 #include "carla/nav/Navigation.h"
14 #include "carla/geom/Math.h"
15 
16 #include <iterator>
17 #include <fstream>
18 #include <mutex>
19 
20 namespace carla {
21 namespace nav {
22 
23  enum UpdateFlags {
29  };
30 
31  // these settings are the same than in RecastBuilder, so if you change the height of the agent,
32  // you should do the same in RecastBuilder
33  static const int MAX_POLYS = 256;
34  static const int MAX_AGENTS = 500;
35  static const int MAX_QUERY_SEARCH_NODES = 2048;
36  static const float AGENT_HEIGHT = 1.8f;
37  static const float AGENT_RADIUS = 0.3f;
38 
39  static const float AGENT_UNBLOCK_DISTANCE = 0.5f;
40  static const float AGENT_UNBLOCK_DISTANCE_SQUARED = AGENT_UNBLOCK_DISTANCE * AGENT_UNBLOCK_DISTANCE;
41  static const float AGENT_UNBLOCK_TIME = 4.0f;
42 
43  static const float AREA_GRASS_COST = 1.0f;
44  static const float AREA_ROAD_COST = 10.0f;
45 
46  // return a random float
47  static float frand() {
48  return static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
49  }
50 
52  // assign walker manager
53  _walker_manager.SetNav(this);
54  }
55 
57  _ready = false;
58  _time_to_unblock = 0.0f;
59  _mapped_walkers_id.clear();
60  _mapped_vehicles_id.clear();
61  _mapped_by_index.clear();
63  _yaw_walkers.clear();
64  _binary_mesh.clear();
65  dtFreeCrowd(_crowd);
66  dtFreeNavMeshQuery(_nav_query);
67  dtFreeNavMesh(_nav_mesh);
68  }
69 
70  // load navigation data
71  bool Navigation::Load(const std::string &filename) {
72  std::ifstream f;
73  std::istream_iterator<uint8_t> start(f), end;
74 
75  // read the whole file
76  f.open(filename, std::ios::binary);
77  if (!f.is_open()) {
78  return false;
79  }
80  std::vector<uint8_t> content(start, end);
81  f.close();
82 
83  // parse the content
84  return Load(std::move(content));
85  }
86 
87  // load navigation data from memory
88  bool Navigation::Load(std::vector<uint8_t> content) {
89  const int NAVMESHSET_MAGIC = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; // 'MSET';
90  const int NAVMESHSET_VERSION = 1;
91 #pragma pack(push, 1)
92 
93  struct NavMeshSetHeader {
94  int magic;
95  int version;
96  int num_tiles;
97  dtNavMeshParams params;
98  } header;
99  struct NavMeshTileHeader {
100  dtTileRef tile_ref;
101  int data_size;
102  };
103 #pragma pack(pop)
104 
105  // check size for header
106  if (content.size() < sizeof(header)) {
107  logging::log("Nav: failed loading binary");
108  return false;
109  }
110 
111  // read the file header
112  unsigned long pos = 0;
113  memcpy(&header, &content[pos], sizeof(header));
114  pos += sizeof(header);
115 
116  // check file magic and version
117  if (header.magic != NAVMESHSET_MAGIC || header.version != NAVMESHSET_VERSION) {
118  return false;
119  }
120 
121  // allocate object
122  dtNavMesh *mesh = dtAllocNavMesh();
123  if (!mesh) {
124  return false;
125  }
126 
127  // set number of tiles and origin
128  dtStatus status = mesh->init(&header.params);
129  if (dtStatusFailed(status)) {
130  return false;
131  }
132 
133  // read the tiles data
134  for (int i = 0; i < header.num_tiles; ++i) {
135  NavMeshTileHeader tile_header;
136 
137  // read the tile header
138  memcpy(&tile_header, &content[pos], sizeof(tile_header));
139  pos += sizeof(tile_header);
140  if (pos >= content.size()) {
141  dtFreeNavMesh(mesh);
142  return false;
143  }
144 
145  // check for valid tile
146  if (!tile_header.tile_ref || !tile_header.data_size) {
147  break;
148  }
149 
150  // allocate the buffer
151  char *data = static_cast<char *>(dtAlloc(static_cast<size_t>(tile_header.data_size), DT_ALLOC_PERM));
152  if (!data) {
153  break;
154  }
155 
156  // read the tile
157  memcpy(data, &content[pos], static_cast<size_t>(tile_header.data_size));
158  pos += static_cast<unsigned long>(tile_header.data_size);
159  if (pos > content.size()) {
160  dtFree(data);
161  dtFreeNavMesh(mesh);
162  return false;
163  }
164 
165  // add the tile data
166  mesh->addTile(reinterpret_cast<unsigned char *>(data), tile_header.data_size, DT_TILE_FREE_DATA,
167  tile_header.tile_ref, 0);
168  }
169 
170  // exchange
171  dtFreeNavMesh(_nav_mesh);
172  _nav_mesh = mesh;
173 
174  // prepare the query object
175  dtFreeNavMeshQuery(_nav_query);
176  _nav_query = dtAllocNavMeshQuery();
177  _nav_query->init(_nav_mesh, MAX_QUERY_SEARCH_NODES);
178 
179  // copy
180  _binary_mesh = std::move(content);
181  _ready = true;
182 
183  // create and init the crowd manager
184  CreateCrowd();
185 
186  return true;
187  }
188 
190 
191  // check if all is ready
192  if (!_ready) {
193  return;
194  }
195 
196  DEBUG_ASSERT(_crowd == nullptr);
197 
198  // create and init
199  _crowd = dtAllocCrowd();
200  // these radius should be the maximum size of the vehicles (CarlaCola for Carla)
201  const float max_agent_radius = AGENT_RADIUS * 20;
202  if (!_crowd->init(MAX_AGENTS, max_agent_radius, _nav_mesh)) {
203  logging::log("Nav: failed to create crowd");
204  return;
205  }
206 
207  // set different filters
208  // filter 0 can not walk on roads
209  _crowd->getEditableFilter(0)->setIncludeFlags(CARLA_TYPE_WALKABLE);
210  _crowd->getEditableFilter(0)->setExcludeFlags(CARLA_TYPE_ROAD);
211  _crowd->getEditableFilter(0)->setAreaCost(CARLA_AREA_ROAD, AREA_ROAD_COST);
212  _crowd->getEditableFilter(0)->setAreaCost(CARLA_AREA_GRASS, AREA_GRASS_COST);
213  // filter 1 can walk on roads
214  _crowd->getEditableFilter(1)->setIncludeFlags(CARLA_TYPE_WALKABLE);
215  _crowd->getEditableFilter(1)->setExcludeFlags(CARLA_TYPE_NONE);
216  _crowd->getEditableFilter(1)->setAreaCost(CARLA_AREA_ROAD, AREA_ROAD_COST);
217  _crowd->getEditableFilter(1)->setAreaCost(CARLA_AREA_GRASS, AREA_GRASS_COST);
218 
219  // Setup local avoidance params to different qualities.
220  dtObstacleAvoidanceParams params;
221  // Use mostly default settings, copy from dtCrowd.
222  memcpy(&params, _crowd->getObstacleAvoidanceParams(0), sizeof(dtObstacleAvoidanceParams));
223 
224  // Low (11)
225  params.velBias = 0.5f;
226  params.adaptiveDivs = 5;
227  params.adaptiveRings = 2;
228  params.adaptiveDepth = 1;
229  _crowd->setObstacleAvoidanceParams(0, &params);
230 
231  // Medium (22)
232  params.velBias = 0.5f;
233  params.adaptiveDivs = 5;
234  params.adaptiveRings = 2;
235  params.adaptiveDepth = 2;
236  _crowd->setObstacleAvoidanceParams(1, &params);
237 
238  // Good (45)
239  params.velBias = 0.5f;
240  params.adaptiveDivs = 7;
241  params.adaptiveRings = 2;
242  params.adaptiveDepth = 3;
243  _crowd->setObstacleAvoidanceParams(2, &params);
244 
245  // High (66)
246  params.velBias = 0.5f;
247  params.adaptiveDivs = 7;
248  params.adaptiveRings = 3;
249  params.adaptiveDepth = 3;
250 
251  _crowd->setObstacleAvoidanceParams(3, &params);
252  }
253 
254  // return the path points to go from one position to another
257  dtQueryFilter * filter,
258  std::vector<carla::geom::Location> &path,
259  std::vector<unsigned char> &area) {
260  // path found
261  float straight_path[MAX_POLYS * 3];
262  unsigned char straight_path_flags[MAX_POLYS];
263  dtPolyRef straight_path_polys[MAX_POLYS];
264  int num_straight_path;
265  int straight_path_options = DT_STRAIGHTPATH_AREA_CROSSINGS;
266 
267  // polys in path
268  dtPolyRef polys[MAX_POLYS];
269  int num_polys;
270 
271  // check if all is ready
272  if (!_ready) {
273  return false;
274  }
275 
276  DEBUG_ASSERT(_nav_query != nullptr);
277 
278  // point extension
279  float poly_pick_ext[3];
280  poly_pick_ext[0] = 2;
281  poly_pick_ext[1] = 4;
282  poly_pick_ext[2] = 2;
283 
284  // filter
285  dtQueryFilter filter2;
286  if (filter == nullptr) {
287  filter2.setAreaCost(CARLA_AREA_ROAD, AREA_ROAD_COST);
288  filter2.setAreaCost(CARLA_AREA_GRASS, AREA_GRASS_COST);
289  filter2.setIncludeFlags(CARLA_TYPE_WALKABLE);
290  filter2.setExcludeFlags(CARLA_TYPE_NONE);
291  filter = &filter2;
292  }
293 
294  // set the points
295  dtPolyRef start_ref = 0;
296  dtPolyRef end_ref = 0;
297  float start_pos[3] = { from.x, from.z, from.y };
298  float end_pos[3] = { to.x, to.z, to.y };
299  {
300  // critical section, force single thread running this
301  std::lock_guard<std::mutex> lock(_mutex);
302  _nav_query->findNearestPoly(start_pos, poly_pick_ext, filter, &start_ref, 0);
303  _nav_query->findNearestPoly(end_pos, poly_pick_ext, filter, &end_ref, 0);
304  }
305  if (!start_ref || !end_ref) {
306  return false;
307  }
308 
309  {
310  // critical section, force single thread running this
311  std::lock_guard<std::mutex> lock(_mutex);
312  // get the path of nodes
313  _nav_query->findPath(start_ref, end_ref, start_pos, end_pos, filter, polys, &num_polys, MAX_POLYS);
314  }
315 
316  // get the path of points
317  num_straight_path = 0;
318  if (num_polys == 0) {
319  return false;
320  }
321 
322  // in case of partial path, make sure the end point is clamped to the last
323  // polygon
324  float end_pos2[3];
325  dtVcopy(end_pos2, end_pos);
326  if (polys[num_polys - 1] != end_ref) {
327  // critical section, force single thread running this
328  std::lock_guard<std::mutex> lock(_mutex);
329  _nav_query->closestPointOnPoly(polys[num_polys - 1], end_pos, end_pos2, 0);
330  }
331 
332  // get the points
333  {
334  // critical section, force single thread running this
335  std::lock_guard<std::mutex> lock(_mutex);
336  _nav_query->findStraightPath(start_pos, end_pos2, polys, num_polys,
337  straight_path, straight_path_flags,
338  straight_path_polys, &num_straight_path, MAX_POLYS, straight_path_options);
339  }
340 
341  // copy the path to the output buffer
342  path.clear();
343  path.reserve(static_cast<unsigned long>(num_straight_path));
344  unsigned char area_type;
345  for (int i = 0, j = 0; j < num_straight_path; i += 3, ++j) {
346  // save coordinate for Unreal axis (x, z, y)
347  path.emplace_back(straight_path[i], straight_path[i + 2], straight_path[i + 1]);
348  // save area type
349  {
350  // critical section, force single thread running this
351  std::lock_guard<std::mutex> lock(_mutex);
352  _nav_mesh->getPolyArea(straight_path_polys[j], &area_type);
353  }
354  area.emplace_back(area_type);
355  }
356 
357  return true;
358  }
359 
361  std::vector<carla::geom::Location> &path, std::vector<unsigned char> &area) {
362  // path found
363  float straight_path[MAX_POLYS * 3];
364  unsigned char straight_path_flags[MAX_POLYS];
365  dtPolyRef straight_path_polys[MAX_POLYS];
366  int num_straight_path = 0;
367  int straight_path_options = DT_STRAIGHTPATH_AREA_CROSSINGS;
368 
369  // polys in path
370  dtPolyRef polys[MAX_POLYS];
371  int num_polys;
372 
373  // check if all is ready
374  if (!_ready) {
375  return false;
376  }
377 
378  DEBUG_ASSERT(_nav_query != nullptr);
379 
380  // point extension
381  float poly_pick_ext[3] = {2,4,2};
382 
383  // get current filter from agent
384  auto it = _mapped_walkers_id.find(id);
385  if (it == _mapped_walkers_id.end())
386  return false;
387 
388  const dtQueryFilter *filter;
389  {
390  // critical section, force single thread running this
391  std::lock_guard<std::mutex> lock(_mutex);
392  filter = _crowd->getFilter(_crowd->getAgent(it->second)->params.queryFilterType);
393  }
394 
395  // set the points
396  dtPolyRef start_ref = 0;
397  dtPolyRef end_ref = 0;
398  float start_pos[3] = { from.x, from.z, from.y };
399  float end_pos[3] = { to.x, to.z, to.y };
400  {
401  // critical section, force single thread running this
402  std::lock_guard<std::mutex> lock(_mutex);
403  _nav_query->findNearestPoly(start_pos, poly_pick_ext, filter, &start_ref, 0);
404  _nav_query->findNearestPoly(end_pos, poly_pick_ext, filter, &end_ref, 0);
405  }
406  if (!start_ref || !end_ref) {
407  return false;
408  }
409 
410  // get the path of nodes
411  {
412  // critical section, force single thread running this
413  std::lock_guard<std::mutex> lock(_mutex);
414  _nav_query->findPath(start_ref, end_ref, start_pos, end_pos, filter, polys, &num_polys, MAX_POLYS);
415  }
416 
417  // get the path of points
418  if (num_polys == 0) {
419  return false;
420  }
421 
422  // in case of partial path, make sure the end point is clamped to the last
423  // polygon
424  float end_pos2[3];
425  dtVcopy(end_pos2, end_pos);
426  if (polys[num_polys - 1] != end_ref) {
427  // critical section, force single thread running this
428  std::lock_guard<std::mutex> lock(_mutex);
429  _nav_query->closestPointOnPoly(polys[num_polys - 1], end_pos, end_pos2, 0);
430  }
431 
432  // get the points
433  {
434  // critical section, force single thread running this
435  std::lock_guard<std::mutex> lock(_mutex);
436  _nav_query->findStraightPath(start_pos, end_pos2, polys, num_polys,
437  straight_path, straight_path_flags,
438  straight_path_polys, &num_straight_path, MAX_POLYS, straight_path_options);
439  }
440 
441  // copy the path to the output buffer
442  path.clear();
443  path.reserve(static_cast<unsigned long>(num_straight_path));
444  unsigned char area_type;
445  for (int i = 0, j = 0; j < num_straight_path; i += 3, ++j) {
446  // save coordinate for Unreal axis (x, z, y)
447  path.emplace_back(straight_path[i], straight_path[i + 2], straight_path[i + 1]);
448  // save area type
449  {
450  // critical section, force single thread running this
451  std::lock_guard<std::mutex> lock(_mutex);
452  _nav_mesh->getPolyArea(straight_path_polys[j], &area_type);
453  }
454  area.emplace_back(area_type);
455  }
456 
457  return true;
458  }
459 
460  // create a new walker in crowd
462  dtCrowdAgentParams params;
463 
464  // check if all is ready
465  if (!_ready) {
466  return false;
467  }
468 
469  DEBUG_ASSERT(_crowd != nullptr);
470 
471  // set parameters
472  memset(&params, 0, sizeof(params));
473  params.radius = AGENT_RADIUS;
474  params.height = AGENT_HEIGHT;
475  params.maxAcceleration = 160.0f;
476  params.maxSpeed = 1.47f;
477  params.collisionQueryRange = 10;
478  params.obstacleAvoidanceType = 3;
479  params.separationWeight = 0.5f;
480 
481  // set if the agent can cross roads or not
482  if (frand() <= _probability_crossing) {
483  params.queryFilterType = 1;
484  } else {
485  params.queryFilterType = 0;
486  }
487 
488  // flags
489  params.updateFlags = 0;
490  params.updateFlags |= DT_CROWD_ANTICIPATE_TURNS;
491  params.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
492  params.updateFlags |= DT_CROWD_SEPARATION;
493 
494  // from Unreal coordinates (subtract half height to move pivot from center
495  // (unreal) to bottom (recast))
496  float point_from[3] = { from.x, from.z - (AGENT_HEIGHT / 2.0f), from.y };
497  // add walker
498  int index;
499  {
500  // critical section, force single thread running this
501  std::lock_guard<std::mutex> lock(_mutex);
502  index = _crowd->addAgent(point_from, &params);
503  if (index == -1) {
504  return false;
505  }
506  }
507 
508  // save the id
509  _mapped_walkers_id[id] = index;
510  _mapped_by_index[index] = id;
511 
512  // init yaw
513  _yaw_walkers[id] = 0.0f;
514 
515  // add walker for the route planning
517 
518  return true;
519  }
520 
521  // create a new vehicle in crowd to be avoided by walkers
523  namespace cg = carla::geom;
524  dtCrowdAgentParams params;
525 
526  // check if all is ready
527  if (!_ready) {
528  return false;
529  }
530 
531  DEBUG_ASSERT(_crowd != nullptr);
532 
533  // get the bounding box extension plus some space around
534  float marge = 0.8f;
535  float hx = vehicle.bounding.extent.x + marge;
536  float hy = vehicle.bounding.extent.y + marge;
537  // define the 4 corners of the bounding box
538  cg::Vector3D box_corner1 {-hx, -hy, 0};
539  cg::Vector3D box_corner2 { hx + 0.2f, -hy, 0};
540  cg::Vector3D box_corner3 { hx + 0.2f, hy, 0};
541  cg::Vector3D box_corner4 {-hx, hy, 0};
542  // rotate the points
543  float angle = cg::Math::ToRadians(vehicle.transform.rotation.yaw);
544  box_corner1 = cg::Math::RotatePointOnOrigin2D(box_corner1, angle);
545  box_corner2 = cg::Math::RotatePointOnOrigin2D(box_corner2, angle);
546  box_corner3 = cg::Math::RotatePointOnOrigin2D(box_corner3, angle);
547  box_corner4 = cg::Math::RotatePointOnOrigin2D(box_corner4, angle);
548  // translate to world position
549  box_corner1 += vehicle.transform.location;
550  box_corner2 += vehicle.transform.location;
551  box_corner3 += vehicle.transform.location;
552  box_corner4 += vehicle.transform.location;
553 
554  // check if this actor exists
555  auto it = _mapped_vehicles_id.find(vehicle.id);
556  if (it != _mapped_vehicles_id.end()) {
557  // get the index found
558  int index = it->second;
559  if (index != -1) {
560  // get the agent
561  dtCrowdAgent *agent;
562  {
563  // critical section, force single thread running this
564  std::lock_guard<std::mutex> lock(_mutex);
565  agent = _crowd->getEditableAgent(index);
566  }
567  if (agent) {
568  // update its position
569  agent->npos[0] = vehicle.transform.location.x;
570  agent->npos[1] = vehicle.transform.location.z;
571  agent->npos[2] = vehicle.transform.location.y;
572  // update its oriented bounding box
573  agent->params.obb[0] = box_corner1.x;
574  agent->params.obb[1] = box_corner1.z;
575  agent->params.obb[2] = box_corner1.y;
576  agent->params.obb[3] = box_corner2.x;
577  agent->params.obb[4] = box_corner2.z;
578  agent->params.obb[5] = box_corner2.y;
579  agent->params.obb[6] = box_corner3.x;
580  agent->params.obb[7] = box_corner3.z;
581  agent->params.obb[8] = box_corner3.y;
582  agent->params.obb[9] = box_corner4.x;
583  agent->params.obb[10] = box_corner4.z;
584  agent->params.obb[11] = box_corner4.y;
585  }
586  return true;
587  }
588  }
589 
590  // set parameters
591  memset(&params, 0, sizeof(params));
592  params.radius = 2;
593  params.height = AGENT_HEIGHT;
594  params.maxAcceleration = 0.0f;
595  params.maxSpeed = 1.47f;
596  params.collisionQueryRange = 0;
597  params.obstacleAvoidanceType = 0;
598  params.separationWeight = 100.0f;
599 
600  // flags
601  params.updateFlags = 0;
602  params.updateFlags |= DT_CROWD_SEPARATION;
603 
604  // update its oriented bounding box
605  // data: [x][y][z] [x][y][z] [x][y][z] [x][y][z]
606  params.useObb = true;
607  params.obb[0] = box_corner1.x;
608  params.obb[1] = box_corner1.z;
609  params.obb[2] = box_corner1.y;
610  params.obb[3] = box_corner2.x;
611  params.obb[4] = box_corner2.z;
612  params.obb[5] = box_corner2.y;
613  params.obb[6] = box_corner3.x;
614  params.obb[7] = box_corner3.z;
615  params.obb[8] = box_corner3.y;
616  params.obb[9] = box_corner4.x;
617  params.obb[10] = box_corner4.z;
618  params.obb[11] = box_corner4.y;
619 
620  // from Unreal coordinates (vertical is Z) to Recast coordinates (vertical is Y)
621  float point_from[3] = { vehicle.transform.location.x,
622  vehicle.transform.location.z,
623  vehicle.transform.location.y };
624 
625  // add walker
626  int index;
627  {
628  // critical section, force single thread running this
629  std::lock_guard<std::mutex> lock(_mutex);
630  index = _crowd->addAgent(point_from, &params);
631  if (index == -1) {
632  logging::log("Vehicle agent not added to the crowd by some problem!");
633  return false;
634  }
635 
636  // mark as valid
637  dtCrowdAgent *agent = _crowd->getEditableAgent(index);
638  if (agent) {
639  agent->state = DT_CROWDAGENT_STATE_WALKING;
640  }
641  }
642 
643  // save the id
644  _mapped_vehicles_id[vehicle.id] = index;
645  _mapped_by_index[index] = vehicle.id;
646 
647  return true;
648  }
649 
650  // remove an agent
652 
653  // check if all is ready
654  if (!_ready) {
655  return false;
656  }
657 
658  DEBUG_ASSERT(_crowd != nullptr);
659 
660  // get the internal walker index
661  auto it = _mapped_walkers_id.find(id);
662  if (it != _mapped_walkers_id.end()) {
663  // remove from crowd
664  {
665  // critical section, force single thread running this
666  std::lock_guard<std::mutex> lock(_mutex);
667  _crowd->removeAgent(it->second);
668  }
670  // remove from mapping
671  _mapped_walkers_id.erase(it);
672  _mapped_by_index.erase(it->second);
673 
674  return true;
675  }
676 
677  // get the internal vehicle index
678  it = _mapped_vehicles_id.find(id);
679  if (it != _mapped_vehicles_id.end()) {
680  // remove from crowd
681  {
682  // critical section, force single thread running this
683  std::lock_guard<std::mutex> lock(_mutex);
684  _crowd->removeAgent(it->second);
685  }
686  // remove from mapping
687  _mapped_vehicles_id.erase(it);
688  _mapped_by_index.erase(it->second);
689 
690  return true;
691  }
692 
693  return false;
694  }
695 
696  // add/update/delete vehicles in crowd
697  bool Navigation::UpdateVehicles(std::vector<VehicleCollisionInfo> vehicles) {
698  std::unordered_set<carla::rpc::ActorId> updated;
699 
700  // add all current mapped vehicles in the set
701  for (auto &&entry : _mapped_vehicles_id) {
702  updated.insert(entry.first);
703  }
704 
705  // add all vehicles (if already exists, it gets updated only)
706  for (auto &&entry : vehicles) {
707  // try to add or update the vehicle
708  AddOrUpdateVehicle(entry);
709  // mark as updated (to avoid removing it in this frame)
710  updated.erase(entry.id);
711  }
712 
713  // remove all vehicles not updated (they don't exist in this frame)
714  for (auto &&entry : updated) {
715  // remove agent not updated
716  RemoveAgent(entry);
717  }
718 
719  return true;
720  }
721 
722  // set new max speed
723  bool Navigation::SetWalkerMaxSpeed(ActorId id, float max_speed) {
724 
725  // check if all is ready
726  if (!_ready) {
727  return false;
728  }
729 
730  DEBUG_ASSERT(_crowd != nullptr);
731 
732  // get the internal index
733  auto it = _mapped_walkers_id.find(id);
734  if (it == _mapped_walkers_id.end()) {
735  return false;
736  }
737 
738  // get the agent
739  {
740  // critical section, force single thread running this
741  std::lock_guard<std::mutex> lock(_mutex);
742  dtCrowdAgent *agent = _crowd->getEditableAgent(it->second);
743  if (agent) {
744  agent->params.maxSpeed = max_speed;
745  return true;
746  }
747  }
748 
749  return false;
750  }
751 
752  // set a new target point to go
754 
755  // check if all is ready
756  if (!_ready) {
757  return false;
758  }
759 
760  // get the internal index
761  auto it = _mapped_walkers_id.find(id);
762  if (it == _mapped_walkers_id.end()) {
763  return false;
764  }
765 
766  return _walker_manager.SetWalkerRoute(id, to);
767  }
768 
769  // set a new target point to go directly without events
771 
772  // check if all is ready
773  if (!_ready) {
774  return false;
775  }
776 
777  // get the internal index
778  auto it = _mapped_walkers_id.find(id);
779  if (it == _mapped_walkers_id.end()) {
780  return false;
781  }
782 
783  return SetWalkerDirectTargetIndex(it->second, to);
784  }
785 
786  // set a new target point to go directly without events
788 
789  // check if all is ready
790  if (!_ready) {
791  return false;
792  }
793 
794  DEBUG_ASSERT(_crowd != nullptr);
795  DEBUG_ASSERT(_nav_query != nullptr);
796 
797  if (index == -1) {
798  return false;
799  }
800 
801  // set target position
802  float point_to[3] = { to.x, to.z, to.y };
803  float nearest[3];
804  bool res;
805  {
806  // critical section, force single thread running this
807  std::lock_guard<std::mutex> lock(_mutex);
808  const dtQueryFilter *filter = _crowd->getFilter(0);
809  dtPolyRef target_ref;
810  _nav_query->findNearestPoly(point_to, _crowd->getQueryHalfExtents(), filter, &target_ref, nearest);
811  if (!target_ref) {
812  return false;
813  }
814 
815  res = _crowd->requestMoveTarget(index, target_ref, point_to);
816  }
817 
818  return res;
819  }
820 
821  // update all walkers in crowd
823 
824  // check if all is ready
825  if (!_ready) {
826  return;
827  }
828 
829  DEBUG_ASSERT(_crowd != nullptr);
830 
831  // update crowd agents
832  _delta_seconds = state.GetTimestamp().delta_seconds;
833  {
834  // critical section, force single thread running this
835  std::lock_guard<std::mutex> lock(_mutex);
836  _crowd->update(static_cast<float>(_delta_seconds), nullptr);
837  }
838 
839  // update the walkers route
841 
842  // update the time to check for blocked agents
844 
845  // check all active agents
846  int total_unblocked = 0;
847  int total_agents;
848  {
849  // critical section, force single thread running this
850  std::lock_guard<std::mutex> lock(_mutex);
851  total_agents = _crowd->getAgentCount();
852  }
853  const dtCrowdAgent *ag;
854  for (int i = 0; i < total_agents; ++i) {
855  {
856  // critical section, force single thread running this
857  std::lock_guard<std::mutex> lock(_mutex);
858  ag = _crowd->getAgent(i);
859  }
860  if (!ag->active || ag->paused) {
861  continue;
862  }
863 
864  // check only pedestrians not paused, and no vehicles
865  if (!ag->params.useObb && !ag->paused) {
866  bool reset_target_pos = false;
867  bool use_same_filter = false;
868 
869  // check for unblocking actors
870  if (_time_to_unblock >= AGENT_UNBLOCK_TIME) {
871  // get the distance moved by each actor
873  carla::geom::Vector3D current = carla::geom::Vector3D(ag->npos[0], ag->npos[1], ag->npos[2]);
874  carla::geom::Vector3D distance = current - previous;
875  float d = distance.SquaredLength();
876  if (d < AGENT_UNBLOCK_DISTANCE_SQUARED) {
877  ++total_unblocked;
878  reset_target_pos = true;
879  use_same_filter = true;
880  }
881  // update with current position
882  _walkers_blocked_position[i] = current;
883 
884  // check to assign a new target position
885  if (reset_target_pos) {
886  // set if the agent can cross roads or not
887  if (!use_same_filter) {
888  if (frand() <= _probability_crossing) {
889  SetAgentFilter(i, 1);
890  } else {
891  SetAgentFilter(i, 0);
892  }
893  }
894  // set a new random target
895  carla::geom::Location location;
896  GetRandomLocation(location, nullptr);
898  }
899  }
900  }
901  }
902 
903  // check for resetting time
904  if (_time_to_unblock >= AGENT_UNBLOCK_TIME) {
905  _time_to_unblock = 0.0f;
906  }
907  }
908 
909  // get the walker current transform
911 
912  // check if all is ready
913  if (!_ready) {
914  return false;
915  }
916 
917  DEBUG_ASSERT(_crowd != nullptr);
918 
919  // get the internal index
920  auto it = _mapped_walkers_id.find(id);
921  if (it == _mapped_walkers_id.end()) {
922  return false;
923  }
924 
925  // get the index found
926  int index = it->second;
927  if (index == -1) {
928  return false;
929  }
930 
931  // get the walker
932  const dtCrowdAgent *agent;
933  {
934  // critical section, force single thread running this
935  std::lock_guard<std::mutex> lock(_mutex);
936  agent = _crowd->getAgent(index);
937  }
938 
939  if (!agent->active) {
940  return false;
941  }
942 
943  // set its position in Unreal coordinates
944  trans.location.x = agent->npos[0];
945  trans.location.y = agent->npos[2];
946  trans.location.z = agent->npos[1];
947 
948  // set its rotation
949  float yaw;
950  float speed = 0.0f;
951  float min = 0.1f;
952  if (agent->vel[0] < -min || agent->vel[0] > min ||
953  agent->vel[2] < -min || agent->vel[2] > min) {
954  yaw = atan2f(agent->vel[2], agent->vel[0]) * (180.0f / static_cast<float>(M_PI));
955  speed = sqrtf(agent->vel[0] * agent->vel[0] + agent->vel[1] * agent->vel[1] + agent->vel[2] * agent->vel[2]);
956  } else {
957  yaw = atan2f(agent->dvel[2], agent->dvel[0]) * (180.0f / static_cast<float>(M_PI));
958  speed = sqrtf(agent->dvel[0] * agent->dvel[0] + agent->dvel[1] * agent->dvel[1] + agent->dvel[2] * agent->dvel[2]);
959  }
960 
961  // interpolate current and target angle
962  float shortest_angle = fmod(yaw - _yaw_walkers[id] + 540.0f, 360.0f) - 180.0f;
963  float per = (speed / 1.5f);
964  if (per > 1.0f) per = 1.0f;
965  float rotation_speed = per * 6.0f;
966  trans.rotation.yaw = _yaw_walkers[id] +
967  (shortest_angle * rotation_speed * static_cast<float>(_delta_seconds));
968  _yaw_walkers[id] = trans.rotation.yaw;
969 
970  return true;
971  }
972 
973  // get the walker current location
975 
976  // check if all is ready
977  if (!_ready) {
978  return false;
979  }
980 
981  DEBUG_ASSERT(_crowd != nullptr);
982 
983  // get the internal index
984  auto it = _mapped_walkers_id.find(id);
985  if (it == _mapped_walkers_id.end()) {
986  return false;
987  }
988 
989  // get the index found
990  int index = it->second;
991  if (index == -1) {
992  return false;
993  }
994 
995  // get the walker
996  const dtCrowdAgent *agent;
997  {
998  // critical section, force single thread running this
999  std::lock_guard<std::mutex> lock(_mutex);
1000  agent = _crowd->getAgent(index);
1001  }
1002 
1003  if (!agent->active) {
1004  return false;
1005  }
1006 
1007  // set its position in Unreal coordinates
1008  location.x = agent->npos[0];
1009  location.y = agent->npos[2];
1010  location.z = agent->npos[1];
1011 
1012  return true;
1013  }
1014 
1016 
1017  // check if all is ready
1018  if (!_ready) {
1019  return 0.0f;
1020  }
1021 
1022  DEBUG_ASSERT(_crowd != nullptr);
1023 
1024  // get the internal index
1025  auto it = _mapped_walkers_id.find(id);
1026  if (it == _mapped_walkers_id.end()) {
1027  return 0.0f;
1028  }
1029 
1030  // get the index found
1031  int index = it->second;
1032  if (index == -1) {
1033  return 0.0f;
1034  }
1035 
1036  // get the walker
1037  const dtCrowdAgent *agent;
1038  {
1039  // critical section, force single thread running this
1040  std::lock_guard<std::mutex> lock(_mutex);
1041  agent = _crowd->getAgent(index);
1042  }
1043 
1044  return sqrt(agent->vel[0] * agent->vel[0] + agent->vel[1] * agent->vel[1] + agent->vel[2] *
1045  agent->vel[2]);
1046  }
1047 
1048  // get a random location for navigation
1049  bool Navigation::GetRandomLocation(carla::geom::Location &location, dtQueryFilter * filter) const {
1050 
1051  // check if all is ready
1052  if (!_ready) {
1053  return false;
1054  }
1055 
1056  DEBUG_ASSERT(_nav_query != nullptr);
1057 
1058  // filter
1059  dtQueryFilter filter2;
1060  if (filter == nullptr) {
1061  filter2.setIncludeFlags(CARLA_TYPE_SIDEWALK);
1062  filter2.setExcludeFlags(CARLA_TYPE_NONE);
1063  filter = &filter2;
1064  }
1065 
1066  // we will try up to 10 rounds, otherwise we failed to find a good location
1067  dtPolyRef random_ref { 0 };
1068  float point[3] { 0.0f, 0.0f, 0.0f };
1069  int rounds = 10;
1070  {
1071  dtStatus status;
1072  // critical section, force single thread running this
1073  std::lock_guard<std::mutex> lock(_mutex);
1074  do {
1075  status = _nav_query->findRandomPoint(filter, frand, &random_ref, point);
1076  // set the location in Unreal coords
1077  if (status == DT_SUCCESS) {
1078  location.x = point[0];
1079  location.y = point[2];
1080  location.z = point[1];
1081  }
1082  --rounds;
1083  } while (status != DT_SUCCESS && rounds > 0);
1084  }
1085 
1086  return (rounds > 0);
1087  }
1088 
1089  // assign a filter index to an agent
1090  void Navigation::SetAgentFilter(int agent_index, int filter_index)
1091  {
1092  // get the walker
1093  dtCrowdAgent *agent;
1094  {
1095  // critical section, force single thread running this
1096  std::lock_guard<std::mutex> lock(_mutex);
1097  agent = _crowd->getEditableAgent(agent_index);
1098  }
1099  agent->params.queryFilterType = static_cast<unsigned char>(filter_index);
1100  }
1101 
1102  // set the probability that an agent could cross the roads in its path following
1103  // percentage of 0.0f means no pedestrian can cross roads
1104  // percentage of 0.5f means 50% of all pedestrians can cross roads
1105  // percentage of 1.0f means all pedestrians can cross roads if needed
1107  {
1108  _probability_crossing = percentage;
1109  }
1110 
1111  // set an agent as paused for the crowd
1112  void Navigation::PauseAgent(ActorId id, bool pause) {
1113  // check if all is ready
1114  if (!_ready) {
1115  return;
1116  }
1117 
1118  DEBUG_ASSERT(_crowd != nullptr);
1119 
1120  // get the internal index
1121  auto it = _mapped_walkers_id.find(id);
1122  if (it == _mapped_walkers_id.end()) {
1123  return;
1124  }
1125 
1126  // get the index found
1127  int index = it->second;
1128  if (index == -1) {
1129  return;
1130  }
1131 
1132  // get the walker
1133  dtCrowdAgent *agent;
1134  {
1135  // critical section, force single thread running this
1136  std::lock_guard<std::mutex> lock(_mutex);
1137  agent = _crowd->getEditableAgent(index);
1138  }
1139 
1140  // mark
1141  agent->paused = pause;
1142  }
1143 
1144  bool Navigation::HasVehicleNear(ActorId id, float distance, carla::geom::Location direction) {
1145  // get the internal index (walker or vehicle)
1146  auto it = _mapped_walkers_id.find(id);
1147  if (it == _mapped_walkers_id.end()) {
1148  it = _mapped_vehicles_id.find(id);
1149  if (it == _mapped_vehicles_id.end()) {
1150  return false;
1151  }
1152  }
1153 
1154  float dir[3] = { direction.x, direction.z, direction.y };
1155  bool result;
1156  {
1157  // critical section, force single thread running this
1158  std::lock_guard<std::mutex> lock(_mutex);
1159  result = _crowd->hasVehicleNear(it->second, distance * distance, dir, false);
1160  }
1161  return result;
1162  }
1163 
1164  /// make agent look at some location
1166  // get the internal index (walker or vehicle)
1167  auto it = _mapped_walkers_id.find(id);
1168  if (it == _mapped_walkers_id.end()) {
1169  it = _mapped_vehicles_id.find(id);
1170  if (it == _mapped_vehicles_id.end()) {
1171  return false;
1172  }
1173  }
1174 
1175  dtCrowdAgent *agent;
1176  {
1177  // critical section, force single thread running this
1178  std::lock_guard<std::mutex> lock(_mutex);
1179  agent = _crowd->getEditableAgent(it->second);
1180  }
1181 
1182  // get the position
1183  float x = (location.x - agent->npos[0]) * 0.0001f;
1184  float y = (location.y - agent->npos[2]) * 0.0001f;
1185  float z = (location.z - agent->npos[1]) * 0.0001f;
1186 
1187  // set its velocity
1188  agent->vel[0] = x;
1189  agent->vel[2] = y;
1190  agent->vel[1] = z;
1191  agent->nvel[0] = x;
1192  agent->nvel[2] = y;
1193  agent->nvel[1] = z;
1194  agent->dvel[0] = x;
1195  agent->dvel[2] = y;
1196  agent->dvel[1] = z;
1197 
1198  return true;
1199  }
1200 
1201 } // namespace nav
1202 } // namespace carla
static const float AGENT_RADIUS
Definition: Navigation.cpp:37
dtCrowd * _crowd
crowd
Definition: Navigation.h:124
void SetNav(Navigation *nav)
assign the navigation module
Definition: WalkerManager.h:50
std::unordered_map< ActorId, int > _mapped_vehicles_id
Definition: Navigation.h:127
std::vector< uint8_t > _binary_mesh
Definition: Navigation.h:118
std::unordered_map< int, ActorId > _mapped_by_index
Definition: Navigation.h:129
static void log(Args &&... args)
Definition: Logging.h:59
static const float AGENT_UNBLOCK_DISTANCE_SQUARED
Definition: Navigation.cpp:40
static const int MAX_AGENTS
Definition: Navigation.cpp:34
bool AddWalker(ActorId id, carla::geom::Location from)
create a new walker
Definition: Navigation.cpp:461
bool RemoveAgent(ActorId id)
remove an agent
Definition: Navigation.cpp:651
Vector3D extent
Half the size of the BoundingBox in local space.
bool GetWalkerPosition(ActorId id, carla::geom::Location &location)
get the walker current location
Definition: Navigation.cpp:974
dtNavMeshQuery * _nav_query
Definition: Navigation.h:122
rpc::ActorId ActorId
Definition: ActorId.h:18
carla::rpc::ActorId id
Definition: Navigation.h:48
std::unordered_map< ActorId, int > _mapped_walkers_id
mapping Id
Definition: Navigation.h:126
float SquaredLength() const
Definition: geom/Vector3D.h:45
void PauseAgent(ActorId id, bool pause)
set an agent as paused for the crowd
static const int MAX_QUERY_SEARCH_NODES
Definition: Navigation.cpp:35
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
Represents the state of all the actors of an episode at a given frame.
Definition: EpisodeState.h:26
bool HasVehicleNear(ActorId id, float distance, carla::geom::Location direction)
return if the agent has a vehicle near (as neighbour)
static float frand()
Definition: Navigation.cpp:47
bool SetWalkerTarget(ActorId id, carla::geom::Location to)
set a new target point to go through a route with events
Definition: Navigation.cpp:753
bool RemoveWalker(ActorId id)
remove a walker route
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
bool SetWalkerLookAt(ActorId id, carla::geom::Location location)
make agent look at some location
carla::geom::BoundingBox bounding
Definition: Navigation.h:50
WalkerManager _walker_manager
walker manager for the route planning with events
Definition: Navigation.h:137
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
carla::geom::Transform transform
Definition: Navigation.h:49
bool AddOrUpdateVehicle(VehicleCollisionInfo &vehicle)
create a new vehicle in crowd to be avoided by walkers
Definition: Navigation.cpp:522
bool SetWalkerDirectTarget(ActorId id, carla::geom::Location to)
Definition: Navigation.cpp:770
std::unordered_map< ActorId, float > _yaw_walkers
store walkers yaw angle from previous tick
Definition: Navigation.h:131
bool SetWalkerDirectTargetIndex(int index, carla::geom::Location to)
Definition: Navigation.cpp:787
bool GetPath(carla::geom::Location from, carla::geom::Location to, dtQueryFilter *filter, std::vector< carla::geom::Location > &path, std::vector< unsigned char > &area)
return the path points to go from one position to another
Definition: Navigation.cpp:255
std::unordered_map< int, carla::geom::Vector3D > _walkers_blocked_position
saves the position of each actor at intervals and check if any is blocked
Definition: Navigation.h:133
static const float AGENT_UNBLOCK_TIME
Definition: Navigation.cpp:41
void SetPedestriansCrossFactor(float percentage)
set the probability that an agent could cross the roads in its path following
void UpdateCrowd(const client::detail::EpisodeState &state)
update all walkers in crowd
Definition: Navigation.cpp:822
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
const auto & GetTimestamp() const
Definition: EpisodeState.h:46
bool GetWalkerTransform(ActorId id, carla::geom::Transform &trans)
get the walker current transform
Definition: Navigation.cpp:910
static const int MAX_POLYS
Definition: Navigation.cpp:33
static const float AGENT_UNBLOCK_DISTANCE
Definition: Navigation.cpp:39
static constexpr T ToRadians(T deg)
Definition: Math.h:43
float GetWalkerSpeed(ActorId id)
get the walker current transform
bool GetRandomLocation(carla::geom::Location &location, dtQueryFilter *filter=nullptr) const
get a random location for navigation
bool Update(double delta)
update all routes
void SetAgentFilter(int agent_index, int filter_index)
assign a filter index to an agent
bool SetWalkerMaxSpeed(ActorId id, float max_speed)
set new max speed
Definition: Navigation.cpp:723
static const float AREA_GRASS_COST
Definition: Navigation.cpp:43
bool SetWalkerRoute(ActorId id)
set a new route from its current position
static const float AREA_ROAD_COST
Definition: Navigation.cpp:44
struct to send info about vehicles to the crowd
Definition: Navigation.h:47
static const float AGENT_HEIGHT
Definition: Navigation.cpp:36
dtNavMesh * _nav_mesh
meshes
Definition: Navigation.h:121
bool AddWalker(ActorId id)
create a new walker route
bool Load(const std::string &filename)
load navigation data
Definition: Navigation.cpp:71
bool UpdateVehicles(std::vector< VehicleCollisionInfo > vehicles)
add/update/delete vehicles in crowd
Definition: Navigation.cpp:697
void CreateCrowd(void)
create the crowd object
Definition: Navigation.cpp:189
static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle)
Definition: Math.cpp:111