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