7 #define _USE_MATH_DEFINES // to avoid undefined error of M_PI (bug in Visual 48 return static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
78 std::istream_iterator<uint8_t> start(f), end;
81 f.open(filename, std::ios::binary);
85 std::vector<uint8_t> content(start, end);
89 return Load(std::move(content));
94 const int NAVMESHSET_MAGIC =
'M' << 24 |
'S' << 16 |
'E' << 8 |
'T';
95 const int NAVMESHSET_VERSION = 1;
98 struct NavMeshSetHeader {
102 dtNavMeshParams params;
104 struct NavMeshTileHeader {
111 if (content.size() <
sizeof(header)) {
117 unsigned long pos = 0;
118 memcpy(&header, &content[pos],
sizeof(header));
119 pos +=
sizeof(header);
122 if (header.magic != NAVMESHSET_MAGIC || header.version != NAVMESHSET_VERSION) {
127 dtNavMesh *mesh = dtAllocNavMesh();
133 dtStatus status = mesh->init(&header.params);
134 if (dtStatusFailed(status)) {
139 for (
int i = 0; i < header.num_tiles; ++i) {
140 NavMeshTileHeader tile_header;
143 memcpy(&tile_header, &content[pos],
sizeof(tile_header));
144 pos +=
sizeof(tile_header);
145 if (pos >= content.size()) {
151 if (!tile_header.tile_ref || !tile_header.data_size) {
156 char *data =
static_cast<char *
>(dtAlloc(static_cast<size_t>(tile_header.data_size), DT_ALLOC_PERM));
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()) {
171 mesh->addTile(reinterpret_cast<unsigned char *>(data), tile_header.data_size, DT_TILE_FREE_DATA,
172 tile_header.tile_ref, 0);
206 const float max_agent_radius = AGENT_RADIUS * 20;
225 dtObstacleAvoidanceParams params;
227 memcpy(¶ms,
_crowd->getObstacleAvoidanceParams(0),
sizeof(dtObstacleAvoidanceParams));
230 params.velBias = 0.5f;
231 params.adaptiveDivs = 5;
232 params.adaptiveRings = 2;
233 params.adaptiveDepth = 1;
234 _crowd->setObstacleAvoidanceParams(0, ¶ms);
237 params.velBias = 0.5f;
238 params.adaptiveDivs = 5;
239 params.adaptiveRings = 2;
240 params.adaptiveDepth = 2;
241 _crowd->setObstacleAvoidanceParams(1, ¶ms);
244 params.velBias = 0.5f;
245 params.adaptiveDivs = 7;
246 params.adaptiveRings = 2;
247 params.adaptiveDepth = 3;
248 _crowd->setObstacleAvoidanceParams(2, ¶ms);
251 params.velBias = 0.5f;
252 params.adaptiveDivs = 7;
253 params.adaptiveRings = 3;
254 params.adaptiveDepth = 3;
256 _crowd->setObstacleAvoidanceParams(3, ¶ms);
262 dtQueryFilter * filter,
263 std::vector<carla::geom::Location> &path,
264 std::vector<unsigned char> &area) {
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;
284 float poly_pick_ext[3];
285 poly_pick_ext[0] = 2;
286 poly_pick_ext[1] = 4;
287 poly_pick_ext[2] = 2;
290 dtQueryFilter filter2;
291 if (filter ==
nullptr) {
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 };
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);
310 if (!start_ref || !end_ref) {
316 std::lock_guard<std::mutex> lock(
_mutex);
318 _nav_query->findPath(start_ref, end_ref, start_pos, end_pos, filter, polys, &num_polys, MAX_POLYS);
322 num_straight_path = 0;
323 if (num_polys == 0) {
330 dtVcopy(end_pos2, end_pos);
331 if (polys[num_polys - 1] != end_ref) {
333 std::lock_guard<std::mutex> lock(
_mutex);
334 _nav_query->closestPointOnPoly(polys[num_polys - 1], end_pos, end_pos2, 0);
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);
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) {
352 path.emplace_back(straight_path[i], straight_path[i + 2], straight_path[i + 1]);
356 std::lock_guard<std::mutex> lock(
_mutex);
357 _nav_mesh->getPolyArea(straight_path_polys[j], &area_type);
359 area.emplace_back(area_type);
366 std::vector<carla::geom::Location> &path, std::vector<unsigned char> &area) {
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;
386 float poly_pick_ext[3] = {2,4,2};
393 const dtQueryFilter *filter;
396 std::lock_guard<std::mutex> lock(
_mutex);
397 filter =
_crowd->getFilter(
_crowd->getAgent(it->second)->params.queryFilterType);
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 };
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);
411 if (!start_ref || !end_ref) {
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);
423 if (num_polys == 0) {
430 dtVcopy(end_pos2, end_pos);
431 if (polys[num_polys - 1] != end_ref) {
433 std::lock_guard<std::mutex> lock(
_mutex);
434 _nav_query->closestPointOnPoly(polys[num_polys - 1], end_pos, end_pos2, 0);
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);
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) {
452 path.emplace_back(straight_path[i], straight_path[i + 2], straight_path[i + 1]);
456 std::lock_guard<std::mutex> lock(
_mutex);
457 _nav_mesh->getPolyArea(straight_path_polys[j], &area_type);
459 area.emplace_back(area_type);
467 dtCrowdAgentParams params;
477 memset(¶ms, 0,
sizeof(params));
480 params.maxAcceleration = 160.0f;
481 params.maxSpeed = 1.47f;
482 params.collisionQueryRange = 10;
483 params.obstacleAvoidanceType = 3;
484 params.separationWeight = 0.5f;
488 params.queryFilterType = 1;
490 params.queryFilterType = 0;
494 params.updateFlags = 0;
501 float point_from[3] = { from.
x, from.
z - (AGENT_HEIGHT / 2.0f), from.
y };
506 std::lock_guard<std::mutex> lock(
_mutex);
507 index =
_crowd->addAgent(point_from, ¶ms);
529 dtCrowdAgentParams params;
563 int index = it->second;
569 std::lock_guard<std::mutex> lock(
_mutex);
570 agent =
_crowd->getEditableAgent(index);
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;
596 memset(¶ms, 0,
sizeof(params));
599 params.maxAcceleration = 0.0f;
600 params.maxSpeed = 1.47f;
601 params.collisionQueryRange = 0;
602 params.obstacleAvoidanceType = 0;
603 params.separationWeight = 100.0f;
606 params.updateFlags = 0;
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;
634 std::lock_guard<std::mutex> lock(
_mutex);
635 index =
_crowd->addAgent(point_from, ¶ms);
637 logging::log(
"Vehicle agent not added to the crowd by some problem!");
642 dtCrowdAgent *agent =
_crowd->getEditableAgent(index);
644 agent->state = DT_CROWDAGENT_STATE_WALKING;
671 std::lock_guard<std::mutex> lock(
_mutex);
672 _crowd->removeAgent(it->second);
688 std::lock_guard<std::mutex> lock(
_mutex);
689 _crowd->removeAgent(it->second);
703 std::unordered_set<carla::rpc::ActorId> updated;
707 updated.insert(entry.first);
711 for (
auto &&entry : vehicles) {
715 updated.erase(entry.id);
719 for (
auto &&entry : updated) {
746 std::lock_guard<std::mutex> lock(
_mutex);
747 dtCrowdAgent *agent =
_crowd->getEditableAgent(it->second);
749 agent->params.maxSpeed = max_speed;
807 float point_to[3] = { to.
x, to.
z, to.
y };
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);
820 res =
_crowd->requestMoveTarget(index, target_ref, point_to);
840 std::lock_guard<std::mutex> lock(
_mutex);
851 int total_unblocked = 0;
855 std::lock_guard<std::mutex> lock(
_mutex);
856 total_agents =
_crowd->getAgentCount();
858 const dtCrowdAgent *ag;
859 for (
int i = 0; i < total_agents; ++i) {
862 std::lock_guard<std::mutex> lock(
_mutex);
865 if (!ag->active || ag->paused) {
870 if (!ag->params.useObb && !ag->paused) {
871 bool reset_target_pos =
false;
872 bool use_same_filter =
false;
881 if (d < AGENT_UNBLOCK_DISTANCE_SQUARED) {
883 reset_target_pos =
true;
884 use_same_filter =
true;
890 if (reset_target_pos) {
892 if (!use_same_filter) {
931 int index = it->second;
937 const dtCrowdAgent *agent;
940 std::lock_guard<std::mutex> lock(
_mutex);
941 agent =
_crowd->getAgent(index);
944 if (!agent->active) {
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]);
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]);
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;
972 (shortest_angle * rotation_speed *
static_cast<float>(
_delta_seconds));
995 int index = it->second;
1001 const dtCrowdAgent *agent;
1004 std::lock_guard<std::mutex> lock(
_mutex);
1005 agent =
_crowd->getAgent(index);
1008 if (!agent->active) {
1013 location.
x = agent->npos[0];
1014 location.
y = agent->npos[2];
1015 location.
z = agent->npos[1];
1036 int index = it->second;
1042 const dtCrowdAgent *agent;
1045 std::lock_guard<std::mutex> lock(
_mutex);
1046 agent =
_crowd->getAgent(index);
1049 return sqrt(agent->vel[0] * agent->vel[0] + agent->vel[1] * agent->vel[1] + agent->vel[2] *
1064 dtQueryFilter filter2;
1065 if (filter ==
nullptr) {
1072 dtPolyRef random_ref { 0 };
1073 float point[3] { 0.0f, 0.0f, 0.0f };
1078 std::lock_guard<std::mutex> lock(
_mutex);
1080 status =
_nav_query->findRandomPoint(filter,
frand, &random_ref, point);
1082 if (status == DT_SUCCESS) {
1083 location.
x = point[0];
1084 location.
y = point[2];
1085 location.
z = point[1];
1088 }
while (status != DT_SUCCESS && rounds > 0);
1091 return (rounds > 0);
1098 dtCrowdAgent *agent;
1101 std::lock_guard<std::mutex> lock(
_mutex);
1102 agent =
_crowd->getEditableAgent(agent_index);
1104 agent->params.queryFilterType =
static_cast<unsigned char>(filter_index);
1132 int index = it->second;
1138 dtCrowdAgent *agent;
1141 std::lock_guard<std::mutex> lock(
_mutex);
1142 agent =
_crowd->getEditableAgent(index);
1146 agent->paused = pause;
1159 float dir[3] = { direction.
x, direction.
z, direction.
y };
1163 std::lock_guard<std::mutex> lock(
_mutex);
1164 result =
_crowd->hasVehicleNear(it->second, distance * distance, dir,
false);
1180 dtCrowdAgent *agent;
1183 std::lock_guard<std::mutex> lock(
_mutex);
1184 agent =
_crowd->getEditableAgent(it->second);
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;
static const float AGENT_RADIUS
void SetNav(Navigation *nav)
assign the navigation module
std::unordered_map< ActorId, int > _mapped_vehicles_id
std::vector< uint8_t > _binary_mesh
std::unordered_map< int, ActorId > _mapped_by_index
static void log(Args &&... args)
static const float AGENT_UNBLOCK_DISTANCE_SQUARED
static const int MAX_AGENTS
bool AddWalker(ActorId id, carla::geom::Location from)
create a new walker
bool RemoveAgent(ActorId id)
remove an agent
Vector3D extent
Half the size of the BoundingBox in local space.
bool GetWalkerPosition(ActorId id, carla::geom::Location &location)
get the walker current location
dtNavMeshQuery * _nav_query
std::unordered_map< ActorId, int > _mapped_walkers_id
mapping Id
float SquaredLength() const
void PauseAgent(ActorId id, bool pause)
set an agent as paused for the crowd
static const int MAX_QUERY_SEARCH_NODES
This file contains definitions of common data structures used in traffic manager. ...
Represents the state of all the actors of an episode at a given frame.
bool HasVehicleNear(ActorId id, float distance, carla::geom::Location direction)
return if the agent has a vehicle near (as neighbour)
bool SetWalkerTarget(ActorId id, carla::geom::Location to)
set a new target point to go through a route with events
bool RemoveWalker(ActorId id)
remove a walker route
bool SetWalkerLookAt(ActorId id, carla::geom::Location location)
make agent look at some location
carla::geom::BoundingBox bounding
WalkerManager _walker_manager
walker manager for the route planning with events
#define DEBUG_ASSERT(predicate)
carla::geom::Transform transform
bool AddOrUpdateVehicle(VehicleCollisionInfo &vehicle)
create a new vehicle in crowd to be avoided by walkers
bool SetWalkerDirectTarget(ActorId id, carla::geom::Location to)
std::unordered_map< ActorId, float > _yaw_walkers
store walkers yaw angle from previous tick
bool SetWalkerDirectTargetIndex(int index, carla::geom::Location to)
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
std::unordered_map< int, carla::geom::Vector3D > _walkers_blocked_position
saves the position of each actor at intervals and check if any is blocked
static const float AGENT_UNBLOCK_TIME
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
bool GetAgentRoute(ActorId id, carla::geom::Location from, carla::geom::Location to, std::vector< carla::geom::Location > &path, std::vector< unsigned char > &area)
const auto & GetTimestamp() const
bool GetWalkerTransform(ActorId id, carla::geom::Transform &trans)
get the walker current transform
static const int MAX_POLYS
static const float AGENT_UNBLOCK_DISTANCE
static constexpr T ToRadians(T deg)
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
float _probability_crossing
void SetSeed(unsigned int seed)
set the seed to use with random numbers
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
static const float AREA_GRASS_COST
bool SetWalkerRoute(ActorId id)
set a new route from its current position
static const float AREA_ROAD_COST
struct to send info about vehicles to the crowd
static const float AGENT_HEIGHT
dtNavMesh * _nav_mesh
meshes
bool AddWalker(ActorId id)
create a new walker route
bool Load(const std::string &filename)
load navigation data
bool UpdateVehicles(std::vector< VehicleCollisionInfo > vehicles)
add/update/delete vehicles in crowd
void CreateCrowd(void)
create the crowd object
static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle)