From 8138571ce866a1ff6ffede6de8b8ea5c2dad4aca Mon Sep 17 00:00:00 2001 From: bernatx Date: Tue, 4 Jun 2019 15:03:47 +0200 Subject: [PATCH] Random points generation --- LibCarla/source/carla/nav/Navigation.cpp | 84 ++++++++++++++++++------ LibCarla/source/carla/nav/Navigation.h | 4 ++ PythonAPI/examples/draw_path.py | 14 ++-- 3 files changed, 79 insertions(+), 23 deletions(-) diff --git a/LibCarla/source/carla/nav/Navigation.cpp b/LibCarla/source/carla/nav/Navigation.cpp index fa4ec3e31..64bea303a 100644 --- a/LibCarla/source/carla/nav/Navigation.cpp +++ b/LibCarla/source/carla/nav/Navigation.cpp @@ -20,6 +20,11 @@ namespace nav { static const float AGENT_HEIGHT = 2.0f; static const float AGENT_HEIGHT_HALF = AGENT_HEIGHT / 2.0f; + // return a random float + float frand() { + return static_cast(rand()) / static_cast(RAND_MAX); + } + Navigation::Navigation() { } @@ -192,7 +197,6 @@ namespace nav { int m_npolys; // check to load the binary _navMesh from server - logging::log("Nav: ", _binaryMesh.size()); if (_binaryMesh.size() == 0) { return false; } @@ -257,7 +261,6 @@ namespace nav { void Navigation::AddWalker(ActorId id) { // add to the queue to be added later and next tick where we can access the transform _walkersQueueToAdd.push_back(id); - logging::log("Nav: Added new walker in queue"); } // create a new walker in crowd @@ -268,6 +271,10 @@ namespace nav { return false; } + // random location + // GetRandomLocation(from); + // logging::log("Nav: from ", from.x, from.y, from.z); + // set from Unreal coordinates (and adjust center of walker, from middle to bottom) float y = from.y; from.y = from.z; @@ -290,7 +297,7 @@ namespace nav { params.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE; params.updateFlags |= DT_CROWD_SEPARATION; params.obstacleAvoidanceType = 3; - params.separationWeight = 0.5; + params.separationWeight = 0.1f; // add walker float PointFrom[3] = { from.x, from.y, from.z }; @@ -301,17 +308,8 @@ namespace nav { // save the id _mappedId[id] = index; - logging::log("Nav: Set index ", index, " to id ", id); - - int total = 0; - for (int i=0; i<_crowd->getAgentCount(); ++i) { - if (_crowd->getAgent(i)->active) - ++total; - } - logging::log("Nav: agents now ", total); return true; - } // set a new target point to go @@ -321,23 +319,24 @@ namespace nav { if (it == _mappedId.end()) return false; - // get the index found - int index = it->second; + return SetWalkerTargetIndex(it->second, to); + } + + // set a new target point to go + bool Navigation::SetWalkerTargetIndex(int index, carla::geom::Location to) { if (index == -1) return false; - logging::log("Nav: Found index ", index, " with id ", it->first); - // set target position float pointTo[3] = { to.x, to.z, to.y }; float nearest[3]; const dtQueryFilter *filter = _crowd->getFilter(0); dtPolyRef targetRef; _navQuery->findNearestPoly(pointTo, _crowd->getQueryHalfExtents(), filter, &targetRef, nearest); - if (targetRef) - _crowd->requestMoveTarget(index, targetRef, pointTo); + if (!targetRef) + return false; - return true; + return _crowd->requestMoveTarget(index, targetRef, pointTo); } // update all walkers in crowd @@ -365,6 +364,24 @@ namespace nav { // update all double deltaTime = state.GetTimestamp().delta_seconds; _crowd->update(static_cast(deltaTime), nullptr); + + // check if walker has finished + for (int i = 0; i<_crowd->getAgentCount(); ++i) + { + const dtCrowdAgent* ag = _crowd->getAgent(i); + if (!ag->active) + continue; + + // check distance to the target point + const float *end = &ag->cornerVerts[(ag->ncorners-1)*3]; + carla::geom::Vector3D dist(end[0] - ag->npos[0], end[1] - ag->npos[1], end[2] - ag->npos[2]); + if (dist.SquaredLength() <= 2) { + // set a new random target + carla::geom::Location location; + GetRandomLocation(location); + SetWalkerTargetIndex(i, location); + } + } } // get the walker current transform @@ -417,5 +434,34 @@ namespace nav { agent->vel[2]); } + // get a random location for navigation + bool Navigation::GetRandomLocation(carla::geom::Location &location, dtQueryFilter *filter) { + DEBUG_ASSERT(_navQuery != nullptr); + + // filter + dtQueryFilter filter2; + if (filter == nullptr) { + filter2.setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED); + filter2.setExcludeFlags(0); + filter = &filter2; + } + + // search + dtPolyRef randomRef { 0 }; + float point[3] { 0.0f, 0.0f, 0.0f }; + + dtStatus status = _navQuery->findRandomPoint(filter, &frand, &randomRef, point); + + if (status == DT_SUCCESS) { + // set the location in Unreal coords + location.x = point[0]; + location.y = point[2]; + location.z = point[1]; + return true; + } + + return false; + } + } // namespace nav } // namespace carla diff --git a/LibCarla/source/carla/nav/Navigation.h b/LibCarla/source/carla/nav/Navigation.h index 8ba7f1c1c..f71a60f2e 100644 --- a/LibCarla/source/carla/nav/Navigation.h +++ b/LibCarla/source/carla/nav/Navigation.h @@ -53,6 +53,7 @@ namespace nav { public: Navigation(); ~Navigation(); + // load navigation data bool Load(const std::string filename); // load navigation data from memory @@ -68,12 +69,15 @@ namespace nav { bool AddWalkerInCrowd(ActorId id, carla::geom::Location from); // set a new target point to go bool SetWalkerTarget(ActorId id, carla::geom::Location to); + bool SetWalkerTargetIndex(int index, carla::geom::Location to); // get the walker current transform bool GetWalkerTransform(ActorId id, carla::geom::Transform &trans); // get the walker current transform float GetWalkerSpeed(ActorId id); // update all walkers in crowd void UpdateCrowd(const client::detail::EpisodeState &state); + // get a random location for navigation + bool GetRandomLocation(carla::geom::Location &location, dtQueryFilter *filter = nullptr); private: std::vector _binaryMesh; diff --git a/PythonAPI/examples/draw_path.py b/PythonAPI/examples/draw_path.py index 79d2256a2..4de63ad65 100644 --- a/PythonAPI/examples/draw_path.py +++ b/PythonAPI/examples/draw_path.py @@ -75,7 +75,7 @@ def main(): # exit(1) - for i in range(10): + for i in range(50): spawn_point = carla.Transform() spawn_point.location.x = -25 + random.randint(-5, 5) spawn_point.location.y = 12 + random.randint(-5, 5) @@ -84,13 +84,19 @@ def main(): spawn_point.rotation.pitch = 0.0 blueprint = random.choice(world.get_blueprint_library().filter('walker.pedestrian.*')) - player = world.spawn_actor(blueprint, spawn_point) + while 1: + try: + player = world.spawn_actor(blueprint, spawn_point) + except: + pass + if (player): + break blueprint = random.choice(world.get_blueprint_library().filter('controller.ai.walker')) walker_controller = world.spawn_actor(blueprint, spawn_point, attach_to=player) walker_controller.start() - time.sleep(0.5) + time.sleep(0.4) walker_controller.go_to_location(carla.Location(-15, 32, 1.2)) - time.sleep(0.5) + # time.sleep(0.5) while (1): time.sleep(1);