Random points generation

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<float>(rand()) / static_cast<float>(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<float>(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

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<uint8_t> _binaryMesh;

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);