diff --git a/LibCarla/source/carla/AtomicList.h b/LibCarla/source/carla/AtomicList.h index a835944bf..23991797c 100644 --- a/LibCarla/source/carla/AtomicList.h +++ b/LibCarla/source/carla/AtomicList.h @@ -9,9 +9,9 @@ #include "carla/AtomicSharedPtr.h" #include "carla/NonCopyable.h" +#include #include #include -#include namespace carla { namespace client { diff --git a/LibCarla/source/carla/client/detail/WalkerNavigation.cpp b/LibCarla/source/carla/client/detail/WalkerNavigation.cpp index be34a4b55..8a9b3b521 100644 --- a/LibCarla/source/carla/client/detail/WalkerNavigation.cpp +++ b/LibCarla/source/carla/client/detail/WalkerNavigation.cpp @@ -99,71 +99,6 @@ namespace detail { // update the vehicles found _nav.UpdateVehicles(vehicles); - - /* - // draw vehicle bounding boxes for debug - if (_nav.GetCrowd() == nullptr) return; - 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 = std::move(carla::rpc::DebugShape::Line {p1, p2, 0.2}); - line1.color = { 0, 255, 0 }; - _client.DrawDebugShape(line1); - // line 2 - line1.primitive = std::move(carla::rpc::DebugShape::Line {p2, p3, 0.2}); - line1.color = { 255, 0, 0 }; - _client.DrawDebugShape(line1); - // line 3 - line1.primitive = std::move(carla::rpc::DebugShape::Line {p3, p4, 0.2}); - line1.color = { 0, 0, 255 }; - _client.DrawDebugShape(line1); - // line 4 - line1.primitive = std::move(carla::rpc::DebugShape::Line {p4, p1, 0.2}); - line1.color = { 255, 255, 0 }; - _client.DrawDebugShape(line1); - } - } - */ - /* - // draw speed text for debug - if (_nav.GetCrowd() == nullptr) return; - 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); - float speed = carla::geom::Vector3D(agent->vel[0], agent->vel[1], agent->vel[2]).Length(); - std::ostringstream out; - out << speed; - carla::rpc::DebugShape text; - text.life_time = 0.01f; - text.persistent_lines = false; - text.primitive = std::move(carla::rpc::DebugShape::String {p1, out.str(), false}); - text.color = { 0, 255, 0 }; - _client.DrawDebugShape(text); - } - } - */ } } // namespace detail diff --git a/LibCarla/source/carla/nav/Navigation.cpp b/LibCarla/source/carla/nav/Navigation.cpp index 009714279..da8c17b6a 100644 --- a/LibCarla/source/carla/nav/Navigation.cpp +++ b/LibCarla/source/carla/nav/Navigation.cpp @@ -21,14 +21,13 @@ namespace carla { namespace nav { enum SamplePolyFlags { - SAMPLE_POLYFLAGS_WALK = 0x01, // Ability to walk (ground, grass, - // road) + SAMPLE_POLYFLAGS_WALK = 0x01, // Ability to walk (sidewalks, crosswalks) SAMPLE_POLYFLAGS_SWIM = 0x02, // Ability to swim (water). SAMPLE_POLYFLAGS_DOOR = 0x04, // Ability to move through doors. SAMPLE_POLYFLAGS_JUMP = 0x08, // Ability to jump. SAMPLE_POLYFLAGS_DISABLED = 0x10, // Disabled polygon - SAMPLE_POLYFLAGS_ROAD = 0x20, // Ability to walk on road - SAMPLE_POLYFLAGS_GRASS = 0x40, // Ability to walk on grass + SAMPLE_POLYFLAGS_ROAD = 0x20, // Ability to walk on road + SAMPLE_POLYFLAGS_GRASS = 0x40, // Ability to walk on grass SAMPLE_POLYFLAGS_ALL = 0xffff // All abilities. }; @@ -211,7 +210,8 @@ namespace nav { // create and init _crowd = dtAllocCrowd(); - if (!_crowd->init(MAX_AGENTS, AGENT_RADIUS * 20, _navMesh)) { + const float maxAgentRadius = AGENT_RADIUS * 20; + if (!_crowd->init(MAX_AGENTS, maxAgentRadius, _navMesh)) { logging::log("Nav: failed to create crowd"); return; } @@ -375,7 +375,7 @@ namespace nav { float m_straightPath[MAX_POLYS * 3]; unsigned char m_straightPathFlags[MAX_POLYS]; dtPolyRef m_straightPathPolys[MAX_POLYS]; - int m_nstraightPath; + int m_nstraightPath = 0; int m_straightPathOptions = DT_STRAIGHTPATH_AREA_CROSSINGS; // polys in path @@ -390,11 +390,8 @@ namespace nav { DEBUG_ASSERT(_navQuery != nullptr); // point extension - float m_polyPickExt[3]; - m_polyPickExt[0] = 2; - m_polyPickExt[1] = 4; - m_polyPickExt[2] = 2; - + float m_polyPickExt[3] = {2,4,2}; + // get current filter from agent auto it = _mappedWalkersId.find(id); if (it == _mappedWalkersId.end()) @@ -430,7 +427,6 @@ namespace nav { } // get the path of points - m_nstraightPath = 0; if (m_npolys == 0) { return false; } @@ -618,10 +614,6 @@ namespace nav { // flags params.updateFlags = 0; - // params.updateFlags |= DT_CROWD_ANTICIPATE_TURNS; - // params.updateFlags |= DT_CROWD_OPTIMIZE_VIS; - // params.updateFlags |= DT_CROWD_OPTIMIZE_TOPO; - // params.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE; params.updateFlags |= DT_CROWD_SEPARATION; // update its oriented bounding box @@ -690,7 +682,6 @@ namespace nav { _crowd->removeAgent(it->second); } _walkerManager.RemoveWalker(id); - logging::log("Nav: removing walker agent", id); // remove from mapping _mappedWalkersId.erase(it); _mappedByIndex.erase(it->second); @@ -707,7 +698,6 @@ namespace nav { std::lock_guard lock(_mutex); _crowd->removeAgent(it->second); } - // logging::log("Nav: removing vehicle agent", id); // remove from mapping _mappedVehiclesId.erase(it); _mappedByIndex.erase(it->second); @@ -1080,8 +1070,8 @@ namespace nav { // filter dtQueryFilter filter2; if (filter == nullptr) { - filter2.setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED); - filter2.setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED | SAMPLE_POLYFLAGS_ROAD | SAMPLE_POLYFLAGS_GRASS); + filter2.setIncludeFlags(SAMPLE_POLYFLAGS_WALK); + filter2.setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED); filter = &filter2; } @@ -1090,21 +1080,16 @@ namespace nav { float point[3] { 0.0f, 0.0f, 0.0f }; { + dtStatus status; // critical section, force single thread running this std::lock_guard lock(_mutex); do { - 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]; - // check for max height (to avoid roofs, it is a workaround until next version) - if (maxHeight == -1.0f || (maxHeight >= 0.0f && location.z <= maxHeight)) { - break; - } - } - } while (1); + status = _navQuery->findRandomPoint(filter, frand, &randomRef, point); + // set the location in Unreal coords + location.x = point[0]; + location.y = point[2]; + location.z = point[1]; + } while (status != DT_SUCCESS); } return true; @@ -1164,7 +1149,7 @@ namespace nav { // mark agent->paused = pause; } - + bool Navigation::hasVehicleNear(ActorId id) { // get the internal index (walker or vehicle) auto it = _mappedWalkersId.find(id); diff --git a/LibCarla/source/carla/nav/Navigation.h b/LibCarla/source/carla/nav/Navigation.h index 3fae9dd5d..65b44c88d 100644 --- a/LibCarla/source/carla/nav/Navigation.h +++ b/LibCarla/source/carla/nav/Navigation.h @@ -30,7 +30,7 @@ namespace nav { SAMPLE_POLYAREA_DOOR, SAMPLE_POLYAREA_GRASS, SAMPLE_POLYAREA_JUMP, - SAMPLE_POLYAREA_CROSS + SAMPLE_POLYAREA_CROSS }; /// struct to send info about vehicles to the crowd @@ -127,7 +127,7 @@ namespace nav { mutable std::mutex _mutex; - float _probabilityCrossing { 0.05f }; + float _probabilityCrossing { 0.0f }; /// assign a filter index to an agent void SetAgentFilter(int agentIndex, int filterIndex); diff --git a/LibCarla/source/carla/nav/WalkerEvent.h b/LibCarla/source/carla/nav/WalkerEvent.h index f38e115bc..5686a6e22 100644 --- a/LibCarla/source/carla/nav/WalkerEvent.h +++ b/LibCarla/source/carla/nav/WalkerEvent.h @@ -16,28 +16,33 @@ namespace nav { class Navigation; class WalkerManager; + /// result of an event enum class EventResult : uint8_t { Continue, End, TimeOut }; + /// empty event that just ignores struct WalkerEventIgnore { }; + /// event to wait for a while struct WalkerEventWait { double time; WalkerEventWait(double duration) : time(duration) {}; }; + /// event to pause and check for near vehicles struct WalkerEventStopAndCheck { double time; WalkerEventStopAndCheck(double duration) : time(duration) {}; }; + /// walker event variant using WalkerEvent = boost::variant; - // visitor class + /// visitor class class WalkerEventVisitor { public: WalkerEventVisitor(WalkerManager *manager, ActorId id, double delta) : _manager(manager), _id(id), _delta(delta) {}; diff --git a/LibCarla/source/carla/nav/WalkerManager.h b/LibCarla/source/carla/nav/WalkerManager.h index cbc3edefc..f6729fdc0 100644 --- a/LibCarla/source/carla/nav/WalkerManager.h +++ b/LibCarla/source/carla/nav/WalkerManager.h @@ -7,45 +7,36 @@ #pragma once #include "carla/NonCopyable.h" -// #include "carla/AtomicList.h" -// #include "carla/client/detail/EpisodeState.h" -// #include "carla/geom/BoundingBox.h" + #include "carla/geom/Location.h" #include "carla/nav/WalkerEvent.h" -// #include "carla/geom/Transform.h" #include "carla/rpc/ActorId.h" -// #include -// #include -// #include -// #include -// #include -// #include namespace carla { namespace nav { - class Navigation; + class Navigation; - enum WalkerState { - WALKER_IDLE, - WALKER_WALKING, - WALKER_IN_EVENT, - WALKER_STOP - }; + enum WalkerState { + WALKER_IDLE, + WALKER_WALKING, + WALKER_IN_EVENT, + WALKER_STOP + }; - struct WalkerRoutePoint { - WalkerEvent event; - carla::geom::Location location; - WalkerRoutePoint(WalkerEvent ev, carla::geom::Location loc) : event(ev), location(loc) {}; - }; + struct WalkerRoutePoint { + WalkerEvent event; + carla::geom::Location location; + WalkerRoutePoint(WalkerEvent ev, carla::geom::Location loc) : event(ev), location(loc) {}; + }; - struct WalkerInfo { - carla::geom::Location from; - carla::geom::Location to; - unsigned int currentIndex { 0 }; - WalkerState state; - std::vector route; - }; + struct WalkerInfo { + carla::geom::Location from; + carla::geom::Location to; + unsigned int currentIndex { 0 }; + WalkerState state; + std::vector route; + }; class WalkerManager : private NonCopyable { @@ -54,34 +45,34 @@ namespace nav { WalkerManager(); ~WalkerManager(); - /// assign the navigation module - void SetNav(Navigation *nav) { _nav = nav; }; + /// assign the navigation module + void SetNav(Navigation *nav) { _nav = nav; }; - /// create a new walker route + /// create a new walker route bool AddWalker(ActorId id); - /// remove a walker route + /// remove a walker route bool RemoveWalker(ActorId id); - /// update all routes + /// update all routes bool Update(double delta); - /// set a new route from its current position + /// set a new route from its current position bool SetWalkerRoute(ActorId id); bool SetWalkerRoute(ActorId id, carla::geom::Location to); - /// set the next point in the route + /// set the next point in the route bool SetWalkerNextPoint(ActorId id); - /// return the navigation object - Navigation *GetNavigation() { return _nav; }; + /// return the navigation object + Navigation *GetNavigation() { return _nav; }; - private: + private: EventResult ExecuteEvent(ActorId id, WalkerInfo &info, double delta); std::unordered_map _walkers; - Navigation *_nav { nullptr }; + Navigation *_nav { nullptr }; }; } // namespace nav diff --git a/PythonAPI/examples/manual_control.py b/PythonAPI/examples/manual_control.py index 3122a19ab..2fd3285e1 100755 --- a/PythonAPI/examples/manual_control.py +++ b/PythonAPI/examples/manual_control.py @@ -190,8 +190,6 @@ class World(object): self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) else: - self.player_max_speed = 1.5 - self.player_max_speed_fast = 30 print("No recommended values for 'speed' attribute") # Spawn the player. if self.player is not None: @@ -384,8 +382,6 @@ class KeyboardControl(object): self._control.speed = .01 self._rotation.yaw += 0.08 * milliseconds if keys[K_UP] or keys[K_w]: - print(world.player_max_speed) - print(world.player_max_speed_fast) self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed self._control.jump = keys[K_SPACE] self._rotation.yaw = round(self._rotation.yaw, 1) diff --git a/PythonAPI/examples/spawn_npc.py b/PythonAPI/examples/spawn_npc.py index c6d56656f..b5c102d03 100755 --- a/PythonAPI/examples/spawn_npc.py +++ b/PythonAPI/examples/spawn_npc.py @@ -130,6 +130,9 @@ def main(): # ------------- # Spawn Walkers # ------------- + # some settings + percentagePedestriansRunning = 0.1 # how many pedestrians will run + percentagePedestriansCrossing = 0.01 # how many pedestrians will walk through the road # 1. take all the random locations to spawn spawn_points = [] for i in range(args.number_of_walkers): @@ -148,7 +151,7 @@ def main(): walker_bp.set_attribute('is_invincible', 'false') # set the max speed if walker_bp.has_attribute('speed'): - if (random.random() > 0.1): + if (random.random() > percentagePedestriansRunning): # walking walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) else: @@ -189,7 +192,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.01) + world.set_pedestrians_cross_factor(percentagePedestriansCrossing) for i in range(0, len(all_id), 2): # start walker all_actors[i].start() @@ -214,7 +217,7 @@ def main(): print('\ndestroying %d walkers' % len(walkers_list)) client.apply_batch([carla.command.DestroyActor(x) for x in all_id]) - + time.sleep(0.5) if __name__ == '__main__':