From 0ee3dd0305fa322fa141d70eb123862285b6fce5 Mon Sep 17 00:00:00 2001
From: bernatx <bernatx@gmail.com>
Date: Mon, 7 Oct 2019 15:32:58 +0200
Subject: [PATCH] Unblocking walkers when they are blocked by something

---
 LibCarla/source/carla/nav/Navigation.cpp | 42 ++++++++++++++++++++----
 LibCarla/source/carla/nav/Navigation.h   |  4 ++-
 PythonAPI/examples/spawn_npc.py          |  4 +--
 3 files changed, 41 insertions(+), 9 deletions(-)

diff --git a/LibCarla/source/carla/nav/Navigation.cpp b/LibCarla/source/carla/nav/Navigation.cpp
index 7385dae44..1d60fe0c8 100644
--- a/LibCarla/source/carla/nav/Navigation.cpp
+++ b/LibCarla/source/carla/nav/Navigation.cpp
@@ -55,6 +55,10 @@ namespace nav {
   static const int MAX_QUERY_SEARCH_NODES = 2048;
   static const float AGENT_HEIGHT = 1.8f;
   static const float AGENT_RADIUS = 0.3f;
+
+  static const float AGENT_LOW_SPEED = 0.5f;
+  static const float AGENT_LOW_SPEED_SQUARED = AGENT_LOW_SPEED * AGENT_LOW_SPEED;
+  static const float AGENT_LOW_SPEED_TIME = 3.0f;
   
   static const float AREA_WATER_COST = 10.0f;
 
@@ -67,6 +71,7 @@ namespace nav {
     _ready = false;
     _mappedWalkersId.clear();
     _mappedVehiclesId.clear();
+    _walkersBlockedTime.clear();
     _yaw_walkers.clear();
     _binaryMesh.clear();
     dtFreeCrowd(_crowd);
@@ -359,7 +364,7 @@ namespace nav {
     params.collisionQueryRange = params.radius * 15.0f;
     params.pathOptimizationRange = params.radius * 30.0f;
     params.obstacleAvoidanceType = 3;
-    params.separationWeight = 0.1f;
+    params.separationWeight = 0.3f;
         
     // set if the agent can cross roads or not
     if (frand() <= _probabilityCrossing) {
@@ -469,7 +474,7 @@ namespace nav {
     params.collisionQueryRange = params.radius * 20.0f;
     params.pathOptimizationRange = 0.0f;
     params.obstacleAvoidanceType = 3;
-    params.separationWeight = 0.1f;
+    params.separationWeight = 0.5f;
 
     // flags
     params.updateFlags = 0;
@@ -693,15 +698,40 @@ namespace nav {
         continue;
       }
 
+      // count the time the agent is quiet or in low speed
+      float speed = carla::geom::Vector3D(ag->nvel[0], ag->nvel[1], ag->nvel[2]).SquaredLength();
+      bool resetTargetPos = false;
+      bool useSameFilter = false;
+      if (speed <= AGENT_LOW_SPEED_SQUARED) {
+        // count time
+        _walkersBlockedTime[i] += _delta_seconds;
+        // check time
+        if (_walkersBlockedTime[i] >= AGENT_LOW_SPEED_TIME) {
+          resetTargetPos = true;
+          useSameFilter = true;
+          _walkersBlockedTime[i] = 0;
+        }
+      } else {
+        // reset time
+        _walkersBlockedTime[i] = 0;
+      }
+
       // 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) {
+        resetTargetPos = true;
+      }
+      
+      // check to assign a new target position
+      if (resetTargetPos) {
         // set if the agent can cross roads or not
-        if (frand() <= _probabilityCrossing) {
-          SetAgentFilter(i, 1);
-        } else {
-          SetAgentFilter(i, 0);
+        if (!useSameFilter) {
+          if (frand() <= _probabilityCrossing) {
+            SetAgentFilter(i, 1);
+          } else {
+            SetAgentFilter(i, 0);
+          }
         }
         // set a new random target
         carla::geom::Location location;
diff --git a/LibCarla/source/carla/nav/Navigation.h b/LibCarla/source/carla/nav/Navigation.h
index 713a2aeac..f983d4609 100644
--- a/LibCarla/source/carla/nav/Navigation.h
+++ b/LibCarla/source/carla/nav/Navigation.h
@@ -90,8 +90,10 @@ namespace nav {
     /// mapping Id
     std::unordered_map<ActorId, int> _mappedWalkersId;
     std::unordered_map<ActorId, int> _mappedVehiclesId;
-    /// Store walkers yaw angle from previous tick
+    /// store walkers yaw angle from previous tick
     std::unordered_map<ActorId, float> _yaw_walkers;
+    /// saves how much time an agent is quiet or in low speed (to avoid agents blocked by some reason)
+    std::unordered_map<int, float> _walkersBlockedTime;
 
     mutable std::mutex _mutex;
 
diff --git a/PythonAPI/examples/spawn_npc.py b/PythonAPI/examples/spawn_npc.py
index 2ee5cd55b..4b81779a7 100755
--- a/PythonAPI/examples/spawn_npc.py
+++ b/PythonAPI/examples/spawn_npc.py
@@ -148,7 +148,7 @@ def main():
                 walker_bp.set_attribute('is_invincible', 'false')
             # set the max speed
             if walker_bp.has_attribute('speed'):
-                walker_speed.append(random.choice(walker_bp.get_attribute('speed').recommended_values))
+                walker_speed.append(random.choice(walker_bp.get_attribute('speed').recommended_values[1:]))
             else:
                 print("Walker has no speed")
                 walker_speed.append(0.0)
@@ -184,7 +184,7 @@ def main():
 
         # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
         # set how many pedestrians can cross the road
-        world.set_pedestrians_cross_factor(0.05)
+        world.set_pedestrians_cross_factor(0.5)
         for i in range(0, len(all_id), 2):
             # start walker
             all_actors[i].start()