From 8a6ab631f17c31f91d37d529ed3d195f9dcbbf11 Mon Sep 17 00:00:00 2001 From: bernatx Date: Mon, 17 Jun 2019 12:31:27 +0200 Subject: [PATCH] Fixes from PR --- .../carla/client/WalkerAIController.cpp | 22 +++++-- .../carla/client/detail/WalkerNavigation.cpp | 8 --- .../carla/client/detail/WalkerNavigation.h | 9 ++- LibCarla/source/carla/nav/Navigation.cpp | 57 +++++++++++++++++-- LibCarla/source/carla/nav/Navigation.h | 31 +--------- PythonAPI/examples/spawn_npc.py | 1 + 6 files changed, 79 insertions(+), 49 deletions(-) diff --git a/LibCarla/source/carla/client/WalkerAIController.cpp b/LibCarla/source/carla/client/WalkerAIController.cpp index e1fb6f0ea..e9e7bc318 100644 --- a/LibCarla/source/carla/client/WalkerAIController.cpp +++ b/LibCarla/source/carla/client/WalkerAIController.cpp @@ -21,24 +21,34 @@ namespace client { auto walker = GetParent(); if (walker != nullptr) { auto nav = GetEpisode().Lock()->GetNavigation(); - nav->AddWalker(walker->GetId(), location); + if (nav != nullptr) { + nav->AddWalker(walker->GetId(), location); + } } } geom::Location WalkerAIController::GetRandomLocation() { auto nav = GetEpisode().Lock()->GetNavigation(); - return nav->GetRandomLocation(); + if (nav != nullptr) { + return nav->GetRandomLocation(); + } + return geom::Location(0, 0, 0); } void WalkerAIController::GoToLocation(const carla::geom::Location &destination) { auto nav = GetEpisode().Lock()->GetNavigation(); - if (!nav->SetWalkerTarget(GetParent()->GetId(), destination)) - logging::log("NAV: Failed to set request to go to ", destination.x, destination.y, destination.z); + if (nav != nullptr) { + if (!nav->SetWalkerTarget(GetParent()->GetId(), destination)) + logging::log("NAV: Failed to set request to go to ", destination.x, destination.y, destination.z); + } } void WalkerAIController::SetMaxSpeed(const float max_speed) { - - logging::log("SET MAX SPEED: ", max_speed); + auto nav = GetEpisode().Lock()->GetNavigation(); + if (nav != nullptr) { + if (!nav->SetWalkerMaxSpeed(GetParent()->GetId(), max_speed)) + logging::log("NAV: failed to set max speed"); + } } } // namespace client diff --git a/LibCarla/source/carla/client/detail/WalkerNavigation.cpp b/LibCarla/source/carla/client/detail/WalkerNavigation.cpp index 4edd7ddd2..26f63e6a1 100644 --- a/LibCarla/source/carla/client/detail/WalkerNavigation.cpp +++ b/LibCarla/source/carla/client/detail/WalkerNavigation.cpp @@ -18,14 +18,6 @@ namespace detail { WalkerNavigation::WalkerNavigation(Client &client) : _client(client) { // Here call the server to retrieve the navmesh data. _nav.Load(_client.GetNavigationMesh()); - - // query the navigation to find a path of points - // std::vector Path; - // if (!_nav.GetPath(From, To, nullptr, Path)) { - // logging::log("NAV: Path not found"); - // } - // return Path; - } void WalkerNavigation::Tick(const EpisodeState &state) { diff --git a/LibCarla/source/carla/client/detail/WalkerNavigation.h b/LibCarla/source/carla/client/detail/WalkerNavigation.h index d7a7f4a91..cd23788af 100644 --- a/LibCarla/source/carla/client/detail/WalkerNavigation.h +++ b/LibCarla/source/carla/client/detail/WalkerNavigation.h @@ -46,16 +46,21 @@ namespace detail { // Get Random location in nav mesh geom::Location GetRandomLocation() { - geom::Location random_location; + geom::Location random_location(0, 0, 0); _nav.GetRandomLocation(random_location, 1.0f); return random_location; } // set a new target point to go - bool SetWalkerTarget(ActorId id, carla::geom::Location to) { + bool SetWalkerTarget(ActorId id, const carla::geom::Location to) { return _nav.SetWalkerTarget(id, to); } + // set new max speed + bool SetWalkerMaxSpeed(ActorId id, float max_speed) { + return _nav.SetWalkerMaxSpeed(id, max_speed); + } + private: Client &_client; diff --git a/LibCarla/source/carla/nav/Navigation.cpp b/LibCarla/source/carla/nav/Navigation.cpp index e9a5265e8..b9821b381 100644 --- a/LibCarla/source/carla/nav/Navigation.cpp +++ b/LibCarla/source/carla/nav/Navigation.cpp @@ -10,15 +10,42 @@ #include "carla/Logging.h" #include "carla/nav/Navigation.h" -#include #include #include namespace carla { namespace nav { + enum SamplePolyAreas { + SAMPLE_POLYAREA_GROUND, + SAMPLE_POLYAREA_WATER, + SAMPLE_POLYAREA_ROAD, + SAMPLE_POLYAREA_DOOR, + SAMPLE_POLYAREA_GRASS, + SAMPLE_POLYAREA_JUMP, + }; + + enum SamplePolyFlags { + SAMPLE_POLYFLAGS_WALK = 0x01, // Ability to walk (ground, grass, road) + SAMPLE_POLYFLAGS_SWIM = 0x02, // Ability to swim (water). + SAMPLE_POLYFLAGS_DOOR = 0x04, // Ability to move through doors. + SAMPLE_POLYFLAGS_JUMP = 0x08, // Ability to jump. + SAMPLE_POLYFLAGS_DISABLED = 0x10, // Disabled polygon + SAMPLE_POLYFLAGS_ALL = 0xffff // All abilities. + }; + + enum UpdateFlags + { + DT_CROWD_ANTICIPATE_TURNS = 1, + DT_CROWD_OBSTACLE_AVOIDANCE = 2, + DT_CROWD_SEPARATION = 4, + DT_CROWD_OPTIMIZE_VIS = 8, ///< Use #dtPathCorridor::optimizePathVisibility() to optimize the agent path. + DT_CROWD_OPTIMIZE_TOPO = 16, ///< Use dtPathCorridor::optimizePathTopology() to optimize the agent path. + }; + static const int MAX_POLYS = 256; static const int MAX_AGENTS = 500; + static const int MAX_QUERY_SEARCH_NODES = 2048; static const float AGENT_RADIUS = 0.3f; // return a random float @@ -26,9 +53,6 @@ namespace nav { return static_cast(rand()) / static_cast(RAND_MAX); } - Navigation::Navigation() { - } - Navigation::~Navigation() { _mappedId.clear(); _baseHeight.clear(); @@ -84,6 +108,10 @@ namespace nav { // allocate object dtNavMesh* mesh = dtAllocNavMesh(); + if (!mesh) { + return false; + } + // set number of tiles and origin dtStatus status = mesh->init(&header.params); if (dtStatusFailed(status)) { @@ -131,7 +159,7 @@ namespace nav { // prepare the query object dtFreeNavMeshQuery(_navQuery); _navQuery = dtAllocNavMeshQuery(); - _navQuery->init(_navMesh, 2048); + _navQuery->init(_navMesh, MAX_QUERY_SEARCH_NODES); // create and init the crowd manager CreateCrowd(); @@ -309,6 +337,23 @@ namespace nav { return true; } + // set new max speed + bool Navigation::SetWalkerMaxSpeed(ActorId id, float max_speed) { + // get the internal index + auto it = _mappedId.find(id); + if (it == _mappedId.end()) + return false; + + // get the agent + dtCrowdAgent *agent = _crowd->getEditableAgent(it->second); + if (agent) { + agent->params.maxSpeed = max_speed; + return true; + } + + return false; + } + // set a new target point to go bool Navigation::SetWalkerTarget(ActorId id, carla::geom::Location to) { // get the internal index @@ -459,6 +504,8 @@ namespace nav { location.z = point[1]; if (maxHeight == -1.0f || (maxHeight >= 0.0f && location.z <= maxHeight)) break; + } else { + throw_exception(std::runtime_error("no valid random point from navigation could be found, check filter or mesh.")); } } while (1); diff --git a/LibCarla/source/carla/nav/Navigation.h b/LibCarla/source/carla/nav/Navigation.h index 4461f90e6..9fda94f0d 100644 --- a/LibCarla/source/carla/nav/Navigation.h +++ b/LibCarla/source/carla/nav/Navigation.h @@ -21,37 +21,10 @@ namespace carla { namespace nav { - enum SamplePolyAreas { - SAMPLE_POLYAREA_GROUND, - SAMPLE_POLYAREA_WATER, - SAMPLE_POLYAREA_ROAD, - SAMPLE_POLYAREA_DOOR, - SAMPLE_POLYAREA_GRASS, - SAMPLE_POLYAREA_JUMP, - }; - - enum SamplePolyFlags { - SAMPLE_POLYFLAGS_WALK = 0x01, // Ability to walk (ground, grass, road) - SAMPLE_POLYFLAGS_SWIM = 0x02, // Ability to swim (water). - SAMPLE_POLYFLAGS_DOOR = 0x04, // Ability to move through doors. - SAMPLE_POLYFLAGS_JUMP = 0x08, // Ability to jump. - SAMPLE_POLYFLAGS_DISABLED = 0x10, // Disabled polygon - SAMPLE_POLYFLAGS_ALL = 0xffff // All abilities. - }; - - enum UpdateFlags - { - DT_CROWD_ANTICIPATE_TURNS = 1, - DT_CROWD_OBSTACLE_AVOIDANCE = 2, - DT_CROWD_SEPARATION = 4, - DT_CROWD_OPTIMIZE_VIS = 8, ///< Use #dtPathCorridor::optimizePathVisibility() to optimize the agent path. - DT_CROWD_OPTIMIZE_TOPO = 16, ///< Use dtPathCorridor::optimizePathTopology() to optimize the agent path. - }; - class Navigation { public: - Navigation(); + Navigation() = default; ~Navigation(); // load navigation data @@ -65,6 +38,8 @@ namespace nav { void CreateCrowd(void); // create a new walker bool AddWalker(ActorId id, carla::geom::Location from, float base_offset); + // set new max speed + bool SetWalkerMaxSpeed(ActorId id, float max_speed); // set a new target point to go bool SetWalkerTarget(ActorId id, carla::geom::Location to); bool SetWalkerTargetIndex(int index, carla::geom::Location to); diff --git a/PythonAPI/examples/spawn_npc.py b/PythonAPI/examples/spawn_npc.py index fedc20213..f44ff1e4d 100755 --- a/PythonAPI/examples/spawn_npc.py +++ b/PythonAPI/examples/spawn_npc.py @@ -178,6 +178,7 @@ def main(): # walk to random point target = world.get_random_location_from_navigation() all_actors[i].go_to_location(target) + all_actors[i].set_max_speed(1.3 + random.random() * 0.5) print('spawned %d walkers, press Ctrl+C to exit.' % len(info))