From 1c4b8c4beac47eac0c0cf9c6c8298884b2ae2bad Mon Sep 17 00:00:00 2001 From: bernatx Date: Wed, 23 Oct 2019 11:10:23 +0200 Subject: [PATCH] Pedestrians check if a vehicle is near before crossing --- LibCarla/source/carla/nav/Navigation.cpp | 27 ++++++++++++++++++--- LibCarla/source/carla/nav/Navigation.h | 2 ++ LibCarla/source/carla/nav/WalkerEvent.cpp | 6 ++--- LibCarla/source/carla/nav/WalkerEvent.h | 10 ++++++-- LibCarla/source/carla/nav/WalkerManager.cpp | 3 +-- Util/BuildTools/Setup.sh | 2 +- Util/InstallersWin/install_recast.bat | 2 +- 7 files changed, 39 insertions(+), 13 deletions(-) diff --git a/LibCarla/source/carla/nav/Navigation.cpp b/LibCarla/source/carla/nav/Navigation.cpp index 02685c8a3..d45f3047d 100644 --- a/LibCarla/source/carla/nav/Navigation.cpp +++ b/LibCarla/source/carla/nav/Navigation.cpp @@ -47,9 +47,9 @@ namespace nav { static const int MAX_AGENTS = 500; 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_RADIUS = 0.4f; - static const float AGENT_UNBLOCK_DISTANCE = 1.0f; + static const float AGENT_UNBLOCK_DISTANCE = 0.5f; static const float AGENT_UNBLOCK_DISTANCE_SQUARED = AGENT_UNBLOCK_DISTANCE * AGENT_UNBLOCK_DISTANCE; static const float AGENT_UNBLOCK_TIME = 2.0f; @@ -545,8 +545,8 @@ namespace nav { DEBUG_ASSERT(_crowd != nullptr); // get the bounding box extension plus some space around - float hx = vehicle.bounding.extent.x + 1.5f; - float hy = vehicle.bounding.extent.y + 1.5f; + float hx = vehicle.bounding.extent.x + 1.0f; + float hy = vehicle.bounding.extent.y + 1.0f; // define the 4 corners of the bounding box cg::Vector3D boxCorner1 {-hx, -hy, 0}; cg::Vector3D boxCorner2 { hx, -hy, 0}; @@ -1159,6 +1159,25 @@ namespace nav { // mark agent->paused = pause; } + + bool Navigation::hasVehicleNear(ActorId id) { + // get the internal index (walker or vehicle) + auto it = _mappedWalkersId.find(id); + if (it == _mappedWalkersId.end()) { + it = _mappedVehiclesId.find(id); + if (it == _mappedVehiclesId.end()) { + return false; + } + } + + bool result; + { + // critical section, force single thread running this + std::lock_guard lock(_mutex); + result = _crowd->hasVehicleNear(it->second); + } + return result; + } } // namespace nav } // namespace carla diff --git a/LibCarla/source/carla/nav/Navigation.h b/LibCarla/source/carla/nav/Navigation.h index a8d8f645a..3fae9dd5d 100644 --- a/LibCarla/source/carla/nav/Navigation.h +++ b/LibCarla/source/carla/nav/Navigation.h @@ -93,6 +93,8 @@ namespace nav { void SetPedestriansCrossFactor(float percentage); /// set an agent as paused for the crowd void PauseAgent(ActorId id, bool pause); + /// return if the agent has a vehicle near (as neighbour) + bool hasVehicleNear(ActorId id); dtCrowd *GetCrowd() { return _crowd; }; diff --git a/LibCarla/source/carla/nav/WalkerEvent.cpp b/LibCarla/source/carla/nav/WalkerEvent.cpp index 45269731e..03bf117d2 100644 --- a/LibCarla/source/carla/nav/WalkerEvent.cpp +++ b/LibCarla/source/carla/nav/WalkerEvent.cpp @@ -5,6 +5,7 @@ // For a copy, see . #include "carla/Logging.h" +#include "carla/nav/Navigation.h" #include "carla/nav/WalkerEvent.h" namespace carla { @@ -21,9 +22,8 @@ namespace nav { } bool WalkerEventVisitor::operator()(WalkerEventStopAndCheck &event, double delta) { - // refresh time and check - event.time -= delta; - return (event.time <= 0.0f); + // check if the agent has any vehicle around + return (!event.nav->hasVehicleNear(event.id)); } } // namespace nav diff --git a/LibCarla/source/carla/nav/WalkerEvent.h b/LibCarla/source/carla/nav/WalkerEvent.h index f8b2cf5a8..2ae758c8d 100644 --- a/LibCarla/source/carla/nav/WalkerEvent.h +++ b/LibCarla/source/carla/nav/WalkerEvent.h @@ -8,9 +8,14 @@ #include +#include "carla/rpc/ActorId.h" +// #include "carla/nav/Navigation.h" + namespace carla { namespace nav { + class Navigation; + struct WalkerEventIgnore { }; @@ -20,8 +25,9 @@ namespace nav { }; struct WalkerEventStopAndCheck { - double time; - WalkerEventStopAndCheck(double duration) : time(duration) {}; + ActorId id; + Navigation *nav; + WalkerEventStopAndCheck(ActorId id, Navigation *nav) : id(id), nav(nav) {}; }; using WalkerEvent = boost::variant; diff --git a/LibCarla/source/carla/nav/WalkerManager.cpp b/LibCarla/source/carla/nav/WalkerManager.cpp index 5abbfd144..deca7aa67 100644 --- a/LibCarla/source/carla/nav/WalkerManager.cpp +++ b/LibCarla/source/carla/nav/WalkerManager.cpp @@ -123,8 +123,7 @@ namespace nav { // stop and check case SAMPLE_POLYAREA_ROAD: case SAMPLE_POLYAREA_CROSS: - // info.route.emplace_back(WalkerEventStopAndCheck(4), std::move(path[i])); - info.route.emplace_back(WalkerEventIgnore(), std::move(path[i])); + info.route.emplace_back(WalkerEventStopAndCheck(id, _nav), std::move(path[i])); break; default: diff --git a/Util/BuildTools/Setup.sh b/Util/BuildTools/Setup.sh index 0041b3964..b9be7d3ec 100755 --- a/Util/BuildTools/Setup.sh +++ b/Util/BuildTools/Setup.sh @@ -289,7 +289,7 @@ unset GTEST_BASENAME # -- Get Recast&Detour and compile it with libc++ ------------------------------ # ============================================================================== -RECAST_COMMIT="163651bfa4aab314a1a3e7b1c476b3cbc5e4fdf1" +RECAST_COMMIT="8af33f0c5ae996fe4223d2d40852c5fd06cf9f36" RECAST_BASENAME=recast-${CXX_TAG} RECAST_INCLUDE=${PWD}/${RECAST_BASENAME}-install/include diff --git a/Util/InstallersWin/install_recast.bat b/Util/InstallersWin/install_recast.bat index 4bd787d83..4abebbda1 100644 --- a/Util/InstallersWin/install_recast.bat +++ b/Util/InstallersWin/install_recast.bat @@ -38,7 +38,7 @@ set RECAST_INSTALL=recast-install set RECAST_INSTALL_DIR=%BUILD_DIR%%RECAST_INSTALL%\ set RECAST_BUILD_DIR=%RECAST_SRC_DIR%build -set RECAST_COMMIT="163651bfa4aab314a1a3e7b1c476b3cbc5e4fdf1" +set RECAST_COMMIT="8af33f0c5ae996fe4223d2d40852c5fd06cf9f36" set RECAST_BASENAME=%RECAST_SRC%