Fixing pedestrians collision with vehicles

LibCarla/source/carla/client/detail/WalkerNavigation.cpp

@@ -38,7 +38,7 @@ namespace detail {
     CheckIfWalkerExist(*walkers, *state);
 
     // add/update/delete all vehicles in crowd
-    UpdateVehiclesInCrowd(episode);
+    UpdateVehiclesInCrowd(episode, false);
 
     // update crowd in navigation module
     _nav.UpdateCrowd(*state);
@@ -79,7 +79,7 @@ namespace detail {
   }
 
   // add/update/delete all vehicles in crowd
-  void WalkerNavigation::UpdateVehiclesInCrowd(std::shared_ptr<Episode> episode) {
+  void WalkerNavigation::UpdateVehiclesInCrowd(std::shared_ptr<Episode> episode, bool show_debug) {
     std::vector<carla::nav::VehicleCollisionInfo> vehicles;
 
     // get current state
@@ -98,6 +98,70 @@ namespace detail {
 
     // update the vehicles found
     _nav.UpdateVehicles(vehicles);
+
+    // optional debug info
+    if (show_debug) {
+      if (_nav.GetCrowd() == nullptr) return;
+      
+      // draw bounding boxes for debug
+      for (int i = 0; i < _nav.GetCrowd()->getAgentCount(); ++i) {
+        // get the agent
+        const dtCrowdAgent *agent = _nav.GetCrowd()->getAgent(i);
+        if (agent && agent->params.useObb) {
+          // draw for debug
+          carla::geom::Location p1, p2, p3, p4;
+          p1.x = agent->params.obb[0];
+          p1.z = agent->params.obb[1];
+          p1.y = agent->params.obb[2];
+          p2.x = agent->params.obb[3];
+          p2.z = agent->params.obb[4];
+          p2.y = agent->params.obb[5];
+          p3.x = agent->params.obb[6];
+          p3.z = agent->params.obb[7];
+          p3.y = agent->params.obb[8];
+          p4.x = agent->params.obb[9];
+          p4.z = agent->params.obb[10];
+          p4.y = agent->params.obb[11];
+          carla::rpc::DebugShape line1;
+          line1.life_time = 0.01f;
+          line1.persistent_lines = false;
+          // line 1
+          line1.primitive = carla::rpc::DebugShape::Line {p1, p2, 0.2f};
+          line1.color = { 0, 255, 0 };
+          _client.DrawDebugShape(line1);
+          // line 2
+          line1.primitive = carla::rpc::DebugShape::Line {p2, p3, 0.2f};
+          line1.color = { 255, 0, 0 };
+          _client.DrawDebugShape(line1);
+          // line 3
+          line1.primitive = carla::rpc::DebugShape::Line {p3, p4, 0.2f};
+          line1.color = { 0, 0, 255 };
+          _client.DrawDebugShape(line1);
+          // line 4
+          line1.primitive = carla::rpc::DebugShape::Line {p4, p1, 0.2f};
+          line1.color = { 255, 255, 0 };
+          _client.DrawDebugShape(line1);
+        }
+      }
+
+      // draw some text for debug
+      for (int i = 0; i < _nav.GetCrowd()->getAgentCount(); ++i) {
+        // get the agent
+        const dtCrowdAgent *agent = _nav.GetCrowd()->getAgent(i);
+        if (agent) {
+          // draw for debug
+          carla::geom::Location p1(agent->npos[0], agent->npos[2], agent->npos[1] + 1);
+          std::ostringstream out;
+          out << *reinterpret_cast<const float *>(&agent->params.userData);
+          carla::rpc::DebugShape text;
+          text.life_time = 0.01f;
+          text.persistent_lines = false;
+          text.primitive = carla::rpc::DebugShape::String {p1, out.str(), false};
+          text.color = { 0, 255, 0 };
+          _client.DrawDebugShape(text);
+        }
+      }
+    }
   }
 
 } // namespace detail

LibCarla/source/carla/client/detail/WalkerNavigation.h

@@ -103,7 +103,7 @@ namespace detail {
     /// check a few walkers and if they don't exist then remove from the crowd
     void CheckIfWalkerExist(std::vector<WalkerHandle> walkers, const EpisodeState &state);
     /// add/update/delete all vehicles in crowd
-    void UpdateVehiclesInCrowd(std::shared_ptr<Episode> episode);
+    void UpdateVehiclesInCrowd(std::shared_ptr<Episode> episode, bool show_debug = false);
   };
 
 } // namespace detail

LibCarla/source/carla/nav/Navigation.cpp

@@ -531,12 +531,13 @@ namespace nav {
     DEBUG_ASSERT(_crowd != nullptr);
 
     // get the bounding box extension plus some space around
-    float hx = vehicle.bounding.extent.x + 0.6f;
-    float hy = vehicle.bounding.extent.y + 0.6f;
+    float marge = 0.8f;
+    float hx = vehicle.bounding.extent.x + marge;
+    float hy = vehicle.bounding.extent.y + marge;
     // define the 4 corners of the bounding box
     cg::Vector3D box_corner1 {-hx, -hy, 0};
-    cg::Vector3D box_corner2 { hx, -hy, 0};
-    cg::Vector3D box_corner3 { hx,  hy, 0};
+    cg::Vector3D box_corner2 { hx + 0.2f, -hy, 0};
+    cg::Vector3D box_corner3 { hx + 0.2f,  hy, 0};
     cg::Vector3D box_corner4 {-hx,  hy, 0};
     // rotate the points
     float angle = cg::Math::ToRadians(vehicle.transform.rotation.yaw);
@@ -592,9 +593,9 @@ namespace nav {
     params.height = AGENT_HEIGHT;
     params.maxAcceleration = 0.0f;
     params.maxSpeed = 1.47f;
-    params.collisionQueryRange = 1;
-    params.obstacleAvoidanceType = 3;
-    params.separationWeight = 1000.0f;
+    params.collisionQueryRange = 0;
+    params.obstacleAvoidanceType = 0;
+    params.separationWeight = 100.0f;
 
     // flags
     params.updateFlags = 0;

TrafficManager/CMakeLists.txt

@@ -15,12 +15,12 @@ set(CARLA_LOCATION $ENV{CARLA_LOCATION})
 include_directories(source/pipeline/)
 include_directories(${LIBCARLA_LOCATION}/source/)
 include_directories(${CARLA_LOCATION}/Build/boost-1.69.0-c7-install/include)
-include_directories(${CARLA_LOCATION}/Build/recast-88ae9d-c7-install/include)
+include_directories(${CARLA_LOCATION}/Build/recast-aca316-c7-install/include)
 include_directories(${CARLA_LOCATION}/Build/rpclib-v2.2.1_c2-c7-libstdcxx-install/include)
 
 link_directories(${LIBCARLA_LOCATION}/lib/)
 link_directories(${CARLA_LOCATION}/Build/boost-1.69.0-c7-install/lib)
-link_directories(${CARLA_LOCATION}/Build/recast-88ae9d-c7-install/lib)
+link_directories(${CARLA_LOCATION}/Build/recast-aca316-c7-install/lib)
 link_directories(${CARLA_LOCATION}/Build/rpclib-v2.2.1_c2-c7-libstdcxx-install/lib)
 link_directories(${CARLA_LOCATION}/PythonAPI/carla/dependencies/lib)
 

Util/BuildTools/Setup.sh

@@ -289,8 +289,8 @@ unset GTEST_BASENAME
 # -- Get Recast&Detour and compile it with libc++ ------------------------------
 # ==============================================================================
 
-RECAST_HASH=88ae9d
-RECAST_COMMIT=88ae9dd7017d382146e011a3c17ce844c24592f8
+RECAST_HASH=aca316
+RECAST_COMMIT=aca31602cd38f881e278bfe4179405265e785a68
 RECAST_BASENAME=recast-${RECAST_HASH}-${CXX_TAG}
 
 RECAST_INCLUDE=${PWD}/${RECAST_BASENAME}-install/include

Util/InstallersWin/install_recast.bat

@@ -32,8 +32,8 @@ if not "%1"=="" (
     goto :arg-parse
 )
 
-set RECAST_HASH=88ae9d
-set RECAST_COMMIT=88ae9dd7017d382146e011a3c17ce844c24592f8
+set RECAST_HASH=aca316
+set RECAST_COMMIT=aca31602cd38f881e278bfe4179405265e785a68
 set RECAST_SRC=recast-%RECAST_HASH%-src
 set RECAST_SRC_DIR=%BUILD_DIR%%RECAST_SRC%\
 set RECAST_INSTALL=recast-%RECAST_HASH%-install