diff --git a/CHANGELOG.md b/CHANGELOG.md index 27f7f2a94..26fc1026c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,11 +1,18 @@ ## latest + * Traffic Manager: + - Added benchmark + - Added synchronous mode + - Fixed change map error + - Added multiclient architecture + - Added multi Traffic Manager architecture + - Fixed linkage between waypoints + - Implemented intersection anticipation + - Implemented vehicle destruction when stuck + - Implemented tunable parameters * Added landmark class for signal-related queries. * Added support to parse OpenDRIVE signals. * Added junction class as queryable object from waypoint - * Fixed linkage between waypoints in InMemoryMap in Traffic Manager - * Vehicles get destroyed when they are stuck in Traffic Manager - * Implemented intersection anticipation algorithm in Traffic Manager * Added simple physical map generation from standalone OpenDRIVE data * Added support for new geometry: `spiral`, `poly3`, and `paramPoly3` * Improved `get_waypoint(location)` performance @@ -42,7 +49,7 @@ * Added new sensor: Radar * Exposed rgb camera attributes: exposure, depth of field, tonemapper, color correction, and chromatic aberration * Now all the camera-based sensors are provided with an additional parametrized lens distortion shader - * Added TrafficManager to replace autopilot in managing the NPC vehicles + * Added Traffic Manager to replace autopilot in managing the NPC vehicles * Improved pedestrians navigation * API changes: - Lidar: `range` is now set in meters, not in centimeters @@ -66,7 +73,6 @@ * Fixed an error in `automatic_control.py` failing because the `Num Lock` key * Fixed client_bounding_boxes.py example script * Fixed materials and semantic segmentation issues regarding importing assets - * Added TrafficManager to replace autopilot in managing the NPC vehicles * Fixed ObstacleSensor to return HitDistance instead of HitRadius ## CARLA 0.9.6 diff --git a/LibCarla/source/carla/Sockets.h b/LibCarla/source/carla/Sockets.h new file mode 100644 index 000000000..11346015d --- /dev/null +++ b/LibCarla/source/carla/Sockets.h @@ -0,0 +1,17 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#if _WIN32 + #include ///< socket + #include +#else + #include ///< socket + #include ///< sockaddr_in + #include ///< getsockname + #include ///< close +#endif diff --git a/LibCarla/source/carla/client/Client.h b/LibCarla/source/carla/client/Client.h index 7746b46b0..cd29370b1 100644 --- a/LibCarla/source/carla/client/Client.h +++ b/LibCarla/source/carla/client/Client.h @@ -6,13 +6,16 @@ #pragma once -#include "carla/PythonUtil.h" -#include "carla/client/World.h" #include "carla/client/detail/Simulator.h" +#include "carla/client/World.h" +#include "carla/PythonUtil.h" +#include "carla/trafficmanager/TrafficManager.h" namespace carla { namespace client { + using namespace carla::traffic_manager; + class Client { public: @@ -33,6 +36,10 @@ namespace client { _simulator->SetNetworkingTimeout(timeout); } + time_duration GetTimeout() { + return _simulator->GetNetworkingTimeout(); + } + /// Return the version string of this client API. std::string GetClientVersion() const { return _simulator->GetClientVersion(); @@ -64,6 +71,16 @@ namespace client { return World{_simulator->GetCurrentEpisode()}; } + /// Return an instance of the TrafficManager currently active in the simulator. + TrafficManager GetInstanceTM(uint16_t port = TM_DEFAULT_PORT) const { + return TrafficManager(_simulator->GetCurrentEpisode(), port); + } + + /// Return an instance of the Episode currently active in the simulator. + carla::client::detail::EpisodeProxy GetCurrentEpisode() const { + return _simulator->GetCurrentEpisode(); + } + std::string StartRecorder(std::string name) { return _simulator->StartRecorder(name); } @@ -111,6 +128,7 @@ namespace client { private: std::shared_ptr _simulator; + }; inline Client::Client( diff --git a/LibCarla/source/carla/client/Map.h b/LibCarla/source/carla/client/Map.h index 996e864e3..1ce41826d 100644 --- a/LibCarla/source/carla/client/Map.h +++ b/LibCarla/source/carla/client/Map.h @@ -56,9 +56,9 @@ namespace client { uint32_t lane_type = static_cast(road::Lane::LaneType::Driving)) const; SharedPtr GetWaypointXODR( - carla::road::RoadId road_id, - carla::road::LaneId lane_id, - float s) const; + carla::road::RoadId road_id, + carla::road::LaneId lane_id, + float s) const; using TopologyList = std::vector, SharedPtr>>; diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 501057442..74a5c5754 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/LibCarla/source/carla/client/Vehicle.cpp @@ -34,7 +34,7 @@ namespace client { _is_control_sticky(GetControlIsSticky(GetAttributes())) {} void Vehicle::SetAutopilot(bool enabled) { - TM &tm = TM::GetInstance(TM::GetUniqueLocalClient()); + TM tm(GetEpisode()); if (enabled) { tm.RegisterVehicles({shared_from_this()}); } else { diff --git a/LibCarla/source/carla/client/World.h b/LibCarla/source/carla/client/World.h index 2b07447c1..2124ff538 100644 --- a/LibCarla/source/carla/client/World.h +++ b/LibCarla/source/carla/client/World.h @@ -35,6 +35,8 @@ namespace client { explicit World(detail::EpisodeProxy episode) : _episode(std::move(episode)) {} + ~World(){} + World(const World &) = default; World(World &&) = default; @@ -127,6 +129,10 @@ namespace client { return DebugHelper{_episode}; } + detail::EpisodeProxy GetEpisode() const { + return _episode; + }; + private: detail::EpisodeProxy _episode; diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 97e33f9a2..8ebcaa192 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/LibCarla/source/carla/client/detail/Client.cpp @@ -102,6 +102,22 @@ namespace detail { const size_t worker_threads) : _pimpl(std::make_unique(host, port, worker_threads)) {} + bool Client::IsTrafficManagerRunning(uint16_t port) const { + return _pimpl->CallAndWait("is_traffic_manager_running", port); + } + + std::pair Client::GetTrafficManagerRunning(uint16_t port) const { + return _pimpl->CallAndWait>("get_traffic_manager_running", port); + }; + + bool Client::AddTrafficManagerRunning(std::pair trafficManagerInfo) const { + return _pimpl->CallAndWait("add_traffic_manager_running", trafficManagerInfo); + }; + + void Client::DestroyTrafficManager(uint16_t port) const { + _pimpl->AsyncCall("destroy_traffic_manager", port); + } + Client::~Client() = default; void Client::SetTimeout(time_duration timeout) { @@ -112,7 +128,7 @@ namespace detail { return _pimpl->GetTimeout(); } - const std::string &Client::GetEndpoint() const { + const std::string Client::GetEndpoint() const { return _pimpl->endpoint; } diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index 56e240a0b..fc471ae1f 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/LibCarla/source/carla/client/detail/Client.h @@ -62,11 +62,23 @@ namespace detail { ~Client(); + /// Querry to know if a Traffic Manager is running on port + bool IsTrafficManagerRunning(uint16_t port) const; + + /// Gets a pair filled with the of the Trafic Manager running on port. + /// If there is no Traffic Manager running the pair will be ("", 0) + std::pair GetTrafficManagerRunning(uint16_t port) const; + + /// Informs the server that a Traffic Manager is running on + bool AddTrafficManagerRunning(std::pair trafficManagerInfo) const; + + void DestroyTrafficManager(uint16_t port) const; + void SetTimeout(time_duration timeout); time_duration GetTimeout() const; - const std::string &GetEndpoint() const; + const std::string GetEndpoint() const; std::string GetClientVersion(); diff --git a/LibCarla/source/carla/client/detail/Episode.cpp b/LibCarla/source/carla/client/detail/Episode.cpp index 459c22206..8f12f26c0 100644 --- a/LibCarla/source/carla/client/detail/Episode.cpp +++ b/LibCarla/source/carla/client/detail/Episode.cpp @@ -10,6 +10,7 @@ #include "carla/client/detail/Client.h" #include "carla/client/detail/WalkerNavigation.h" #include "carla/sensor/Deserializer.h" +#include "carla/trafficmanager/TrafficManager.h" #include @@ -17,6 +18,8 @@ namespace carla { namespace client { namespace detail { +using namespace std::chrono_literals; + static auto &CastData(const sensor::SensorData &data) { using target_t = const sensor::data::RawEpisodeState; return static_cast(data); @@ -51,33 +54,52 @@ namespace detail { std::weak_ptr weak = shared_from_this(); _client.SubscribeToStream(_token, [weak](auto buffer) { auto self = weak.lock(); - if (self != nullptr) { - auto data = sensor::Deserializer::Deserialize(std::move(buffer)); + if (self != nullptr) { + + auto data = sensor::Deserializer::Deserialize(std::move(buffer)); auto next = std::make_shared(CastData(*data)); auto prev = self->GetState(); - do { - if (prev->GetFrame() >= next->GetFrame()) { - self->_on_tick_callbacks.Call(next); - return; + + /// Check for pending exceptions (Mainly TM server closed) + if(self->_pending_exceptions) { + + /// Mark pending exception false + self->_pending_exceptions = false; + + /// Create exception for the error message + auto exception(self->_pending_exceptions_msg); + // Notify waiting threads that exception occurred + self->_snapshot.SetException(std::runtime_error(exception)); + } + /// Sensor case: inconsistent data + else { + bool episode_changed = (next->GetEpisodeId() != prev->GetEpisodeId()); + + do { + if (prev->GetFrame() >= next->GetFrame()) { + self->_on_tick_callbacks.Call(next); + return; + } + } while (!self->_state.compare_exchange(&prev, next)); + + /// Episode change + if(episode_changed) { + self->OnEpisodeChanged(); } - } while (!self->_state.compare_exchange(&prev, next)); - if (next->GetEpisodeId() != prev->GetEpisodeId()) { - self->OnEpisodeStarted(); + // Notify waiting threads and do the callbacks. + self->_snapshot.SetValue(next); + + // Tick navigation. + auto navigation = self->_navigation.load(); + if (navigation != nullptr) { + navigation->Tick(self); + } + + // Call user callbacks. + self->_on_tick_callbacks.Call(next); } - - // Notify waiting threads and do the callbacks. - self->_snapshot.SetValue(next); - - // Tick navigation. - auto navigation = self->_navigation.load(); - if (navigation != nullptr) { - navigation->Tick(self); - } - - // Call user callbacks. - self->_on_tick_callbacks.Call(next); } }); } @@ -118,8 +140,13 @@ namespace detail { _actors.Clear(); _on_tick_callbacks.Clear(); _navigation.reset(); + traffic_manager::TrafficManager::Release(); } +void Episode::OnEpisodeChanged() { + traffic_manager::TrafficManager::Reset(); +} + } // namespace detail } // namespace client } // namespace carla diff --git a/LibCarla/source/carla/client/detail/Episode.h b/LibCarla/source/carla/client/detail/Episode.h index 2f0f0149b..521949311 100644 --- a/LibCarla/source/carla/client/detail/Episode.h +++ b/LibCarla/source/carla/client/detail/Episode.h @@ -85,18 +85,27 @@ namespace detail { nav->SetPedestriansCrossFactor(percentage); } + void AddPendingException(std::string e) { + _pending_exceptions = true; + _pending_exceptions_msg = e; + } + private: Episode(Client &client, const rpc::EpisodeInfo &info); void OnEpisodeStarted(); + void OnEpisodeChanged(); + Client &_client; AtomicSharedPtr _state; AtomicSharedPtr _navigation; + std::string _pending_exceptions_msg; + CachedActorList _actors; CallbackList _on_tick_callbacks; @@ -104,6 +113,8 @@ namespace detail { RecurrentSharedFuture _snapshot; const streaming::Token _token; + + bool _pending_exceptions = false; }; } // namespace detail diff --git a/LibCarla/source/carla/client/detail/EpisodeProxy.cpp b/LibCarla/source/carla/client/detail/EpisodeProxy.cpp index cd0fbd167..ea1825737 100644 --- a/LibCarla/source/carla/client/detail/EpisodeProxy.cpp +++ b/LibCarla/source/carla/client/detail/EpisodeProxy.cpp @@ -43,11 +43,6 @@ namespace detail { "trying to operate on a destroyed actor; an actor's function " "was called, but the actor is already destroyed.")); } - if (_episode_id != ptr->GetCurrentEpisodeId()) { - throw_exception(std::runtime_error( - "trying to access an expired episode; a new episode was started " - "in the simulation but an object tried accessing the old one.")); - } return ptr; } diff --git a/LibCarla/source/carla/client/detail/Simulator.cpp b/LibCarla/source/carla/client/detail/Simulator.cpp index ff8677663..1043ab7ad 100644 --- a/LibCarla/source/carla/client/detail/Simulator.cpp +++ b/LibCarla/source/carla/client/detail/Simulator.cpp @@ -16,6 +16,7 @@ #include "carla/client/TimeoutException.h" #include "carla/client/WalkerAIController.h" #include "carla/client/detail/ActorFactory.h" +#include "carla/trafficmanager/TrafficManager.h" #include "carla/sensor/Deserializer.h" #include @@ -47,6 +48,7 @@ namespace detail { while (frame > episode.GetState()->GetTimestamp().frame) { std::this_thread::yield(); } + carla::traffic_manager::TrafficManager::Tick(); } // =========================================================================== @@ -128,7 +130,6 @@ namespace detail { DEBUG_ASSERT(_episode != nullptr); const auto frame = _client.SendTickCue(); SynchronizeFrame(frame, *_episode); - RELEASE_ASSERT(frame == _episode->GetState()->GetTimestamp().frame); return frame; } diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index b007b0f48..84500064e 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -126,6 +126,10 @@ namespace detail { _client.SetTimeout(timeout); } + time_duration GetNetworkingTimeout() { + return _client.GetTimeout(); + } + std::string GetClientVersion() { return _client.GetClientVersion(); } @@ -160,6 +164,34 @@ namespace detail { // ========================================================================= /// @{ + std :: string GetEndpoint() { + return _client.GetEndpoint(); + } + + /// Query to know if a Traffic Manager is running on port + bool IsTrafficManagerRunning(uint16_t port) const { + return _client.IsTrafficManagerRunning(port); + } + + /// Gets a pair filled with the of the Trafic Manager running on port. + /// If there is no Traffic Manager running the pair will be ("", 0) + std::pair GetTrafficManagerRunning(uint16_t port) const { + return _client.GetTrafficManagerRunning(port); + } + + /// Informs that a Traffic Manager is running on + bool AddTrafficManagerRunning(std::pair trafficManagerInfo) const { + return _client.AddTrafficManagerRunning(trafficManagerInfo); + } + + void DestroyTrafficManager(uint16_t port) const { + _client.DestroyTrafficManager(port); + } + + void AddPendingException(std::string e) { + _episode->AddPendingException(e); + } + SharedPtr GetBlueprintLibrary(); SharedPtr GetSpectator(); diff --git a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h index 5525d0fea..4fc8adf40 100644 --- a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h +++ b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -42,6 +42,16 @@ namespace traffic_manager { return actor_list; } + std::vector GetIDList() { + + std::lock_guard lock(modification_mutex); + std::vector actor_list; + for (auto it = actor_set.begin(); it != actor_set.end(); ++it) { + actor_list.push_back(it->first); + } + return actor_list; + } + void Insert(std::vector actor_list) { std::lock_guard lock(modification_mutex); @@ -76,8 +86,16 @@ namespace traffic_manager { bool Contains(ActorId id) { + std::lock_guard lock(modification_mutex); return actor_set.find(id) != actor_set.end(); } + + size_t Size() { + + std::lock_guard lock(modification_mutex); + return actor_set.size(); + } + }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/AtomicMap.h b/LibCarla/source/carla/trafficmanager/AtomicMap.h index c8fc5e6ef..e28e37829 100644 --- a/LibCarla/source/carla/trafficmanager/AtomicMap.h +++ b/LibCarla/source/carla/trafficmanager/AtomicMap.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp index 1d1e49c5a..37ce8f614 100644 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp +++ b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp @@ -1,37 +1,39 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "BatchControlStage.h" +#include "carla/trafficmanager/BatchControlStage.h" namespace carla { namespace traffic_manager { - BatchControlStage::BatchControlStage( +BatchControlStage::BatchControlStage( std::string stage_name, std::shared_ptr messenger, - cc::Client &carla_client) + carla::client::detail::EpisodeProxy &episode_proxy, + Parameters ¶meters) : PipelineStage(stage_name), messenger(messenger), - carla_client(carla_client) { + episode_proxy_bcs(episode_proxy), + parameters(parameters) { - // Initializing number of vehicles to zero in the beginning. - number_of_vehicles = 0u; - } + // Initializing number of vehicles to zero in the beginning. + number_of_vehicles = 0u; +} - BatchControlStage::~BatchControlStage() {} +BatchControlStage::~BatchControlStage() {} - void BatchControlStage::Action() { +void BatchControlStage::Action() { - // Looping over registered actors. - for (uint64_t i = 0u; i < number_of_vehicles && data_frame != nullptr; ++i) { + // Looping over registered actors. + for (uint64_t i = 0u; i < number_of_vehicles && data_frame != nullptr; ++i) { - cr::VehicleControl vehicle_control; + carla::rpc::VehicleControl vehicle_control; const PlannerToControlData &element = data_frame->at(i); - if (!element.actor->IsAlive()) { + if (!element.actor || !element.actor->IsAlive()) { continue; } const carla::ActorId actor_id = element.actor->GetId(); @@ -40,38 +42,54 @@ namespace traffic_manager { vehicle_control.brake = element.brake; vehicle_control.steer = element.steer; - commands->at(i) = cr::Command::ApplyVehicleControl(actor_id, vehicle_control); - } + commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control); + } +} + +void BatchControlStage::DataReceiver() { + + data_frame = messenger->Peek(); + + // Allocating new containers for the changed number of registered vehicles. + if (data_frame != nullptr && number_of_vehicles != (*data_frame.get()).size()) { + + number_of_vehicles = static_cast((*data_frame.get()).size()); + + // Allocating array for command batching. + commands = std::make_shared>(number_of_vehicles); + } +} + +void BatchControlStage::DataSender() { + + messenger->Pop(); + bool synch_mode = parameters.GetSynchronousMode(); + + if (commands != nullptr) { + // Run asynchronous mode commands. + episode_proxy_bcs.Lock()->ApplyBatch(*commands.get(), false); } - void BatchControlStage::DataReceiver() { - - data_frame = messenger->Peek(); - - // Allocating new containers for the changed number of registered vehicles. - if (data_frame != nullptr && - number_of_vehicles != (*data_frame.get()).size()) { - - number_of_vehicles = static_cast((*data_frame.get()).size()); - // Allocating array for command batching. - commands = std::make_shared>(number_of_vehicles); - } - - } - - void BatchControlStage::DataSender() { - - messenger->Pop(); - - if (commands != nullptr) { - carla_client.ApplyBatch(*commands.get()); - - } - - // Limiting updates to 100 frames per second. + // Limiting updates to 100 frames per second. + if (!synch_mode) { std::this_thread::sleep_for(10ms); - + } else { + std::unique_lock lock(step_execution_mutex); + // Get timeout value in milliseconds. + double timeout = parameters.GetSynchronousModeTimeOutInMiliSecond(); + // Wait for service to finish. + step_execution_notifier.wait_for(lock, timeout * 1ms, [this]() { return run_step.load(); }); + run_step.store(false); } +} + + +bool BatchControlStage::RunStep() { + // Set run set flag. + run_step.store(true); + + return true; +} } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.h b/LibCarla/source/carla/trafficmanager/BatchControlStage.h index 29acfbac5..880962cb3 100644 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.h +++ b/LibCarla/source/carla/trafficmanager/BatchControlStage.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -6,56 +6,70 @@ #pragma once +#include +#include #include +#include -#include "carla/client/Client.h" +#include "carla/client/detail/EpisodeProxy.h" +#include "carla/client/detail/Simulator.h" #include "carla/Logging.h" #include "carla/rpc/ActorId.h" #include "carla/rpc/Command.h" #include "carla/rpc/VehicleControl.h" #include "carla/trafficmanager/MessengerAndDataTypes.h" +#include "carla/trafficmanager/Parameters.h" #include "carla/trafficmanager/PipelineStage.h" namespace carla { namespace traffic_manager { - namespace cc = carla::client; - namespace cr = carla::rpc; +using namespace std::literals::chrono_literals; - /// This class receives actuation signals (throttle, brake, steer) - /// from MotionPlannerStage class and communicates these signals to - /// the simulator in batches to control vehicles' movement. - class BatchControlStage : public PipelineStage { +/// This class receives actuation signals (throttle, brake, steer) +/// from Motion Planner Stage class and communicates these signals to +/// the simulator in batches to control vehicles' movement. +class BatchControlStage : public PipelineStage { - private: +private: - /// Pointer to frame received from MotionPlanner. - std::shared_ptr data_frame; - /// Pointer to a messenger from MotionPlanner. - std::shared_ptr messenger; - /// Reference to carla client connection object. - cc::Client &carla_client; - /// Array to hold command batch. - std::shared_ptr> commands; - /// Number of vehicles registered with the traffic manager. - uint64_t number_of_vehicles; + /// Pointer to frame received from Motion Planner. + std::shared_ptr data_frame; + /// Pointer to a messenger from Motion Planner. + std::shared_ptr messenger; + /// Reference to CARLA client connection object. + carla::client::detail::EpisodeProxy episode_proxy_bcs; + /// Array to hold command batch. + std::shared_ptr> commands; + /// Number of vehicles registered with the traffic manager. + uint64_t number_of_vehicles; + /// Parameter object for changing synchronous behaviour. + Parameters ¶meters; + /// Step runner flag. + std::atomic run_step; + /// Mutex for progressing synchronous execution. + std::mutex step_execution_mutex; + /// Condition variables for progressing synchronous execution. + std::condition_variable step_execution_notifier; - public: +public: - BatchControlStage( - std::string stage_name, - std::shared_ptr messenger, - cc::Client &carla_client); - ~BatchControlStage(); + BatchControlStage(std::string stage_name, + std::shared_ptr messenger, + carla::client::detail::EpisodeProxy &episode_proxy, + Parameters ¶meters); - void DataReceiver() override; + ~BatchControlStage(); - void Action() override; + void DataReceiver() override; - void DataSender() override; + void Action() override; - }; + void DataSender() override; + + bool RunStep(); +}; } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 35e65d8ed..a20069ad0 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -24,6 +24,7 @@ namespace CollisionStageConstants { static const float WALKER_TIME_EXTENSION = 1.5f; static const float EPSILON_VELOCITY = 0.1f; static const float INTER_BBOX_DISTANCE_THRESHOLD = 0.3f; + static const float BBOX_EXTENT_MULTIPLIER = 1.4f; } // namespace CollisionStageConstants using namespace CollisionStageConstants; @@ -38,7 +39,7 @@ namespace CollisionStageConstants { localization_messenger(localization_messenger), planner_messenger(planner_messenger), parameters(parameters), - debug_helper(debug_helper){ + debug_helper(debug_helper) { // Initializing clock for checking unregistered actors periodically. last_world_actors_pass_instance = chr::system_clock::now(); @@ -46,11 +47,14 @@ namespace CollisionStageConstants { frame_selector = true; // Initializing the number of vehicles to zero in the beginning. number_of_vehicles = 0u; + // Initializing srand. + srand(static_cast(time(NULL))); } CollisionStage::~CollisionStage() {} void CollisionStage::Action() { + const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; // Looping over registered actors. @@ -72,51 +76,44 @@ namespace CollisionStageConstants { bool collision_hazard = false; const SimpleWaypointPtr safe_point_junction = localization_frame->at(vehicle_id_to_index.at(ego_actor->GetId())).safe_point_after_junction; - // Generate number between 0 and 100 - const int r = rand() % 101; + // Check every actor in the vicinity if it poses a collision hazard. + for (auto j = overlapping_actors.begin(); (j != overlapping_actors.end()) && !collision_hazard; ++j) { - // Continue only if random number is lower than our %, default is 0. - if (parameters.GetPercentageIgnoreActors(boost::shared_ptr(ego_actor)) <= r) { - // Check every actor in the vicinity if it poses a collision hazard. - for (auto j = overlapping_actors.begin(); (j != overlapping_actors.end()) && !collision_hazard; ++j) { - try { - const Actor actor = j->second; - const ActorId actor_id = j->first; - const cg::Location other_location = actor->GetLocation(); + try { - // Collision checks increase with speed (Official formula used) - float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f); - collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS); + const Actor other_actor = j->second; + const auto other_actor_type = other_actor->GetTypeId(); + const ActorId other_actor_id = j->first; + const cg::Location other_location = other_actor->GetLocation(); - // Temporary fix to (0,0,0) bug - if (other_location.x != 0 && other_location.y != 0 && other_location.z != 0){ + // Collision checks increase with speed + float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f); + collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS); - if (actor_id != ego_actor_id && - (cg::Math::DistanceSquared(ego_location, other_location) - < std::pow(MAX_COLLISION_RADIUS, 2)) && - (std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) { + // Temporary fix to (0,0,0) bug + if (!(other_location.x == 0 && other_location.y == 0 && other_location.z == 0)) { - if (safe_point_junction != nullptr){ - if(parameters.GetCollisionDetection(ego_actor, actor) && - !IsLocationAfterJunctionSafe(ego_actor, actor, safe_point_junction)){ + if (other_actor_id != ego_actor_id && + (cg::Math::DistanceSquared(ego_location, other_location) + < std::pow(MAX_COLLISION_RADIUS, 2)) && + (std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) { + + if (parameters.GetCollisionDetection(ego_actor, other_actor)) { + + if((safe_point_junction != nullptr && !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction, other_location)) || + NegotiateCollision(ego_actor, other_actor, ego_location, other_location, closest_point, junction_look_ahead)) { + + if ((other_actor_type[0] == 'v' && parameters.GetPercentageIgnoreVehicles(ego_actor) <= (rand() % 101)) || + (other_actor_type[0] == 'w' && parameters.GetPercentageIgnoreWalkers(ego_actor) <= (rand() % 101))) { collision_hazard = true; - break; } } - - if (parameters.GetCollisionDetection(ego_actor, actor) && - NegotiateCollision(ego_actor, actor, closest_point, junction_look_ahead)) { - - collision_hazard = true; - break; - } } } - } catch (const std::exception &e) { - carla::log_info("Actor might not be alive \n"); } - + } catch (const std::exception &e) { + carla::log_info("Actor might not be alive \n"); } } @@ -126,31 +123,34 @@ namespace CollisionStageConstants { } void CollisionStage::DataReceiver() { + localization_frame = localization_messenger->Peek(); if (localization_frame != nullptr) { + // Connecting actor ids to their position indices on data arrays. - // This map also provides us the additional benefit of being able to - // quickly identify - // if a vehicle id is registered with the traffic manager or not. + // This map also provides us the additional benefit of being + // able to quickly identify if a vehicle id is registered + // with the traffic manager or not. vehicle_id_to_index.clear(); uint64_t index = 0u; for (auto &element: *localization_frame.get()) { vehicle_id_to_index.insert({element.actor->GetId(), index++}); } - // Allocating new containers for the changed number of registered - // vehicles. + // Allocating new containers for the changed number + // of registered vehicles. if (number_of_vehicles != (*localization_frame.get()).size()) { number_of_vehicles = static_cast((*localization_frame.get()).size()); + // Allocating output arrays to be shared with motion planner stage. planner_frame_a = std::make_shared(number_of_vehicles); planner_frame_b = std::make_shared(number_of_vehicles); } } - // Cleaning geodesic boundaries from last iteration. + // Cleaning geodesic boundaries from the last iteration. geodesic_boundaries.clear(); } @@ -163,20 +163,18 @@ namespace CollisionStageConstants { } bool CollisionStage::NegotiateCollision(const Actor &reference_vehicle, const Actor &other_vehicle, + const cg::Location &reference_location, const cg::Location &other_location, const SimpleWaypointPtr& closest_point, const SimpleWaypointPtr& junction_look_ahead) { bool hazard = false; - const cg::Location reference_location = reference_vehicle->GetLocation(); - const cg::Location other_location = other_vehicle->GetLocation(); - const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector(); cg::Vector3D reference_to_other = other_location - reference_location; reference_to_other = reference_to_other.MakeUnitVector(); const cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector(); - cg::Vector3D other_to_reference = reference_vehicle->GetLocation() - other_vehicle->GetLocation(); + cg::Vector3D other_to_reference = reference_location - other_location; other_to_reference = other_to_reference.MakeUnitVector(); const auto &waypoint_buffer = localization_frame->at( @@ -186,26 +184,30 @@ namespace CollisionStageConstants { const auto reference_vehicle_ptr = boost::static_pointer_cast(reference_vehicle); const auto other_vehicle_ptr = boost::static_pointer_cast(other_vehicle); - float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x * 1.414f; - float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * 1.414f; + float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x * BBOX_EXTENT_MULTIPLIER; + float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * BBOX_EXTENT_MULTIPLIER; float inter_vehicle_length = reference_vehicle_length + other_vehicle_length; + float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location); + float minimum_inter_vehicle_distance = std::pow(GetBoundingBoxExtention(reference_vehicle) + inter_vehicle_length, 2.0f); + if (!(!reference_front_wp->CheckJunction() && - cg::Math::Dot(reference_heading, reference_to_other) < 0) && + cg::Math::Dot(reference_heading, reference_to_other) < 0 && + inter_vehicle_distance > minimum_inter_vehicle_distance) && !(!closest_point->CheckJunction() && junction_look_ahead->CheckJunction() && reference_vehicle_ptr->GetVelocity().SquaredLength() < 0.1 && - reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green) && + reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green && + inter_vehicle_distance > minimum_inter_vehicle_distance) && !(!reference_front_wp->CheckJunction() && cg::Math::Dot(reference_heading, reference_to_other) > 0 && - (cg::Math::DistanceSquared(reference_location, other_location) > - std::pow(GetBoundingBoxExtention(reference_vehicle) + inter_vehicle_length, 2)))) { + inter_vehicle_distance > minimum_inter_vehicle_distance)) { - const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle)); - const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle)); - const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle)); - const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle)); + const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location)); + const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location)); + const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location)); + const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location)); const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon); const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon); @@ -228,6 +230,7 @@ namespace CollisionStageConstants { hazard = true; } } + return hazard; } @@ -237,6 +240,7 @@ namespace CollisionStageConstants { for (const cg::Location &location: boundary) { boundary_polygon_wkt += std::to_string(location.x) + " " + std::to_string(location.y) + ","; } + boundary_polygon_wkt += std::to_string(boundary[0].x) + " " + std::to_string(boundary[0].y); traffic_manager::Polygon boundary_polygon; @@ -245,18 +249,16 @@ namespace CollisionStageConstants { return boundary_polygon; } - LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor) { + LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location) { if (geodesic_boundaries.find(actor->GetId()) != geodesic_boundaries.end()) { return geodesic_boundaries.at(actor->GetId()); } - const LocationList bbox = GetBoundary(actor); + const LocationList bbox = GetBoundary(actor, vehicle_location); if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) { - const cg::Location vehicle_location = actor->GetLocation(); - float bbox_extension = GetBoundingBoxExtention(actor); const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor); @@ -351,10 +353,9 @@ namespace CollisionStageConstants { return bbox_extension; } - LocationList CollisionStage::GetBoundary(const Actor &actor) { + LocationList CollisionStage::GetBoundary(const Actor &actor, const cg::Location &location) { const auto actor_type = actor->GetTypeId(); - cg::Location location = actor->GetLocation(); cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); heading_vector.z = 0.0f; heading_vector = heading_vector.MakeUnitVector(); @@ -389,11 +390,11 @@ namespace CollisionStageConstants { return bbox_boundary; } - bool CollisionStage::IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor, const SimpleWaypointPtr safe_point){ + bool CollisionStage::IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &other_actor, const SimpleWaypointPtr safe_point , const cg::Location &other_location){ bool safe_junction = true; - if (overlapped_actor->GetVelocity().Length() < EPSILON_VELOCITY){ + if (other_actor->GetVelocity().Length() < EPSILON_VELOCITY){ cg::Location safe_location = safe_point->GetLocation(); cg::Vector3D heading_vector = safe_point->GetForwardVector(); @@ -418,7 +419,7 @@ namespace CollisionStageConstants { }; const Polygon reference_polygon = GetPolygon(ego_actor_boundary); - const Polygon other_polygon = GetPolygon(GetBoundary(overlapped_actor)); + const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location)); const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); if (inter_bbox_distance < INTER_BBOX_DISTANCE_THRESHOLD){ diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index 50a7205ce..923cceb2b 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -31,7 +31,6 @@ #include "carla/trafficmanager/MessengerAndDataTypes.h" #include "carla/trafficmanager/Parameters.h" -#include "carla/trafficmanager/PerformanceDiagnostics.h" #include "carla/trafficmanager/PipelineStage.h" namespace carla { @@ -49,9 +48,9 @@ namespace traffic_manager { using SimpleWaypointPtr = std::shared_ptr; using TLS = carla::rpc::TrafficLightState; - /// This class is the thread executable for the collision detection stage. - /// The class is responsible for checking possible collisions with other - /// vehicles along the vehicle's trajectory. + /// This class is the thread executable for the collision detection stage + /// and is responsible for checking possible collisions with other + /// cars along the vehicle's trajectory. class CollisionStage : public PipelineStage { private: @@ -83,11 +82,11 @@ namespace traffic_manager { SnippetProfiler snippet_profiler; /// Returns the bounding box corners of the vehicle passed to the method. - LocationList GetBoundary(const Actor &actor); + LocationList GetBoundary(const Actor &actor, const cg::Location &location); /// Returns the extrapolated bounding box of the vehicle along its /// trajectory. - LocationList GetGeodesicBoundary(const Actor &actor); + LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location); /// Method to construct a boost polygon object. Polygon GetPolygon(const LocationList &boundary); @@ -95,6 +94,7 @@ namespace traffic_manager { /// The method returns true if ego_vehicle should stop and wait for /// other_vehicle to pass. bool NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle, + const cg::Location &reference_location, const cg::Location &other_location, const SimpleWaypointPtr& closest_point, const SimpleWaypointPtr& junction_look_ahead); @@ -102,7 +102,8 @@ namespace traffic_manager { float GetBoundingBoxExtention(const Actor &ego_vehicle); /// At intersections, used to see if there is space after the junction - bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor, const SimpleWaypointPtr safe_point); + bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor, + const SimpleWaypointPtr safe_point, const cg::Location &other_location); /// A simple method used to draw bounding boxes around vehicles void DrawBoundary(const LocationList &boundary); diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index ea4b28f79..db36f31fa 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -1,10 +1,10 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "InMemoryMap.h" +#include "carla/trafficmanager/InMemoryMap.h" namespace carla { namespace traffic_manager { @@ -244,7 +244,7 @@ namespace MapConstants { } std::string InMemoryMap::MakeGridKey(std::pair grid_key) { - return std::to_string(grid_key.first) + std::to_string(grid_key.second); + return std::to_string(grid_key.first) + "#" + std::to_string(grid_key.second); } SimpleWaypointPtr InMemoryMap::GetWaypointInVicinity(cg::Location location) { diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.h b/LibCarla/source/carla/trafficmanager/InMemoryMap.h index 5ed78d666..a27f444f9 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.h +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index 82b125771..fbf461e89 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -1,10 +1,11 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "LocalizationStage.h" +#include "carla/trafficmanager/LocalizationStage.h" +#include "carla/client/DebugHelper.h" namespace carla { namespace traffic_manager { @@ -16,13 +17,13 @@ namespace LocalizationConstants { static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f; static const float TARGET_WAYPOINT_HORIZON_LENGTH = 5.0f; static const float MINIMUM_JUNCTION_LOOK_AHEAD = 10.0f; - static const float HIGHWAY_SPEED = 50 / 3.6f; + static const float HIGHWAY_SPEED = 50.0f / 3.6f; static const float MINIMUM_LANE_CHANGE_DISTANCE = 50.0f; static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f; static const uint64_t UNREGISTERED_ACTORS_SCAN_INTERVAL = 10; static const float BLOCKED_TIME_THRESHOLD = 90.0f; static const float DELTA_TIME_BETWEEN_DESTRUCTIONS = 10.0f; - static const float STOPPED_VELOCITY_THRESHOLD = 0.4f; // meters per second. + static const float STOPPED_VELOCITY_THRESHOLD = 0.8f; // meters per second. } // namespace LocalizationConstants @@ -36,8 +37,8 @@ namespace LocalizationConstants { AtomicActorSet ®istered_actors, InMemoryMap &local_map, Parameters ¶meters, - cc::DebugHelper &debug_helper, - cc::World& world) + carla::client::DebugHelper &debug_helper, + carla::client::detail::EpisodeProxy &episodeProxy) : PipelineStage(stage_name), planner_messenger(planner_messenger), collision_messenger(collision_messenger), @@ -46,7 +47,7 @@ namespace LocalizationConstants { local_map(local_map), parameters(parameters), debug_helper(debug_helper), - world(world) { + episode_proxy_ls(episodeProxy) { // Initializing various output frame selectors. planner_frame_selector = true; @@ -54,15 +55,15 @@ namespace LocalizationConstants { traffic_light_frame_selector = true; // Initializing the number of vehicles to zero in the begining. number_of_vehicles = 0u; - // Initializing the registered actors container state. registered_actors_state = -1; - // Initializing buffer lists. buffer_list = std::make_shared(); - // Initializing maximum idle time to null. maximum_idle_time = std::make_pair(nullptr, 0.0); + // Initializing srand. + srand(static_cast(time(NULL))); + } LocalizationStage::~LocalizationStage() {} @@ -78,7 +79,7 @@ namespace LocalizationConstants { traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b; // Selecting current timestamp from the world snapshot. - current_timestamp = world.GetSnapshot().GetTimestamp(); + current_timestamp = episode_proxy_ls.Lock()->GetWorldSnapshot().GetTimestamp(); // Looping over registered actors. for (uint64_t i = 0u; i < actor_list.size(); ++i) { @@ -100,17 +101,24 @@ namespace LocalizationConstants { if (buffer_list->find(actor_id) == buffer_list->end()) { buffer_list->insert({actor_id, Buffer()}); } + Buffer &waypoint_buffer = buffer_list->at(actor_id); + // Clear buffer if vehicle is too far from the first waypoint in the buffer. + if (!waypoint_buffer.empty() && + cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), vehicle_location) > 10.0f) { + waypoint_buffer.clear(); + } + // Purge passed waypoints. if (!waypoint_buffer.empty()) { - float dot_product = DeviationDotProduct(vehicle, waypoint_buffer.front()->GetLocation(), true); + float dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation(), true); while (dot_product <= 0.0f && !waypoint_buffer.empty()) { PopWaypoint(waypoint_buffer, actor_id); if (!waypoint_buffer.empty()) { - dot_product = DeviationDotProduct(vehicle, waypoint_buffer.front()->GetLocation(), true); + dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation(), true); } } } @@ -134,7 +142,7 @@ namespace LocalizationConstants { !front_waypoint->CheckJunction()) { SimpleWaypointPtr change_over_point = AssignLaneChange( - vehicle, force_lane_change, lane_change_direction); + vehicle, vehicle_location, force_lane_change, lane_change_direction); if (change_over_point != nullptr) { auto number_of_pops = waypoint_buffer.size(); @@ -183,8 +191,8 @@ namespace LocalizationConstants { target_waypoint = waypoint_buffer.at(j); } const cg::Location target_location = target_waypoint->GetLocation(); - float dot_product = DeviationDotProduct(vehicle, target_location); - float cross_product = DeviationCrossProduct(vehicle, target_location); + float dot_product = DeviationDotProduct(vehicle, vehicle_location, target_location); + float cross_product = DeviationCrossProduct(vehicle, vehicle_location, target_location); dot_product = 1.0f - dot_product; if (cross_product < 0.0f) { dot_product *= -1.0f; @@ -192,8 +200,8 @@ namespace LocalizationConstants { float distance = 0.0f; // TODO: use in PID - // Filtering out false junctions on highways. - // On highways, if there is only one possible path and the section is + // Filtering out false junctions on highways: + // on highways, if there is only one possible path and the section is // marked as intersection, ignore it. const auto vehicle_reference = boost::static_pointer_cast(vehicle); const float speed_limit = vehicle_reference->GetSpeedLimit(); @@ -212,7 +220,7 @@ namespace LocalizationConstants { bool approaching_junction = false; if (look_ahead_point->CheckJunction() && !(waypoint_buffer.front()->CheckJunction())) { - if (speed_limit > HIGHWAY_SPEED*3.6f) { + if (speed_limit*3.6f > HIGHWAY_SPEED) { for (uint64_t j = 0u; (j < look_ahead_index) && !approaching_junction; ++j) { SimpleWaypointPtr swp = waypoint_buffer.at(j); if (swp->GetNextWaypoint().size() > 1) { @@ -224,13 +232,13 @@ namespace LocalizationConstants { } } - // Reset the variables when no longer approaching an intersection + // Reset the variables when no longer approaching a junction. if (!approaching_junction && approached[actor_id]){ final_safe_points[actor_id] = nullptr; approached[actor_id] = false; } - // Only do once, when the intersection has just been seen. + // Only do once, when the junction has just been seen. else if (approaching_junction && !approached[actor_id]){ SimpleWaypointPtr final_point = nullptr; @@ -261,7 +269,9 @@ namespace LocalizationConstants { } else if (unregistered_actors.find(overlapping_actor_id) != unregistered_actors.end()) { actor_ptr = unregistered_actors.at(overlapping_actor_id); } - collision_message.overlapping_actors.insert({overlapping_actor_id, actor_ptr}); + if (actor_ptr!=nullptr) { + collision_message.overlapping_actors.insert({overlapping_actor_id, actor_ptr}); + } collision_message.safe_point_after_junction = final_safe_points[actor_id]; } collision_message.closest_waypoint = waypoint_buffer.front(); @@ -285,16 +295,58 @@ namespace LocalizationConstants { } void LocalizationStage::DataReceiver() { + bool is_deleted_actors_present = false; + std::set world_actor_id; + std::vector actor_list_to_be_deleted; - // Building a list of registered actors and - // connecting the vehicle ids to their position indices on data arrays. + // Filter function to collect the data. + auto Filter = [&](auto &actors, auto &wildcard_pattern) { + std::vector filtered; + for (auto &&actor : actors) { + if (carla::StringUtil::Match(carla::client::detail::ActorVariant(actor).GetTypeId(), wildcard_pattern)) { + filtered.push_back(actor); + } + } + return filtered; + }; - if (registered_actors_state != registered_actors.GetState()) { + // Get all the actors. + auto world_actors_list = episode_proxy_ls.Lock()->GetAllTheActorsInTheEpisode(); + // Filter with vehicle wildcard. + auto vehicles = Filter(world_actors_list, "vehicle.*"); + + // Building a set of vehicle ids in the world. + for (const auto &actor : vehicles) { + world_actor_id.insert(actor.GetId()); + } + + // Search for invalid/destroyed vehicles. + for (auto &actor : actor_list) { + if (world_actor_id.find(actor->GetId()) == world_actor_id.end()) { + actor_list_to_be_deleted.emplace_back(actor); + track_traffic.DeleteActor(actor->GetId()); + } + } + + // Clearing the registered actor list. + if(!actor_list_to_be_deleted.empty()) { + registered_actors.Remove(actor_list_to_be_deleted); + actor_list.clear(); + actor_list = registered_actors.GetList(); + is_deleted_actors_present = true; + } + + // Building a list of registered actors and connecting + // the vehicle ids to their position indices on data arrays. + + if (is_deleted_actors_present || (registered_actors_state != registered_actors.GetState())) { + actor_list.clear(); + actor_list_to_be_deleted.clear(); actor_list = registered_actors.GetList(); uint64_t index = 0u; vehicle_id_to_index.clear(); - for (auto &actor: actor_list) { + for (auto &actor : actor_list) { vehicle_id_to_index.insert({actor->GetId(), index}); ++index; } @@ -303,9 +355,7 @@ namespace LocalizationConstants { // Allocating new containers for the changed number of registered vehicles. if (number_of_vehicles != actor_list.size()) { - number_of_vehicles = static_cast(actor_list.size()); - // Allocating output frames to be shared with the motion planner stage. planner_frame_a = std::make_shared(number_of_vehicles); planner_frame_b = std::make_shared(number_of_vehicles); @@ -316,7 +366,6 @@ namespace LocalizationConstants { traffic_light_frame_a = std::make_shared(number_of_vehicles); traffic_light_frame_b = std::make_shared(number_of_vehicles); } - } void LocalizationStage::DataSender() { @@ -361,31 +410,48 @@ namespace LocalizationConstants { void LocalizationStage::ScanUnregisteredVehicles() { ++unregistered_scan_duration; - // Periodically check for actors not spawned by TrafficManager. - if (unregistered_scan_duration == UNREGISTERED_ACTORS_SCAN_INTERVAL) { - unregistered_scan_duration = 0; + // Periodically check for actors not spawned by TrafficManager. + if (unregistered_scan_duration == UNREGISTERED_ACTORS_SCAN_INTERVAL) { + unregistered_scan_duration = 0; - const auto world_actors = world.GetActors()->Filter("vehicle.*"); - const auto world_walker = world.GetActors()->Filter("walker.*"); - // Scanning for vehicles. - for (auto actor: *world_actors.get()) { - const auto unregistered_id = actor->GetId(); - if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() && - unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { - unregistered_actors.insert({unregistered_id, actor}); + auto Filter = [&](auto &actors, auto &wildcard_pattern) { + std::vector filtered; + for (auto &&actor : actors) { + if (carla::StringUtil::Match + ( carla::client::detail::ActorVariant(actor).GetTypeId() + , wildcard_pattern)) { + filtered.push_back(actor); } } - // Scanning for pedestrians. - for (auto walker: *world_walker.get()) { - const auto unregistered_id = walker->GetId(); - if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { - unregistered_actors.insert({unregistered_id, walker}); - } + return filtered; + }; + + /// Get all actors of the world + auto world_actors_list = episode_proxy_ls.Lock()->GetAllTheActorsInTheEpisode(); + + /// Filter based on wildcard_pattern + const auto world_actors = Filter(world_actors_list, "vehicle.*"); + const auto world_walker = Filter(world_actors_list, "walker.*"); + + // Scanning for vehicles. + for (auto actor: world_actors) { + const auto unregistered_id = actor.GetId(); + if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() && + unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { + unregistered_actors.insert({unregistered_id, actor.Get(episode_proxy_ls)}); } } + // Scanning for pedestrians. + for (auto walker: world_walker) { + const auto unregistered_id = walker.GetId(); + if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { + unregistered_actors.insert({unregistered_id, walker.Get(episode_proxy_ls)}); + } + } + } // Regularly update unregistered actors. - const auto current_snapshot = world.GetSnapshot(); + const auto current_snapshot = episode_proxy_ls.Lock()->GetWorldSnapshot(); for (auto it = unregistered_actors.cbegin(); it != unregistered_actors.cend();) { if (registered_actors.Contains(it->first) || !current_snapshot.Contains(it->first)) { track_traffic.DeleteActor(it->first); @@ -412,10 +478,9 @@ namespace LocalizationConstants { } } - SimpleWaypointPtr LocalizationStage::AssignLaneChange(Actor vehicle, bool force, bool direction) { + SimpleWaypointPtr LocalizationStage::AssignLaneChange(Actor vehicle, const cg::Location &vehicle_location, bool force, bool direction) { const ActorId actor_id = vehicle->GetId(); - const cg::Location vehicle_location = vehicle->GetLocation(); const float vehicle_velocity = vehicle->GetVelocity().Length(); const float speed_limit = boost::static_pointer_cast(vehicle)->GetSpeedLimit(); @@ -457,7 +522,7 @@ namespace LocalizationConstants { for (auto& candidate_lane_wp: other_neighbouring_lanes) { if (candidate_lane_wp != nullptr && track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0 && - speed_limit < HIGHWAY_SPEED) { + speed_limit*3.6f < HIGHWAY_SPEED) { distant_lane_availability = true; } } @@ -471,7 +536,7 @@ namespace LocalizationConstants { cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) { const float squared_vehicle_distance = cg::Math::DistanceSquared(other_location, vehicle_location); - const float deviation_dot = DeviationDotProduct(vehicle, other_location); + const float deviation_dot = DeviationDotProduct(vehicle, vehicle_location, other_location); if (deviation_dot > 0.0f) { @@ -556,16 +621,16 @@ SimpleWaypointPtr LocalizationStage::GetSafeLocationAfterJunction(const Vehicle // First Waypoint before the junction const SimpleWaypointPtr initial_point; - uint initial_index = 0; + uint64_t initial_index = 0; // First Waypoint after the junction SimpleWaypointPtr safe_point = nullptr; - uint safe_index = 0; + uint64_t safe_index = 0; // Vehicle position after the junction SimpleWaypointPtr final_point = nullptr; // Safe space after the junction const float safe_distance = 1.5f*length; - for (uint j = 0u; j < waypoint_buffer.size(); ++j){ + for (uint64_t j = 0u; j < waypoint_buffer.size(); ++j){ if (waypoint_buffer.at(j)->CheckJunction()){ initial_index = j; break; @@ -578,7 +643,7 @@ SimpleWaypointPtr LocalizationStage::GetSafeLocationAfterJunction(const Vehicle } // 2) Search for the end of the intersection (if it is in the buffer) - for (uint i = initial_index; i < waypoint_buffer.size(); ++i){ + for (uint64_t i = initial_index; i < waypoint_buffer.size(); ++i){ if (!waypoint_buffer.at(i)->CheckJunction()){ safe_point = waypoint_buffer.at(i); @@ -592,9 +657,9 @@ SimpleWaypointPtr LocalizationStage::GetSafeLocationAfterJunction(const Vehicle while (waypoint_buffer.back()->CheckJunction()) { std::vector next_waypoints = waypoint_buffer.back()->GetNextWaypoint(); - uint selection_index = 0u; + uint64_t selection_index = 0u; if (next_waypoints.size() > 1) { - selection_index = static_cast(rand()) % next_waypoints.size(); + selection_index = static_cast(rand()) % next_waypoints.size(); } waypoint_buffer.push_back(next_waypoints.at(selection_index)); @@ -609,8 +674,8 @@ SimpleWaypointPtr LocalizationStage::GetSafeLocationAfterJunction(const Vehicle } // 3) Search for final_point (again, if it is in the buffer) - - for(uint k = safe_index; k < waypoint_buffer.size(); ++k){ + + for(uint64_t k = safe_index; k < waypoint_buffer.size(); ++k){ if(safe_point->Distance(waypoint_buffer.at(k)->GetLocation()) > safe_distance){ final_point = waypoint_buffer.at(k); @@ -624,10 +689,10 @@ SimpleWaypointPtr LocalizationStage::GetSafeLocationAfterJunction(const Vehicle // Record the last point as a safe one and save it std::vector next_waypoints = waypoint_buffer.back()->GetNextWaypoint(); - uint selection_index = 0u; + uint64_t selection_index = 0u; // Pseudo-randomized path selection if found more than one choice. if (next_waypoints.size() > 1) { - selection_index = static_cast(rand()) % next_waypoints.size(); + selection_index = static_cast(rand()) % next_waypoints.size(); } waypoint_buffer.push_back(next_waypoints.at(selection_index)); diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index bea8fa068..0715ac0e2 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -15,6 +15,8 @@ #include #include +#include "carla/StringUtil.h" + #include "carla/client/Actor.h" #include "carla/client/Vehicle.h" #include "carla/geom/Location.h" @@ -33,6 +35,10 @@ #include "carla/trafficmanager/SimpleWaypoint.h" #include "carla/trafficmanager/PerformanceDiagnostics.h" +#include "carla/client/detail/ActorVariant.h" +#include "carla/client/detail/EpisodeProxy.h" +#include "carla/client/detail/Simulator.h" + namespace carla { namespace traffic_manager { @@ -84,8 +90,8 @@ namespace traffic_manager { Parameters ¶meters; /// Reference to Carla's debug helper object. cc::DebugHelper &debug_helper; - /// Carla world object; - cc::World& world; + /// Reference to carla client connection object. + carla::client::detail::EpisodeProxy episode_proxy_ls; /// Structures to hold waypoint buffers for all vehicles. /// These are shared with the collisions stage. std::shared_ptr buffer_list; @@ -119,7 +125,7 @@ namespace traffic_manager { void DrawBuffer(Buffer &buffer); /// Method to determine lane change and obtain target lane waypoint. - SimpleWaypointPtr AssignLaneChange(Actor vehicle, bool force, bool direction); + SimpleWaypointPtr AssignLaneChange(Actor vehicle, const cg::Location &vehicle_location, bool force, bool direction); // When near an intersection, extends the buffer throughout all the // intersection to see if there is space after it @@ -128,6 +134,7 @@ namespace traffic_manager { /// Methods to modify waypoint buffer and track traffic. void PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint); void PopWaypoint(Buffer& buffer, ActorId actor_id); + /// Method to scan for unregistered actors and update their grid positioning. void ScanUnregisteredVehicles(); @@ -140,15 +147,15 @@ namespace traffic_manager { public: LocalizationStage( - std::string stage_name, - std::shared_ptr planner_messenger, - std::shared_ptr collision_messenger, - std::shared_ptr traffic_light_messenger, - AtomicActorSet ®istered_actors, - InMemoryMap &local_map, - Parameters ¶meters, - cc::DebugHelper &debug_helper, - cc::World& world); + std::string stage_name, + std::shared_ptr planner_messenger, + std::shared_ptr collision_messenger, + std::shared_ptr traffic_light_messenger, + AtomicActorSet ®istered_actors, + InMemoryMap &local_map, + Parameters ¶meters, + carla::client::DebugHelper &debug_helper, + carla::client::detail::EpisodeProxy &episodeProxy); ~LocalizationStage(); diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp index 71b46c759..22d7474e9 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -15,13 +15,14 @@ namespace LocalizationConstants { using namespace LocalizationConstants; - float DeviationCrossProduct(Actor actor, const cg::Location &target_location) { + float DeviationCrossProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location) { cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); heading_vector.z = 0.0f; heading_vector = heading_vector.MakeUnitVector(); - cg::Location next_vector = target_location - actor->GetLocation(); + cg::Location next_vector = target_location - vehicle_location; next_vector.z = 0.0f; + if (next_vector.Length() > 2.0f * std::numeric_limits::epsilon()) { next_vector = next_vector.MakeUnitVector(); const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x; @@ -31,7 +32,7 @@ namespace LocalizationConstants { } } - float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset) { + float DeviationDotProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location, bool rear_offset) { cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); heading_vector.z = 0.0f; @@ -39,15 +40,16 @@ namespace LocalizationConstants { cg::Location next_vector; if (!rear_offset) { - next_vector = target_location - actor->GetLocation(); + next_vector = target_location - vehicle_location; } else { const auto vehicle_ptr = boost::static_pointer_cast(actor); const float vehicle_half_length = vehicle_ptr->GetBoundingBox().extent.x; next_vector = target_location - (cg::Location(-1* vehicle_half_length * heading_vector) - + vehicle_ptr->GetLocation()); + + vehicle_location); } next_vector.z = 0.0f; + if (next_vector.Length() > 2.0f * std::numeric_limits::epsilon()) { next_vector = next_vector.MakeUnitVector(); const float dot_product = cg::Math::Dot(next_vector, heading_vector); @@ -61,14 +63,14 @@ namespace LocalizationConstants { void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint) { - // Add actor entry if not present. + // Add actor entry, if not present. if (actor_to_grids.find(actor_id) == actor_to_grids.end()) { actor_to_grids.insert({actor_id, {}}); } std::unordered_set& current_grids = actor_to_grids.at(actor_id); - // Clear current actor from all grids containing current actor. + // Clear current actor from all grids containing itself. for (auto& grid_id: current_grids) { if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { ActorIdSet& actor_ids = grid_to_actors.at(grid_id); @@ -87,7 +89,7 @@ namespace LocalizationConstants { GeoGridId ggid = waypoint->GetGeodesicGridId(); current_grids.insert(ggid); - // Add grid entry if not present. + // Add grid entry, if not present. if (grid_to_actors.find(ggid) == grid_to_actors.end()) { grid_to_actors.insert({ggid, {}}); } @@ -102,28 +104,29 @@ namespace LocalizationConstants { void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer& buffer) { - // Add actor entry if not present. - if (actor_to_grids.find(actor_id) == actor_to_grids.end()) { - actor_to_grids.insert({actor_id, {}}); - } + if (!buffer.empty()) { - std::unordered_set& current_grids = actor_to_grids.at(actor_id); + // Add actor entry, if not present. + if (actor_to_grids.find(actor_id) == actor_to_grids.end()) { + actor_to_grids.insert({actor_id, {}}); + } - // Clear current actor from all grids containing current actor. - for (auto& grid_id: current_grids) { - if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { - ActorIdSet& actor_ids = grid_to_actors.at(grid_id); - if (actor_ids.find(actor_id) != actor_ids.end()) { - actor_ids.erase(actor_id); + std::unordered_set& current_grids = actor_to_grids.at(actor_id); + + // Clear current actor from all grids containing itself. + for (auto& grid_id: current_grids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + ActorIdSet& actor_ids = grid_to_actors.at(grid_id); + if (actor_ids.find(actor_id) != actor_ids.end()) { + actor_ids.erase(actor_id); + } } } - } - // Clear all grids current actor is tracking. - current_grids.clear(); + // Clear all grids the current actor is tracking. + current_grids.clear(); - // Step through buffer and update grid list for actor and actor list for grids. - if (!buffer.empty()) { + // Step through buffer and update grid list for actor and actor list for grids. uint64_t buffer_size = buffer.size(); uint64_t step_size = static_cast(std::floor(buffer_size/BUFFER_STEP_THROUGH)); for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) { diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h index f3e786aa4..63882ff8c 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -64,10 +64,10 @@ namespace traffic_manager { /// Returns the cross product (z component value) between the vehicle's /// heading vector and the vector along the direction to the next /// target waypoint on the horizon. - float DeviationCrossProduct(Actor actor, const cg::Location &target_location); + float DeviationCrossProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location); /// Returns the dot product between the vehicle's heading vector and /// the vector along the direction to the next target waypoint on the horizon. - float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset=false); + float DeviationDotProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location, bool rear_offset=false); } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/Messenger.h b/LibCarla/source/carla/trafficmanager/Messenger.h index e05ee758c..02049c3f5 100644 --- a/LibCarla/source/carla/trafficmanager/Messenger.h +++ b/LibCarla/source/carla/trafficmanager/Messenger.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h index 36cc37713..a0b9234ba 100644 --- a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h +++ b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp index 9df846b47..09aaa7cf7 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp @@ -1,10 +1,10 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "MotionPlannerStage.h" +#include "carla/trafficmanager/MotionPlannerStage.h" namespace carla { namespace traffic_manager { @@ -131,7 +131,6 @@ namespace PlannerConstants { state = current_state; // Constructing the actuation signal. - PlannerToControlData &message = current_control_frame->at(i); message.actor = actor; message.throttle = actuation_signal.throttle; @@ -169,10 +168,10 @@ namespace PlannerConstants { void MotionPlannerStage::DrawPIDValues(const boost::shared_ptr vehicle, const float throttle, const float brake) { - - debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,2.0f), + auto vehicle_location = vehicle->GetLocation(); + debug_helper.DrawString(vehicle_location + cg::Location(0.0f,0.0f,2.0f), std::to_string(throttle), false, {0u, 255u, 0u}, 0.005f); - debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,4.0f), + debug_helper.DrawString(vehicle_location + cg::Location(0.0f,0.0f,4.0f), std::to_string(brake), false, {255u, 0u, 0u}, 0.005f); } diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h index b09fb7d48..d7039b438 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -29,9 +29,8 @@ namespace traffic_manager { /// The class is responsible for aggregating information from various stages /// like the localization stage, traffic light stage, collision detection - /// stage - /// and actuation signals from the PID controller and makes decisions on - /// how to move the vehicle to follow it's trajectory safely. + /// stage and actuation signals from the PID controller and makes decisions + /// on how to move the vehicle to follow it's trajectory safely. class MotionPlannerStage : public PipelineStage { private: diff --git a/LibCarla/source/carla/trafficmanager/PIDController.cpp b/LibCarla/source/carla/trafficmanager/PIDController.cpp index 5c7dd55b3..83fb87870 100644 --- a/LibCarla/source/carla/trafficmanager/PIDController.cpp +++ b/LibCarla/source/carla/trafficmanager/PIDController.cpp @@ -1,10 +1,11 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "PIDController.h" +#include "carla/trafficmanager/PIDController.h" + #include namespace carla { @@ -15,6 +16,9 @@ namespace PIDControllerConstants { const float MAX_THROTTLE = 0.7f; const float MAX_BRAKE = 1.0f; + // PID will be stable only over 20 FPS. + const float dt = 1/20.0f; + } // namespace PIDControllerConstants using namespace PIDControllerConstants; @@ -39,13 +43,6 @@ namespace PIDControllerConstants { 0.0f }; - // Calculating dt for 'D' and 'I' controller components. - const chr::duration duration = current_state.time_instance - previous_state.time_instance; - (void) duration; //const float dt = duration.count(); Remove. - - // PID will be stable only over 20 FPS. - const float dt = 1/20.0f; - // Calculating integrals. current_state.deviation_integral = angular_deviation * dt + previous_state.deviation_integral; current_state.distance_integral = distance * dt + previous_state.distance_integral; @@ -60,10 +57,6 @@ namespace PIDControllerConstants { const std::vector &longitudinal_parameters, const std::vector &lateral_parameters) const { - // Calculating dt for updating the integral component. - const chr::duration duration = present_state.time_instance - previous_state.time_instance; - const float dt = duration.count(); - // Longitudinal PID calculation. const float expr_v = longitudinal_parameters[0] * present_state.velocity + diff --git a/LibCarla/source/carla/trafficmanager/PIDController.h b/LibCarla/source/carla/trafficmanager/PIDController.h index 79e8cc700..a4991f3fb 100644 --- a/LibCarla/source/carla/trafficmanager/PIDController.h +++ b/LibCarla/source/carla/trafficmanager/PIDController.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/Parameters.cpp b/LibCarla/source/carla/trafficmanager/Parameters.cpp index 7a22501f8..87290ef36 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.cpp +++ b/LibCarla/source/carla/trafficmanager/Parameters.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -6,35 +6,38 @@ #include -#include "Parameters.h" +#include "carla/trafficmanager/Parameters.h" namespace carla { namespace traffic_manager { -Parameters::Parameters() {} +Parameters::Parameters() { + + /// Set default synchronous mode to false. + synchronous_mode.store(false); + + /// Set default synchronous mode time out. + synchronous_time_out = std::chrono::duration(10); +} Parameters::~Parameters() {} void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { - float new_percentage = std::min(100.0f,percentage); + + float new_percentage = std::min(100.0f, percentage); percentage_difference_from_speed_limit.AddEntry({actor->GetId(), new_percentage}); } void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) { - float new_percentage = std::min(100.0f,percentage); + float new_percentage = std::min(100.0f, percentage); global_percentage_difference_from_limit = new_percentage; } -void Parameters::SetCollisionDetection( - const ActorPtr &reference_actor, - const ActorPtr &other_actor, - const bool detect_collision) { - +void Parameters::SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) { const ActorId reference_id = reference_actor->GetId(); const ActorId other_id = other_actor->GetId(); if (detect_collision) { - if (ignore_collision.Contains(reference_id)) { std::shared_ptr actor_set = ignore_collision.GetValue(reference_id); if (actor_set->Contains(other_id)) { @@ -42,7 +45,6 @@ void Parameters::SetCollisionDetection( } } } else { - if (ignore_collision.Contains(reference_id)) { std::shared_ptr actor_set = ignore_collision.GetValue(reference_id); if (!actor_set->Contains(other_id)) { @@ -71,12 +73,28 @@ void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) { } void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) { - - float new_distance = std::max(0.0f,distance); + + float new_distance = std::max(0.0f, distance); const auto entry = std::make_pair(actor->GetId(), new_distance); distance_to_leading_vehicle.AddEntry(entry); } +bool Parameters::GetSynchronousMode() { + return synchronous_mode.load(); +} + +void Parameters::SetSynchronousModeTimeOutInMiliSecond(const double time) { + synchronous_time_out = std::chrono::duration(time); +} + +double Parameters::GetSynchronousModeTimeOutInMiliSecond() { + return synchronous_time_out.count(); +} + +void Parameters::SetSynchronousMode(const bool mode_switch) { + synchronous_mode.store(mode_switch); +} + float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) { const ActorId actor_id = actor->GetId(); @@ -88,7 +106,7 @@ float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) { percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id); } - return speed_limit * (1.0f - percentage_difference/100.0f); + return speed_limit * (1.0f - percentage_difference / 100.0f); } bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor) { @@ -98,7 +116,7 @@ bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const Ac bool avoid_collision = true; if (ignore_collision.Contains(reference_actor_id) && - ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) { + ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) { avoid_collision = false; } @@ -144,17 +162,31 @@ float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) { } void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) { - - float new_perc = cg::Math::Clamp(perc,0.0f,100.0f); + + float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f); const auto entry = std::make_pair(actor->GetId(), new_perc); perc_run_traffic_light.AddEntry(entry); } -void Parameters::SetPercentageIgnoreActors(const ActorPtr &actor, const float perc) { +void Parameters::SetPercentageRunningSign(const ActorPtr &actor, const float perc) { + + float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f); + const auto entry = std::make_pair(actor->GetId(), new_perc); + perc_run_traffic_sign.AddEntry(entry); +} + +void Parameters::SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc) { + + float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f); + const auto entry = std::make_pair(actor->GetId(), new_perc); + perc_ignore_vehicles.AddEntry(entry); +} + +void Parameters::SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc) { float new_perc = cg::Math::Clamp(perc,0.0f,100.0f); const auto entry = std::make_pair(actor->GetId(), new_perc); - perc_ignore_actors.AddEntry(entry); + perc_ignore_walkers.AddEntry(entry); } float Parameters::GetPercentageRunningLight(const ActorPtr &actor) { @@ -169,13 +201,37 @@ float Parameters::GetPercentageRunningLight(const ActorPtr &actor) { return percentage; } -float Parameters::GetPercentageIgnoreActors(const ActorPtr &actor) { +float Parameters::GetPercentageRunningSign(const ActorPtr &actor) { const ActorId actor_id = actor->GetId(); float percentage = 0.0f; - if (perc_ignore_actors.Contains(actor_id)) { - percentage = perc_ignore_actors.GetValue(actor_id); + if (perc_run_traffic_sign.Contains(actor_id)) { + percentage = perc_run_traffic_sign.GetValue(actor_id); + } + + return percentage; +} + +float Parameters::GetPercentageIgnoreWalkers(const ActorPtr &actor) { + + const ActorId actor_id = actor->GetId(); + float percentage = 0.0f; + + if (perc_ignore_walkers.Contains(actor_id)) { + percentage = perc_ignore_walkers.GetValue(actor_id); + } + + return percentage; +} + +float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) { + + const ActorId actor_id = actor->GetId(); + float percentage = 0.0f; + + if (perc_ignore_vehicles.Contains(actor_id)) { + percentage = perc_ignore_vehicles.GetValue(actor_id); } return percentage; diff --git a/LibCarla/source/carla/trafficmanager/Parameters.h b/LibCarla/source/carla/trafficmanager/Parameters.h index b4e97ab92..70c5cc6e2 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.h +++ b/LibCarla/source/carla/trafficmanager/Parameters.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -6,6 +6,9 @@ #pragma once +#include +#include + #include "carla/client/Actor.h" #include "carla/client/Vehicle.h" #include "carla/Memory.h" @@ -45,21 +48,28 @@ namespace traffic_manager { AtomicMap auto_lane_change; /// Map containing % of running a traffic light. AtomicMap perc_run_traffic_light; - /// Map containing % of ignoring actors. - AtomicMap perc_ignore_actors; - + /// Map containing % of running a traffic sign. + AtomicMap perc_run_traffic_sign; + /// Map containing % of ignoring walkers. + AtomicMap perc_ignore_walkers; + /// Map containing % of ignoring vehicles. + AtomicMap perc_ignore_vehicles; + /// Synchronous mode switch. + std::atomic synchronous_mode; public: Parameters(); ~Parameters(); - /// Set target velocity specific to a vehicle. + /// Set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); - /// Set global target velocity. + /// Set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. void SetGlobalPercentageSpeedDifference(float const percentage); - /// Set collision detection rules between vehicles. + /// Method to set collision detection rules between vehicles. void SetCollisionDetection( const ActorPtr &reference_actor, const ActorPtr &other_actor, @@ -69,7 +79,7 @@ namespace traffic_manager { /// Direction flag can be set to true for left and false for right. void SetForceLaneChange(const ActorPtr &actor, const bool direction); - /// Enable / disable automatic lane change on a vehicle. + /// Enable/disable automatic lane change on a vehicle. void SetAutoLaneChange(const ActorPtr &actor, const bool enable); /// Method to specify how much distance a vehicle should maintain to @@ -91,17 +101,44 @@ namespace traffic_manager { /// Method to query distance to leading vehicle for a given vehicle. float GetDistanceToLeadingVehicle(const ActorPtr &actor); - /// Method to set % to run any traffic light. - void SetPercentageRunningLight(const ActorPtr &actor, const float perc); - - /// Method to set % to ignore any actor. - void SetPercentageIgnoreActors(const ActorPtr &actor, const float perc); + /// Method to get % to run any traffic light. + float GetPercentageRunningSign(const ActorPtr &actor); /// Method to get % to run any traffic light. float GetPercentageRunningLight(const ActorPtr &actor); - /// Method to get % to ignore any actor. - float GetPercentageIgnoreActors(const ActorPtr &actor); + /// Method to get % to ignore any vehicle. + float GetPercentageIgnoreVehicles(const ActorPtr &actor); + + /// Method to get % to ignore any walker. + float GetPercentageIgnoreWalkers(const ActorPtr &actor); + + /// Method to set % to run any traffic sign. + void SetPercentageRunningSign(const ActorPtr &actor, const float perc); + + /// Method to set % to run any traffic light. + void SetPercentageRunningLight(const ActorPtr &actor, const float perc); + + /// Method to set % to ignore any vehicle. + void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc); + + /// Method to set % to ignore any vehicle. + void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc); + + /// Method to get synchronous mode. + bool GetSynchronousMode(); + + /// Method to set synchronous mode. + void SetSynchronousMode(const bool mode_switch = true); + + /// Get synchronous mode time out + double GetSynchronousModeTimeOutInMiliSecond(); + + /// Set Synchronous mode time out. + void SetSynchronousModeTimeOutInMiliSecond(const double time); + + /// Synchronous mode time out variable. + std::chrono::duration synchronous_time_out; }; diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp index 26aba983e..117fdc9c1 100644 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp +++ b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h index 03018e863..cedc97116 100644 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h +++ b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp b/LibCarla/source/carla/trafficmanager/PipelineStage.cpp index 87aeda511..b50039a1f 100644 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp +++ b/LibCarla/source/carla/trafficmanager/PipelineStage.cpp @@ -1,53 +1,60 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "PipelineStage.h" +#include "carla/trafficmanager/PipelineStage.h" namespace carla { namespace traffic_manager { - PipelineStage::PipelineStage(std::string stage_name) - : stage_name(stage_name), - performance_diagnostics(PerformanceDiagnostics(stage_name)) { - run_stage.store(false); - } +PipelineStage::PipelineStage( + const std::string &stage_name) + : stage_name(stage_name), + performance_diagnostics(PerformanceDiagnostics(stage_name)) { + run_stage.store(false); +} - PipelineStage::~PipelineStage() { - worker_thread->join(); +PipelineStage::~PipelineStage() { + Stop(); +} + +void PipelineStage::Start() { + run_stage.store(true); + if(worker_thread) { + Stop(); + } + worker_thread = std::make_unique(&PipelineStage::Update, this); +} + +void PipelineStage::Stop() { + run_stage.store(false); + if(worker_thread) { + if(worker_thread->joinable()){ + worker_thread->join(); + } worker_thread.release(); } +} - void PipelineStage::Start() { - run_stage.store(true); - worker_thread = std::make_unique(&PipelineStage::Update, this); - } - - void PipelineStage::Stop() { - run_stage.store(false); - } - - void PipelineStage::Update() { - while (run_stage.load()){ - // Receive data. - DataReceiver(); - - // Receive data. - if(run_stage.load()){ - performance_diagnostics.RegisterUpdate(true); - Action(); - performance_diagnostics.RegisterUpdate(false); - } - - // Receive data. - if(run_stage.load()) { - DataSender(); - } +void PipelineStage::Update() { + while (run_stage.load()){ + // Receive data. + DataReceiver(); + if(run_stage.load()){ + performance_diagnostics.RegisterUpdate(true); + Action(); + performance_diagnostics.RegisterUpdate(false); } + + if(run_stage.load()) { + DataSender(); + } + } +} } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.h b/LibCarla/source/carla/trafficmanager/PipelineStage.h index d4f6ac62e..80b2f69e2 100644 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.h +++ b/LibCarla/source/carla/trafficmanager/PipelineStage.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -34,8 +34,9 @@ namespace traffic_manager { class PipelineStage { private: - std::unique_ptr worker_thread; + + protected: /// Flag to start/stop stage. std::atomic run_stage; /// Stage name string. @@ -62,7 +63,7 @@ namespace traffic_manager { public: - PipelineStage(std::string stage_name); + PipelineStage(const std::string &stage_name); virtual ~PipelineStage(); diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp index 746f449d1..9162b38fc 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp @@ -1,10 +1,10 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "SimpleWaypoint.h" +#include "carla/trafficmanager/SimpleWaypoint.h" namespace carla { namespace traffic_manager { diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h index a04cfa557..da1e22911 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index 0dcabfba0..dfc6c9115 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -6,7 +6,7 @@ #include -#include "TrafficLightStage.h" +#include "carla/trafficmanager/TrafficLightStage.h" namespace carla { namespace traffic_manager { @@ -27,9 +27,12 @@ namespace traffic_manager { // Initializing output frame selector. frame_selector = true; - // Initializing number of vehicles to zero in the beginning. number_of_vehicles = 0u; + /// @todo: replace with RandomEngine + // Initializing srand. + srand(static_cast(time(NULL))); + } TrafficLightStage::~TrafficLightStage() {} @@ -38,6 +41,7 @@ namespace traffic_manager { // Selecting the output frame based on the selection key. const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; + // Looping over registered actors. for (uint64_t i = 0u; i < number_of_vehicles && localization_frame != nullptr; ++i) { @@ -57,24 +61,22 @@ namespace traffic_manager { const auto ego_vehicle = boost::static_pointer_cast(ego_actor); TLS traffic_light_state = ego_vehicle->GetTrafficLightState(); - // Generate number between 0 and 100 - const int r = rand() % 101; - - // Set to green if random number is lower than percentage, default is 0 - if (parameters.GetPercentageRunningLight(boost::shared_ptr(ego_actor)) > r) - traffic_light_state = TLS::Green; // We determine to stop if the current position of the vehicle is not a // junction and there is a red or yellow light. if (ego_vehicle->IsAtTrafficLight() && - traffic_light_state != TLS::Green) { + traffic_light_state != TLS::Green && + parameters.GetPercentageRunningLight(boost::shared_ptr(ego_actor)) <= (rand() % 101)) { traffic_light_hazard = true; } + // Handle entry negotiation at non-signalised junction. else if (look_ahead_point->CheckJunction() && !ego_vehicle->IsAtTrafficLight() && - traffic_light_state != TLS::Green) { + traffic_light_state != TLS::Green && + parameters.GetPercentageRunningSign(boost::shared_ptr(ego_actor)) <= (rand() % 101)) { + std::lock_guard lock(no_signal_negotiation_mutex); if (vehicle_last_junction.find(ego_actor_id) == vehicle_last_junction.end()) { @@ -114,11 +116,9 @@ namespace traffic_manager { last_ticket += chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL); } } else { - junction_last_ticket.insert({junction_id, current_time + chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL)}); } - if (vehicle_last_ticket.find(ego_actor_id) != vehicle_last_ticket.end()) { vehicle_last_ticket.at(ego_actor_id) = junction_last_ticket.at(junction_id); } else { @@ -138,17 +138,17 @@ namespace traffic_manager { TrafficLightToPlannerData &message = current_planner_frame->at(i); message.traffic_light_hazard = traffic_light_hazard; } - } void TrafficLightStage::DataReceiver() { + localization_frame = localization_messenger->Peek(); // Allocating new containers for the changed number of registered vehicles. - if (localization_frame != nullptr && - number_of_vehicles != (*localization_frame.get()).size()) { + if (localization_frame != nullptr && number_of_vehicles != (*localization_frame.get()).size()) { number_of_vehicles = static_cast((*localization_frame.get()).size()); + // Allocating output frames. planner_frame_a = std::make_shared(number_of_vehicles); planner_frame_b = std::make_shared(number_of_vehicles); @@ -164,30 +164,24 @@ namespace traffic_manager { } void TrafficLightStage::DrawLight(TLS traffic_light_state, const Actor &ego_actor) const { - std::string str; + + const cg::Location ego_location = ego_actor->GetLocation(); if (traffic_light_state == TLS::Green) { - str="Green"; debug_helper.DrawString( - cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f), - str, + cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f), + "Green", false, {0u, 255u, 0u}, 0.1f, true); - } - - else if (traffic_light_state == TLS::Yellow) { - str="Yellow"; + } else if (traffic_light_state == TLS::Yellow) { debug_helper.DrawString( - cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f), - str, + cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f), + "Yellow", false, {255u, 255u, 0u}, 0.1f, true); - } - - else { - str="Red"; + } else { debug_helper.DrawString( - cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f), - str, + cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f), + "Red", false, {255u, 0u, 0u}, 0.1f, true); } diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h index 04ca10396..d2668df09 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp index 7bfd15442..6c2637132 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp @@ -1,247 +1,213 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "TrafficManager.h" +#include "carla/client/Client.h" +#include "carla/client/World.h" +#include "carla/trafficmanager/TrafficManager.h" +#include "carla/trafficmanager/TrafficManagerBase.h" +#include "carla/Exception.h" -#include "carla/client/TrafficLight.h" +#define DEBUG_PRINT_TM 0 namespace carla { namespace traffic_manager { - TrafficManager::TrafficManager( - std::vector longitudinal_PID_parameters, - std::vector longitudinal_highway_PID_parameters, - std::vector lateral_PID_parameters, - std::vector lateral_highway_PID_parameters, - float perc_difference_from_limit, - cc::Client &client_connection) - : longitudinal_PID_parameters(longitudinal_PID_parameters), - longitudinal_highway_PID_parameters(longitudinal_highway_PID_parameters), - lateral_PID_parameters(lateral_PID_parameters), - lateral_highway_PID_parameters(lateral_highway_PID_parameters), - client_connection(client_connection), - world(client_connection.GetWorld()), - debug_helper(client_connection.GetWorld().MakeDebugHelper()) { +std::map> TrafficManager::_tm_map; +std::mutex TrafficManager::_mutex; - const WorldMap world_map = world.GetMap(); - local_map = std::make_shared(world_map); - local_map->SetUp(); +TrafficManager::TrafficManager( + carla::client::detail::EpisodeProxy episode_proxy, + uint16_t port) + : _port(port) { - parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit); - - localization_collision_messenger = std::make_shared(); - localization_traffic_light_messenger = std::make_shared(); - collision_planner_messenger = std::make_shared(); - localization_planner_messenger = std::make_shared(); - traffic_light_planner_messenger = std::make_shared(); - planner_control_messenger = std::make_shared(); - - localization_stage = std::make_unique( - "Localization stage", - localization_planner_messenger, localization_collision_messenger, - localization_traffic_light_messenger, - registered_actors, *local_map.get(), - parameters, debug_helper, - world); - - collision_stage = std::make_unique( - "Collision stage", - localization_collision_messenger, collision_planner_messenger, - parameters, debug_helper); - - traffic_light_stage = std::make_unique( - "Traffic light stage", - localization_traffic_light_messenger, traffic_light_planner_messenger, - parameters, debug_helper); - - planner_stage = std::make_unique( - "Motion planner stage", - localization_planner_messenger, - collision_planner_messenger, - traffic_light_planner_messenger, - planner_control_messenger, - parameters, - longitudinal_PID_parameters, - longitudinal_highway_PID_parameters, - lateral_PID_parameters, - lateral_highway_PID_parameters, - debug_helper); - - control_stage = std::make_unique( - "Batch control stage", - planner_control_messenger, client_connection); - - Start(); - } - - TrafficManager::~TrafficManager() { - - Stop(); - } - - std::unique_ptr TrafficManager::singleton_pointer = nullptr; - - TrafficManager& TrafficManager::GetInstance(cc::Client &client_connection) { - - if (singleton_pointer == nullptr) { - - const std::vector longitudinal_param = {2.0f, 0.05f, 0.07f}; - const std::vector longitudinal_highway_param = {4.0f, 0.02f, 0.03f}; - const std::vector lateral_param = {10.0f, 0.02f, 1.0f}; - const std::vector lateral_highway_param = {9.0f, 0.02f, 1.0f}; - const float perc_difference_from_limit = 30.0f; - - TrafficManager* tm_ptr = new TrafficManager( - longitudinal_param, longitudinal_highway_param, lateral_param, lateral_highway_param, - perc_difference_from_limit, client_connection - ); - - singleton_pointer = std::unique_ptr(tm_ptr); + if(!GetTM(_port)){ + // Check if a TM server already exists and connect to it + if(!CreateTrafficManagerClient(episode_proxy, port)) { + // As TM server not running, create one + CreateTrafficManagerServer(episode_proxy, port); } - - return *singleton_pointer.get(); } +} - std::unique_ptr TrafficManager::singleton_local_client = nullptr; - - cc::Client& TrafficManager::GetUniqueLocalClient() { - - if (singleton_local_client == nullptr) { - cc::Client* client = new cc::Client("localhost", 2000); - singleton_local_client = std::unique_ptr(client); - } - - return *singleton_local_client.get(); +void TrafficManager::Release() { + std::lock_guard lock(_mutex); + for(auto& tm : _tm_map) { + tm.second->Release(); + TrafficManagerBase *base_ptr = tm.second.release(); + delete base_ptr; } + _tm_map.clear(); +} - void TrafficManager::RegisterVehicles(const std::vector &actor_list) { - registered_actors.Insert(actor_list); +void TrafficManager::Reset() { + std::lock_guard lock(_mutex); + for(auto& tm : _tm_map) { + tm.second->Reset(); } +} - void TrafficManager::UnregisterVehicles(const std::vector &actor_list) { - registered_actors.Remove(actor_list); +void TrafficManager::Tick() { + std::lock_guard lock(_mutex); + for(auto& tm : _tm_map) { + tm.second->SynchronousTick(); } +} - void TrafficManager::Start() { +void TrafficManager::CreateTrafficManagerServer( + carla::client::detail::EpisodeProxy episode_proxy, + uint16_t port) { - localization_collision_messenger->Start(); - localization_traffic_light_messenger->Start(); - localization_planner_messenger->Start(); - collision_planner_messenger->Start(); - traffic_light_planner_messenger->Start(); - planner_control_messenger->Start(); - - localization_stage->Start(); - collision_stage->Start(); - traffic_light_stage->Start(); - planner_stage->Start(); - control_stage->Start(); - } - - void TrafficManager::Stop() { - - localization_collision_messenger->Stop(); - localization_traffic_light_messenger->Stop(); - localization_planner_messenger->Stop(); - collision_planner_messenger->Stop(); - traffic_light_planner_messenger->Stop(); - planner_control_messenger->Stop(); - - localization_stage->Stop(); - collision_stage->Stop(); - traffic_light_stage->Stop(); - planner_stage->Stop(); - control_stage->Stop(); - - } - - void TrafficManager::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { - parameters.SetPercentageSpeedDifference(actor, percentage); - } - - void TrafficManager::SetGlobalPercentageSpeedDifference(const float percentage) { - parameters.SetGlobalPercentageSpeedDifference(percentage); - } - - void TrafficManager::SetCollisionDetection( - const ActorPtr &reference_actor, - const ActorPtr &other_actor, - const bool detect_collision) { - - parameters.SetCollisionDetection(reference_actor, other_actor, detect_collision); - } - - void TrafficManager::SetForceLaneChange(const ActorPtr &actor, const bool direction) { - - parameters.SetForceLaneChange(actor, direction); - } - - void TrafficManager::SetAutoLaneChange(const ActorPtr &actor, const bool enable) { - - parameters.SetAutoLaneChange(actor, enable); - } - - void TrafficManager::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) { - - parameters.SetDistanceToLeadingVehicle(actor, distance); - } - - void TrafficManager::SetPercentageIgnoreActors(const ActorPtr &actor, const float perc) { - - parameters.SetPercentageIgnoreActors(actor, perc); - } - - void TrafficManager::SetPercentageRunningLight(const ActorPtr &actor, const float perc) { - - parameters.SetPercentageRunningLight(actor, perc); - } - - - bool TrafficManager::CheckAllFrozen(TLGroup tl_to_freeze) { - for (auto& elem : tl_to_freeze) { - if (!elem->IsFrozen() || elem->GetState() != TLS::Red) { - return false; - } - } - return true; - } - - void TrafficManager::ResetAllTrafficLights() { - const auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*"); - - std::vector list_of_all_groups; - TLGroup tl_to_freeze; - std::vector list_of_ids; - for (auto tl : *world_traffic_lights.get()) { - if (!(std::find(list_of_ids.begin(), list_of_ids.end(), tl->GetId()) != list_of_ids.end())) { - const TLGroup tl_group = boost::static_pointer_cast(tl)->GetGroupTrafficLights(); - list_of_all_groups.push_back(tl_group); - for (uint64_t i=0u; iGetId()); - if(i!=0u) { - tl_to_freeze.push_back(tl_group.at(i)); + // Get local IP details. + auto GetLocalIP = [=](const uint16_t sport)-> std::pair { + std::pair localIP; + int sock = socket(AF_INET, SOCK_DGRAM, 0); + if(sock == INVALID_INDEX) { + #if DEBUG_PRINT_TM + std::cout << "Error number 1: " << errno << std::endl; + std::cout << "Error message: " << strerror(errno) << std::endl; + #endif + } else { + int err; + sockaddr_in loopback; + std::memset(&loopback, 0, sizeof(loopback)); + loopback.sin_family = AF_INET; + loopback.sin_addr.s_addr = INADDR_LOOPBACK; + loopback.sin_port = htons(9); + err = connect(sock, reinterpret_cast(&loopback), sizeof(loopback)); + if(err == INVALID_INDEX) { + #if DEBUG_PRINT_TM + std::cout << "Error number 2: " << errno << std::endl; + std::cout << "Error message: " << strerror(errno) << std::endl; + #endif + } else { + socklen_t addrlen = sizeof(loopback); + err = getsockname(sock, reinterpret_cast (&loopback), &addrlen); + if(err == INVALID_INDEX) { + #if DEBUG_PRINT_TM + std::cout << "Error number 3: " << errno << std::endl; + std::cout << "Error message: " << strerror(errno) << std::endl; + #endif + } else { + char buffer[IP_DATA_BUFFER_SIZE]; + const char* p = inet_ntop(AF_INET, &loopback.sin_addr, buffer, IP_DATA_BUFFER_SIZE); + if(p != NULL) { + localIP = std::pair(std::string(buffer), sport); + } else { + #if DEBUG_PRINT_TM + std::cout << "Error number 4: " << errno << std::endl; + std::cout << "Error message: " << strerror(errno) << std::endl; + #endif } } } + #ifdef _WIN32 + closesocket(sock); + #else + close(sock); + #endif } + return localIP; + }; - for (TLGroup& tl_group : list_of_all_groups) { - tl_group.front()->SetState(TLS::Green); - std::for_each( - tl_group.begin()+1, tl_group.end(), - [] (auto& tl) {tl->SetState(TLS::Red);}); - } + /// Define local constants + const std::vector longitudinal_param = {2.0f, 0.05f, 0.07f}; + const std::vector longitudinal_highway_param = {4.0f, 0.02f, 0.03f}; + const std::vector lateral_param = {9.0f, 0.02f, 1.0f}; + const std::vector lateral_highway_param = {7.0f, 0.02f, 1.0f}; + const float perc_difference_from_limit = 30.0f; - while (!CheckAllFrozen(tl_to_freeze)) { - for (auto& tln : tl_to_freeze) { - tln->SetState(TLS::Red); - tln->Freeze(true); + std::pair serverTM; + + /// Create local instance of TM + TrafficManagerLocal* tm_ptr = new TrafficManagerLocal( + longitudinal_param, + longitudinal_highway_param, + lateral_param, + lateral_highway_param, + perc_difference_from_limit, + episode_proxy, + port); + + /// Get TM server info (Local IP & PORT) + serverTM = GetLocalIP(port); + + /// Set this client as the TM to server + episode_proxy.Lock()->AddTrafficManagerRunning(serverTM); + + #if DEBUG_PRINT_TM + /// Print status + std::cout << "NEW@: Registered TM at " + << serverTM.first << ":" + << serverTM.second << " ..... SUCCESS." + << std::endl; + #endif + + /// Set the pointer of the instance + _tm_map.insert(std::make_pair(port, std::unique_ptr(tm_ptr))); + +} + +bool TrafficManager::CreateTrafficManagerClient( + carla::client::detail::EpisodeProxy episode_proxy, + uint16_t port) { + + bool result = false; + + if(episode_proxy.Lock()->IsTrafficManagerRunning(port)) { + + /// Get TM server info (Remote IP & PORT) + std::pair serverTM = + episode_proxy.Lock()->GetTrafficManagerRunning(port); + + /// Set remote TM server IP and port + TrafficManagerRemote* tm_ptr = new(std::nothrow) + TrafficManagerRemote(serverTM, episode_proxy); + + /// Try to connect to remote TM server + try { + + /// Check memory allocated or not + if(tm_ptr != nullptr) { + + #if DEBUG_PRINT_TM + // Test print + std::cout << "OLD@: Registered TM at " + << serverTM.first << ":" + << serverTM.second << " ..... TRY " + << std::endl; + #endif + /// Try to reset all traffic lights + tm_ptr->HealthCheckRemoteTM(); + + /// Set the pointer of the instance + _tm_map.insert(std::make_pair(port, std::unique_ptr(tm_ptr))); + + result = true; } } + + /// If Connection error occurred + catch (...) { + + /// Clear previously allocated memory + delete tm_ptr; + + #if DEBUG_PRINT_TM + /// Test print + std::cout << "OLD@: Registered TM at " + << serverTM.first << ":" + << serverTM.second << " ..... FAILED " + << std::endl; + #endif + } + } + return result; +} + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.h b/LibCarla/source/carla/trafficmanager/TrafficManager.h index 6ef3d4131..dc3e7dea0 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -16,9 +17,12 @@ #include "carla/client/BlueprintLibrary.h" #include "carla/client/Map.h" #include "carla/client/World.h" +#include "carla/client/TimeoutException.h" + #include "carla/geom/Transform.h" #include "carla/Logging.h" #include "carla/Memory.h" +#include "carla/Sockets.h" #include "carla/trafficmanager/AtomicActorSet.h" #include "carla/trafficmanager/AtomicMap.h" @@ -30,127 +34,203 @@ #include "carla/trafficmanager/Parameters.h" #include "carla/trafficmanager/TrafficLightStage.h" +#include "carla/trafficmanager/TrafficManagerBase.h" +#include "carla/trafficmanager/TrafficManagerLocal.h" +#include "carla/trafficmanager/TrafficManagerRemote.h" + +#define INVALID_INDEX -1 +#define IP_DATA_BUFFER_SIZE 80 + namespace carla { namespace traffic_manager { - namespace cc = carla::client; +using ActorPtr = carla::SharedPtr; - using ActorPtr = carla::SharedPtr; - using TLS = carla::rpc::TrafficLightState; - using TLGroup = std::vector>; +/// This class integrates all the various stages of +/// the traffic manager appropriately using messengers. +class TrafficManager { - /// The function of this class is to integrate all the various stages of - /// the traffic manager appropriately using messengers. - class TrafficManager { +public: + /// Public constructor for singleton life cycle management. + explicit TrafficManager( + carla::client::detail::EpisodeProxy episode_proxy, + uint16_t port = TM_DEFAULT_PORT); - private: + TrafficManager(const TrafficManager& other) { + _port = other._port; + } - /// PID controller parameters. - std::vector longitudinal_PID_parameters; - std::vector longitudinal_highway_PID_parameters; - std::vector lateral_PID_parameters; - std::vector lateral_highway_PID_parameters; - /// Set of all actors registered with traffic manager. - AtomicActorSet registered_actors; - /// Pointer to local map cache. - std::shared_ptr local_map; - /// Carla's client connection object. - cc::Client client_connection; - /// Carla's world object. - cc::World world; - /// Carla's debug helper object. - cc::DebugHelper debug_helper; - /// Pointers to messenger objects connecting stage pairs. - std::shared_ptr collision_planner_messenger; - std::shared_ptr localization_collision_messenger; - std::shared_ptr localization_traffic_light_messenger; - std::shared_ptr localization_planner_messenger; - std::shared_ptr planner_control_messenger; - std::shared_ptr traffic_light_planner_messenger; - /// Pointers to the stage objects of traffic manager. - std::unique_ptr collision_stage; - std::unique_ptr control_stage; - std::unique_ptr localization_stage; - std::unique_ptr planner_stage; - std::unique_ptr traffic_light_stage; - /// Static pointer to singleton object. - static std::unique_ptr singleton_pointer; - /// Static pointer to singleton client connected to localhost, 2000. - static std::unique_ptr singleton_local_client; - /// Parameterization object. - Parameters parameters; + TrafficManager(TrafficManager &&) = default; - /// Private constructor for singleton lifecycle management. - TrafficManager( - std::vector longitudinal_PID_parameters, - std::vector longitudinal_highway_PID_parameters, - std::vector lateral_PID_parameters, - std::vector lateral_highway_PID_parameters, - float perc_decrease_from_limit, - cc::Client &client_connection); + TrafficManager &operator=(const TrafficManager &) = default; + TrafficManager &operator=(TrafficManager &&) = default; - /// To start the TrafficManager. - void Start(); + static void Release(); - /// To stop the TrafficManager. - void Stop(); + static void Reset(); - public: + static void Tick(); - /// Static method for singleton lifecycle management. - static TrafficManager& GetInstance(cc::Client &client_connection); + /// This method registers a vehicle with the traffic manager. + void RegisterVehicles(const std::vector &actor_list) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->RegisterVehicles(actor_list); + } + } - /// Static method to get unique client connected to (localhost, 2000). - static cc::Client& GetUniqueLocalClient(); + /// This method unregisters a vehicle from traffic manager. + void UnregisterVehicles(const std::vector &actor_list) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->UnregisterVehicles(actor_list); + } + } - /// This method registers a vehicle with the traffic manager. - void RegisterVehicles(const std::vector &actor_list); + /// Set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetPercentageSpeedDifference(actor, percentage); + } + } - /// This method unregisters a vehicle from traffic manager. - void UnregisterVehicles(const std::vector &actor_list); + /// Set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetGlobalPercentageSpeedDifference(float const percentage){ + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetGlobalPercentageSpeedDifference(percentage); + } + } - /// Set target velocity specific to a vehicle. - void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); + /// Method to set collision detection rules between vehicles. + void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetCollisionDetection(reference_actor, other_actor, detect_collision); + } + } - /// Set global target velocity. - void SetGlobalPercentageSpeedDifference(float const percentage); + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + void SetForceLaneChange(const ActorPtr &actor, const bool direction) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetForceLaneChange(actor, direction); + } + } - /// Set collision detection rules between vehicles. - void SetCollisionDetection( - const ActorPtr &reference_actor, - const ActorPtr &other_actor, - const bool detect_collision); + /// Enable/disable automatic lane change on a vehicle. + void SetAutoLaneChange(const ActorPtr &actor, const bool enable) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetAutoLaneChange(actor, enable); + } + } - /// Method to force lane change on a vehicle. - /// Direction flag can be set to true for left and false for right. - void SetForceLaneChange(const ActorPtr &actor, const bool direction); + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetDistanceToLeadingVehicle(actor, distance); + } + } - /// Enable / disable automatic lane change on a vehicle. - void SetAutoLaneChange(const ActorPtr &actor, const bool enable); + /// Method to specify the % chance of ignoring collisions with any walker. + void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetPercentageIgnoreWalkers(actor, perc); + } + } - /// Method to specify how much distance a vehicle should maintain to - /// the leading vehicle. - void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); + /// Method to specify the % chance of ignoring collisions with any vehicle. + void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetPercentageIgnoreVehicles(actor, perc); + } + } - /// Method to specify the % chance of ignoring collisions with other actors - void SetPercentageIgnoreActors(const ActorPtr &actor, const float perc); + /// Method to specify the % chance of running a sign. + void SetPercentageRunningSign(const ActorPtr &actor, const float perc) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetPercentageRunningSign(actor, perc); + } + } - /// Method to specify the % chance of running a red light - void SetPercentageRunningLight(const ActorPtr &actor, const float perc); + /// Method to specify the % chance of running a light. + void SetPercentageRunningLight(const ActorPtr &actor, const float perc){ + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetPercentageRunningLight(actor, perc); + } + } - /// Method to check if traffic lights are frozen. - bool CheckAllFrozen(TLGroup tl_to_freeze); + /// Method to reset all traffic lights. + void ResetAllTrafficLights() { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->ResetAllTrafficLights(); + } + } - /// Method to reset all traffic lights. - void ResetAllTrafficLights(); + /// Method to switch traffic manager into synchronous execution. + void SetSynchronousMode(bool mode) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetSynchronousMode(mode); + } + } - /// Return the world object - const cc::World &GetWorld() { return world; }; + /// Method to set tick timeout for synchronous execution. + void SetSynchronousModeTimeOutInMiliSecond(double time) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetSynchronousModeTimeOutInMiliSecond(time); + } + } - /// Destructor. - ~TrafficManager(); + /// Method to provide synchronous tick. + bool SynchronousTick() { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + return tm_ptr->SynchronousTick(); + } + return false; + } - }; +private: + + void CreateTrafficManagerServer( + carla::client::detail::EpisodeProxy episode_proxy, + uint16_t port); + + + bool CreateTrafficManagerClient( + carla::client::detail::EpisodeProxy episode_proxy, + uint16_t port); + + TrafficManagerBase* GetTM(uint16_t port) const { + std::lock_guard lock(_mutex); + auto it = _tm_map.find(port); + if (it != _tm_map.end()) { + _mutex.unlock(); + return it->second.get(); + } + return nullptr; + } + + static std::map> _tm_map; + static std::mutex _mutex; + + uint16_t _port = TM_DEFAULT_PORT; + +}; } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h b/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h new file mode 100644 index 000000000..85b34f9fe --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h @@ -0,0 +1,105 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include +#include "carla/client/Actor.h" +#include "carla/client/detail/Simulator.h" +#include "carla/client/detail/EpisodeProxy.h" + +#define MIN_TRY_COUNT 20 +#define TM_DEFAULT_PORT 8000 + +namespace carla { +namespace traffic_manager { + +using ActorPtr = carla::SharedPtr; + +/// The function of this class is to integrate all the various stages of +/// the traffic manager appropriately using messengers. +class TrafficManagerBase { + +public: + /// To start the traffic manager. + virtual void Start() = 0; + + /// To stop the traffic manager. + virtual void Stop() = 0; + + /// To release the traffic manager. + virtual void Release() = 0; + + /// To reset the traffic manager. + virtual void Reset() = 0; + + /// Protected constructor for singleton lifecycle management. + TrafficManagerBase() {}; + + /// Destructor. + virtual ~TrafficManagerBase() {}; + + /// This method registers a vehicle with the traffic manager. + virtual void RegisterVehicles(const std::vector &actor_list) = 0; + + /// This method unregisters a vehicle from traffic manager. + virtual void UnregisterVehicles(const std::vector &actor_list) = 0; + + /// Set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + virtual void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) = 0; + + /// Set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + virtual void SetGlobalPercentageSpeedDifference(float const percentage) = 0; + + /// Method to set collision detection rules between vehicles. + virtual void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) = 0; + + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + virtual void SetForceLaneChange(const ActorPtr &actor, const bool direction) = 0; + + /// Enable/disable automatic lane change on a vehicle. + virtual void SetAutoLaneChange(const ActorPtr &actor, const bool enable) = 0; + + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + virtual void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) = 0; + + /// Method to specify the % chance of ignoring collisions with any walker. + virtual void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc) = 0; + + /// Method to specify the % chance of ignoring collisions with any vehicle. + virtual void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc) = 0; + + /// Method to specify the % chance of running any traffic light. + virtual void SetPercentageRunningLight(const ActorPtr &actor, const float perc) = 0; + + /// Method to specify the % chance of running any traffic sign. + virtual void SetPercentageRunningSign(const ActorPtr &actor, const float perc) = 0; + + /// Method to switch traffic manager into synchronous execution. + virtual void SetSynchronousMode(bool mode) = 0; + + /// Method to set Tick timeout for synchronous execution. + virtual void SetSynchronousModeTimeOutInMiliSecond(double time) = 0; + + /// Method to provide synchronous tick + virtual bool SynchronousTick() = 0; + + /// Method to reset all traffic lights. + virtual void ResetAllTrafficLights() = 0; + + /// Get carla episode information + virtual carla::client::detail::EpisodeProxy& GetEpisodeProxy() = 0; + +protected: + +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h b/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h new file mode 100644 index 000000000..6bd8d7764 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h @@ -0,0 +1,180 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/client/Actor.h" +#include + +#define TM_TIMEOUT 2000 // In ms +#define TM_DEFAULT_PORT 8000 // TM_SERVER_PORT + +namespace carla { +namespace traffic_manager { + +/// Provides communication with the rpc of TrafficManagerServer. +class TrafficManagerClient { + +public: + + TrafficManagerClient(const TrafficManagerClient &) = default; + TrafficManagerClient(TrafficManagerClient &&) = default; + + TrafficManagerClient &operator=(const TrafficManagerClient &) = default; + TrafficManagerClient &operator=(TrafficManagerClient &&) = default; + + /// Parametric constructor to initialize the parameters. + TrafficManagerClient( + const std::string &_host, + const uint16_t &_port) + : tmhost(_host), + tmport(_port) { + + /// Create client instance. + if(!_client) { + _client = new ::rpc::client(tmhost, tmport); + _client->set_timeout(TM_TIMEOUT); + } + } + + /// Destructor method. + ~TrafficManagerClient() { + if(_client) { + delete _client; + _client = nullptr; + } + }; + + /// Set parameters. + void setServerDetails(const std::string &_host, const uint16_t &_port) { + tmhost = _host; + tmport = _port; + } + + /// Get parameters. + void getServerDetails(std::string &_host, uint16_t &_port) { + _host = tmhost; + _port = tmport; + } + + /// Register vehicles to remote traffic manager server via RPC client. + void RegisterVehicle(const std::vector &actor_list) { + DEBUG_ASSERT(_client != nullptr); + _client->call("register_vehicle", std::move(actor_list)); + } + + /// Unregister vehicles to remote traffic manager server via RPC client. + void UnregisterVehicle(const std::vector &actor_list) { + DEBUG_ASSERT(_client != nullptr); + _client->call("unregister_vehicle", std::move(actor_list)); + } + + /// Method to set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetPercentageSpeedDifference(const carla::rpc::Actor &_actor, const float percentage) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_percentage_speed_difference", std::move(_actor), percentage); + } + + /// Method to set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetGlobalPercentageSpeedDifference(const float percentage) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_global_percentage_speed_difference", percentage); + } + + /// Method to set collision detection rules between vehicles. + void SetCollisionDetection(const carla::rpc::Actor &reference_actor, const carla::rpc::Actor &other_actor, const bool detect_collision) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_collision_detection", reference_actor, other_actor, detect_collision); + } + + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + void SetForceLaneChange(const carla::rpc::Actor &actor, const bool direction) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_force_lane_change", actor, direction); + } + + /// Enable/disable automatic lane change on a vehicle. + void SetAutoLaneChange(const carla::rpc::Actor &actor, const bool enable) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_auto_lane_change", actor, enable); + } + + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + void SetDistanceToLeadingVehicle(const carla::rpc::Actor &actor, const float distance) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_distance_to_leading_vehicle", actor, distance); + } + + /// Method to specify the % chance of ignoring collisions with any walker. + void SetPercentageIgnoreWalkers(const carla::rpc::Actor &actor, const float percentage) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_percentage_ignore_walkers", actor, percentage); + } + + /// Method to specify the % chance of ignoring collisions with any vehicle. + void SetPercentageIgnoreVehicles(const carla::rpc::Actor &actor, const float percentage) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_percentage_ignore_vehicles", actor, percentage); + } + + /// Method to specify the % chance of running a traffic sign. + void SetPercentageRunningLight(const carla::rpc::Actor &actor, const float percentage) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_percentage_running_light", actor, percentage); + } + + /// Method to specify the % chance of running any traffic sign. + void SetPercentageRunningSign(const carla::rpc::Actor &actor, const float percentage) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_percentage_running_sign", actor, percentage); + } + + /// Method to switch traffic manager into synchronous execution. + void SetSynchronousMode(const bool mode) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_synchronous_mode", mode); + } + + /// Method to set tick timeout for synchronous execution. + void SetSynchronousModeTimeOutInMiliSecond(const double time) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_synchronous_mode_timeout_in_milisecond", time); + } + + /// Method to provide synchronous tick. + bool SynchronousTick() { + DEBUG_ASSERT(_client != nullptr); + return _client->call("synchronous_tick").as(); + } + + /// Method to reset all traffic light groups to the initial stage. + void ResetAllTrafficLights() { + DEBUG_ASSERT(_client != nullptr); + _client->call("reset_all_traffic_lights"); + } + + /// Check if remote traffic manager is alive + void HealthCheckRemoteTM() { + DEBUG_ASSERT(_client != nullptr); + _client->call("health_check_remote_TM"); + } + +private: + + /// RPC client. + ::rpc::client *_client = nullptr; + + /// Server port and host. + std::string tmhost; + uint16_t tmport; +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp new file mode 100644 index 000000000..4138f9b42 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp @@ -0,0 +1,277 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "carla/trafficmanager/TrafficManagerLocal.h" + +#include "carla/client/TrafficLight.h" +#include "carla/client/ActorList.h" +#include "carla/client/DebugHelper.h" + +namespace carla { +namespace traffic_manager { + +TrafficManagerLocal::TrafficManagerLocal( + std::vector longitudinal_PID_parameters, + std::vector longitudinal_highway_PID_parameters, + std::vector lateral_PID_parameters, + std::vector lateral_highway_PID_parameters, + float perc_difference_from_limit, + carla::client::detail::EpisodeProxy& episodeProxy, + uint16_t& RPCportTM) + : longitudinal_PID_parameters(longitudinal_PID_parameters), + longitudinal_highway_PID_parameters(longitudinal_highway_PID_parameters), + lateral_PID_parameters(lateral_PID_parameters), + lateral_highway_PID_parameters(lateral_highway_PID_parameters), + episodeProxyTM(episodeProxy), + debug_helper(carla::client::DebugHelper{episodeProxyTM}), + server(TrafficManagerServer(RPCportTM, static_cast(this))) { + + parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit); + + Start(); + +} + +TrafficManagerLocal::~TrafficManagerLocal() { + episodeProxyTM.Lock()->DestroyTrafficManager(server.port()); + Release(); +} + +void TrafficManagerLocal::Start() { + + const carla::SharedPtr world_map = episodeProxyTM.Lock()->GetCurrentMap(); + local_map = std::make_shared(world_map); + local_map->SetUp(); + + localization_collision_messenger = std::make_shared(); + localization_traffic_light_messenger = std::make_shared(); + collision_planner_messenger = std::make_shared(); + localization_planner_messenger = std::make_shared(); + traffic_light_planner_messenger = std::make_shared(); + planner_control_messenger = std::make_shared(); + + localization_stage = std::make_unique( + "Localization stage", + localization_planner_messenger, localization_collision_messenger, + localization_traffic_light_messenger, + registered_actors, *local_map.get(), + parameters, debug_helper, + episodeProxyTM); + + collision_stage = std::make_unique( + "Collision stage", + localization_collision_messenger, collision_planner_messenger, + parameters, debug_helper); + + traffic_light_stage = std::make_unique( + "Traffic light stage", + localization_traffic_light_messenger, traffic_light_planner_messenger, + parameters, debug_helper); + + planner_stage = std::make_unique( + "Motion planner stage", + localization_planner_messenger, + collision_planner_messenger, + traffic_light_planner_messenger, + planner_control_messenger, + parameters, + longitudinal_PID_parameters, + longitudinal_highway_PID_parameters, + lateral_PID_parameters, + lateral_highway_PID_parameters, + debug_helper); + + control_stage = std::make_unique( + "Batch control stage", + planner_control_messenger, + episodeProxyTM, + parameters); + + localization_collision_messenger->Start(); + localization_traffic_light_messenger->Start(); + localization_planner_messenger->Start(); + collision_planner_messenger->Start(); + traffic_light_planner_messenger->Start(); + planner_control_messenger->Start(); + + localization_stage->Start(); + collision_stage->Start(); + traffic_light_stage->Start(); + planner_stage->Start(); + control_stage->Start(); + +} + +void TrafficManagerLocal::Stop() { + localization_collision_messenger->Stop(); + localization_traffic_light_messenger->Stop(); + localization_planner_messenger->Stop(); + collision_planner_messenger->Stop(); + traffic_light_planner_messenger->Stop(); + planner_control_messenger->Stop(); + + localization_stage->Stop(); + collision_stage->Stop(); + traffic_light_stage->Stop(); + planner_stage->Stop(); + control_stage->Stop(); +} + +void TrafficManagerLocal::Release() { + Stop(); + localization_collision_messenger.reset(); + localization_traffic_light_messenger.reset(); + localization_planner_messenger.reset(); + collision_planner_messenger.reset(); + traffic_light_planner_messenger.reset(); + planner_control_messenger.reset(); + localization_stage.reset(); + collision_stage.reset(); + traffic_light_stage.reset(); + planner_stage.reset(); + control_stage.reset(); +} + +void TrafficManagerLocal::Reset() { + + Release(); + + carla::client::detail::EpisodeProxy episode_proxy = episodeProxyTM.Lock()->GetCurrentEpisode(); + episodeProxyTM = episode_proxy; + + Start(); +} + +void TrafficManagerLocal::RegisterVehicles(const std::vector& actor_list) { + registered_actors.Insert(actor_list); +} + +void TrafficManagerLocal::UnregisterVehicles(const std::vector& actor_list) { + registered_actors.Remove(actor_list); +} + +void TrafficManagerLocal::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { + parameters.SetPercentageSpeedDifference(actor, percentage); +} + +void TrafficManagerLocal::SetGlobalPercentageSpeedDifference(const float percentage) { + parameters.SetGlobalPercentageSpeedDifference(percentage); +} + +void TrafficManagerLocal::SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) { + parameters.SetCollisionDetection(reference_actor, other_actor, detect_collision); +} + +void TrafficManagerLocal::SetForceLaneChange(const ActorPtr &actor, const bool direction) { + parameters.SetForceLaneChange(actor, direction); +} + +void TrafficManagerLocal::SetAutoLaneChange(const ActorPtr &actor, const bool enable) { + parameters.SetAutoLaneChange(actor, enable); +} + +void TrafficManagerLocal::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) { + parameters.SetDistanceToLeadingVehicle(actor, distance); +} + +void TrafficManagerLocal::SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc) { + parameters.SetPercentageIgnoreWalkers(actor, perc); +} + +void TrafficManagerLocal::SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc) { + parameters.SetPercentageIgnoreVehicles(actor, perc); +} + +void TrafficManagerLocal::SetPercentageRunningLight(const ActorPtr &actor, const float perc) { + parameters.SetPercentageRunningLight(actor, perc); +} + +void TrafficManagerLocal::SetPercentageRunningSign(const ActorPtr &actor, const float perc) { + parameters.SetPercentageRunningSign(actor, perc); +} + +bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) { + for (auto& elem : tl_to_freeze) { + if (!elem->IsFrozen() || elem->GetState() != TLS::Red) { + return false; + } + } + return true; +} + +void TrafficManagerLocal::ResetAllTrafficLights() { + + auto Filter = [&](auto &actors, auto &wildcard_pattern) { + std::vector filtered; + for (auto &&actor : actors) { + if (carla::StringUtil::Match(carla::client::detail::ActorVariant(actor).GetTypeId(), wildcard_pattern)) { + filtered.push_back(actor); + } + } + return filtered; + }; + + // Get all actors of the world. + auto world_actorsList = episodeProxyTM.Lock()->GetAllTheActorsInTheEpisode(); + + // Filter based on wildcard pattern. + const auto world_traffic_lights = Filter(world_actorsList, "*traffic_light*"); + + std::vector list_of_all_groups; + TLGroup tl_to_freeze; + std::vector list_of_ids; + for (auto tl : world_traffic_lights) { + if (!(std::find(list_of_ids.begin(), list_of_ids.end(), tl.GetId()) != list_of_ids.end())) { + const TLGroup tl_group = boost::static_pointer_cast(tl.Get(episodeProxyTM))->GetGroupTrafficLights(); + list_of_all_groups.push_back(tl_group); + for (uint64_t i=0u; iGetId()); + if(i!=0u) { + tl_to_freeze.push_back(tl_group.at(i)); + } + } + } + } + + for (TLGroup& tl_group : list_of_all_groups) { + tl_group.front()->SetState(TLS::Green); + std::for_each( + tl_group.begin()+1, tl_group.end(), + [] (auto& tl) {tl->SetState(TLS::Red);}); + } + + while (!CheckAllFrozen(tl_to_freeze)) { + for (auto& tln : tl_to_freeze) { + tln->SetState(TLS::Red); + tln->Freeze(true); + } + } + +} + +void TrafficManagerLocal::SetSynchronousMode(bool mode) { + parameters.SetSynchronousMode(mode); +} + +void TrafficManagerLocal::SetSynchronousModeTimeOutInMiliSecond(double time) { + parameters.SetSynchronousModeTimeOutInMiliSecond(time); +} + +bool TrafficManagerLocal::SynchronousTick() { + return control_stage->RunStep(); +} + +carla::client::detail::EpisodeProxy& TrafficManagerLocal::GetEpisodeProxy() { + return episodeProxyTM; +} + +std::vector TrafficManagerLocal::GetRegisteredVehiclesIDs() { + + return registered_actors.GetIDList(); +} + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h new file mode 100644 index 000000000..e1afbbf05 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h @@ -0,0 +1,183 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include +#include +#include +#include +#include + +#include "carla/StringUtil.h" +#include "carla/geom/Transform.h" +#include "carla/Logging.h" +#include "carla/Memory.h" + +#include "carla/client/Actor.h" +#include "carla/client/BlueprintLibrary.h" +#include "carla/client/Map.h" +#include "carla/client/World.h" + +#include "carla/client/detail/Simulator.h" +#include "carla/client/detail/EpisodeProxy.h" + +#include "carla/trafficmanager/AtomicActorSet.h" +#include "carla/trafficmanager/AtomicMap.h" +#include "carla/trafficmanager/BatchControlStage.h" +#include "carla/trafficmanager/CollisionStage.h" +#include "carla/trafficmanager/InMemoryMap.h" +#include "carla/trafficmanager/LocalizationStage.h" +#include "carla/trafficmanager/MotionPlannerStage.h" +#include "carla/trafficmanager/Parameters.h" +#include "carla/trafficmanager/TrafficLightStage.h" + +#include "carla/trafficmanager/TrafficManagerBase.h" +#include "carla/trafficmanager/TrafficManagerServer.h" + +namespace carla { +namespace traffic_manager { + + using ActorPtr = carla::SharedPtr; + using TLS = carla::rpc::TrafficLightState; + using TLGroup = std::vector>; + + /// The function of this class is to integrate all the various stages of + /// the traffic manager appropriately using messengers. + class TrafficManagerLocal : public TrafficManagerBase { + + private: + + /// PID controller parameters. + std::vector longitudinal_PID_parameters; + std::vector longitudinal_highway_PID_parameters; + std::vector lateral_PID_parameters; + std::vector lateral_highway_PID_parameters; + + /// Set of all actors registered with traffic manager. + AtomicActorSet registered_actors; + + /// Pointer to local map cache. + std::shared_ptr local_map; + + /// Carla's client connection object. + carla::client::detail::EpisodeProxy episodeProxyTM; + + /// Carla's debug helper object. + carla::client::DebugHelper debug_helper; + + /// Pointers to messenger objects connecting stage pairs. + std::shared_ptr collision_planner_messenger; + std::shared_ptr localization_collision_messenger; + std::shared_ptr localization_traffic_light_messenger; + std::shared_ptr localization_planner_messenger; + std::shared_ptr planner_control_messenger; + std::shared_ptr traffic_light_planner_messenger; + + /// Pointers to the stage objects of traffic manager. + std::unique_ptr collision_stage; + std::unique_ptr control_stage; + std::unique_ptr localization_stage; + std::unique_ptr planner_stage; + std::unique_ptr traffic_light_stage; + + /// Parameterization object. + Parameters parameters; + + /// Traffic manager server instance. + TrafficManagerServer server; + + /// Method to check if all traffic lights are frozen in a group. + bool CheckAllFrozen(TLGroup tl_to_freeze); + + public: + + /// Private constructor for singleton lifecycle management. + TrafficManagerLocal( + std::vector longitudinal_PID_parameters, + std::vector longitudinal_highway_PID_parameters, + std::vector lateral_PID_parameters, + std::vector lateral_highway_PID_parameters, + float perc_decrease_from_limit, + carla::client::detail::EpisodeProxy &episodeProxy, + uint16_t &RPCportTM); + + /// Destructor. + virtual ~TrafficManagerLocal(); + + /// To start the TrafficManager. + void Start(); + + /// To stop the TrafficManager. + void Stop(); + + /// To release the traffic manager. + void Release(); + + /// To reset the traffic manager. + void Reset(); + + /// This method registers a vehicle with the traffic manager. + void RegisterVehicles(const std::vector &actor_list); + + /// This method unregisters a vehicle from traffic manager. + void UnregisterVehicles(const std::vector &actor_list); + + /// Method to set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); + + /// Methos to set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetGlobalPercentageSpeedDifference(float const percentage); + + /// Method to set collision detection rules between vehicles. + void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision); + + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + void SetForceLaneChange(const ActorPtr &actor, const bool direction); + + /// Enable/disable automatic lane change on a vehicle. + void SetAutoLaneChange(const ActorPtr &actor, const bool enable); + + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); + + /// Method to specify the % chance of ignoring collisions with any walker. + void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc); + + /// Method to specify the % chance of ignoring collisions with any vehicle. + void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc); + + /// Method to specify the % chance of running any traffic light. + void SetPercentageRunningLight(const ActorPtr &actor, const float perc); + + /// Method to specify the % chance of running any traffic sign. + void SetPercentageRunningSign(const ActorPtr &actor, const float perc); + + /// Method to switch traffic manager into synchronous execution. + void SetSynchronousMode(bool mode); + + /// Method to set Tick timeout for synchronous execution. + void SetSynchronousModeTimeOutInMiliSecond(double time); + + /// Method to provide synchronous tick. + bool SynchronousTick(); + + /// Method to reset all traffic light groups to the initial stage. + void ResetAllTrafficLights(); + + /// Get CARLA episode information. + carla::client::detail::EpisodeProxy& GetEpisodeProxy(); + + /// Get list of all registered vehicles. + std::vector GetRegisteredVehiclesIDs(); + }; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp new file mode 100644 index 000000000..18e08bc94 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp @@ -0,0 +1,193 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include +#include "carla/client/TrafficLight.h" +#include "carla/client/ActorList.h" +#include "carla/client/DebugHelper.h" + +#include "carla/trafficmanager/TrafficManagerRemote.h" + +namespace carla { +namespace traffic_manager { + +TrafficManagerRemote::TrafficManagerRemote( + const std::pair &_serverTM, + carla::client::detail::EpisodeProxy &episodeProxy) + : client(_serverTM.first, _serverTM.second), + episodeProxyTM(episodeProxy) { + + Start(); + +} + +/// Destructor. +TrafficManagerRemote::~TrafficManagerRemote() { + Release(); +} + +void TrafficManagerRemote::Start() { + _keep_alive = true; + + std::thread _thread = std::thread([this] () { + std::chrono::milliseconds wait_time(TM_TIMEOUT); + try { + do { + std::this_thread::sleep_for(wait_time); + + client.HealthCheckRemoteTM(); + + /// Until connection active + } while (_keep_alive); + } catch (...) { + + std::string rhost(""); + uint16_t rport = 0; + + client.getServerDetails(rhost, rport); + + std::string strtmserver(rhost + ":" + std::to_string(rport)); + + /// Create error msg + std::string errmsg( + "Trying to connect rpc server of traffic manager; " + "but the system failed to connect at " + strtmserver); + + /// TSet the error message + if(_keep_alive) { + this->episodeProxyTM.Lock()->AddPendingException(errmsg); + } + } + _keep_alive = false; + _cv.notify_one(); + }); + + _thread.detach(); +} + +void TrafficManagerRemote::Stop() { + if(_keep_alive) { + _keep_alive = false; + std::unique_lock lock(_mutex); + std::chrono::milliseconds wait_time(TM_TIMEOUT + 1000); + _cv.wait_for(lock, wait_time); + } +} + +void TrafficManagerRemote::Release() { + Stop(); +} + +void TrafficManagerRemote::Reset() { + Stop(); + + carla::client::detail::EpisodeProxy episode_proxy = episodeProxyTM.Lock()->GetCurrentEpisode(); + episodeProxyTM = episode_proxy; + + Start(); +} + +void TrafficManagerRemote::RegisterVehicles(const std::vector &_actor_list) { + std::vector actor_list; + for (auto &&actor : _actor_list) { + actor_list.emplace_back(actor->Serialize()); + } + client.RegisterVehicle(actor_list); +} + +void TrafficManagerRemote::UnregisterVehicles(const std::vector &_actor_list) { + std::vector actor_list; + for (auto &&actor : _actor_list) { + actor_list.emplace_back(actor->Serialize()); + } + client.UnregisterVehicle(actor_list); +} + +void TrafficManagerRemote::SetPercentageSpeedDifference(const ActorPtr &_actor, const float percentage) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetPercentageSpeedDifference(actor, percentage); +} + +void TrafficManagerRemote::SetGlobalPercentageSpeedDifference(const float percentage) { + client.SetGlobalPercentageSpeedDifference(percentage); +} + +void TrafficManagerRemote::SetCollisionDetection(const ActorPtr &_reference_actor, const ActorPtr &_other_actor, const bool detect_collision) { + carla::rpc::Actor reference_actor(_reference_actor->Serialize()); + carla::rpc::Actor other_actor(_other_actor->Serialize()); + + client.SetCollisionDetection(reference_actor, other_actor, detect_collision); +} + +void TrafficManagerRemote::SetForceLaneChange(const ActorPtr &_actor, const bool direction) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetForceLaneChange(actor, direction); +} + +void TrafficManagerRemote::SetAutoLaneChange(const ActorPtr &_actor, const bool enable) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetAutoLaneChange(actor, enable); +} + +void TrafficManagerRemote::SetDistanceToLeadingVehicle(const ActorPtr &_actor, const float distance) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetDistanceToLeadingVehicle(actor, distance); +} + +void TrafficManagerRemote::SetPercentageIgnoreWalkers(const ActorPtr &_actor, const float percentage) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetPercentageIgnoreWalkers(actor, percentage); +} + +void TrafficManagerRemote::SetPercentageIgnoreVehicles(const ActorPtr &_actor, const float percentage) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetPercentageIgnoreVehicles(actor, percentage); +} + +void TrafficManagerRemote::SetPercentageRunningLight(const ActorPtr &_actor, const float percentage) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetPercentageRunningLight(actor, percentage); +} + +void TrafficManagerRemote::SetPercentageRunningSign(const ActorPtr &_actor, const float percentage) { + carla::rpc::Actor actor(_actor->Serialize()); + + client.SetPercentageRunningSign(actor, percentage); +} + +void TrafficManagerRemote::ResetAllTrafficLights() { + client.ResetAllTrafficLights(); +} + +void TrafficManagerRemote::SetSynchronousMode(bool mode) { + client.SetSynchronousMode(mode); +} + +void TrafficManagerRemote::SetSynchronousModeTimeOutInMiliSecond(double time) { + client.SetSynchronousModeTimeOutInMiliSecond(time); +} + +bool TrafficManagerRemote::SynchronousTick() { + return false; +} + +void TrafficManagerRemote::HealthCheckRemoteTM() { + client.HealthCheckRemoteTM(); +} + +carla::client::detail::EpisodeProxy& TrafficManagerRemote::GetEpisodeProxy() { + return episodeProxyTM; +} + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h new file mode 100644 index 000000000..5046c4f59 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h @@ -0,0 +1,126 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include +#include +#include +#include + +#include "carla/client/Actor.h" +#include "carla/client/detail/Simulator.h" +#include "carla/client/detail/EpisodeProxy.h" +#include "carla/trafficmanager/TrafficManagerBase.h" +#include "carla/trafficmanager/TrafficManagerClient.h" + +namespace carla { +namespace traffic_manager { + +using ActorPtr = carla::SharedPtr; +using TLS = carla::rpc::TrafficLightState; +using TLGroup = std::vector>; + +/// The function of this class is to integrate all the various stages of +/// the traffic manager appropriately using messengers. +class TrafficManagerRemote : public TrafficManagerBase { + +public: + + /// To start the TrafficManager. + void Start(); + + /// To stop the TrafficManager. + void Stop(); + + /// To release the traffic manager. + void Release(); + + /// To reset the traffic manager. + void Reset(); + + /// Constructor store remote location information. + TrafficManagerRemote(const std::pair &_serverTM, carla::client::detail::EpisodeProxy &episodeProxy); + + /// Destructor. + virtual ~TrafficManagerRemote(); + + /// This method registers a vehicle with the traffic manager. + void RegisterVehicles(const std::vector &actor_list); + + /// This method unregisters a vehicle from traffic manager. + void UnregisterVehicles(const std::vector &actor_list); + + /// Method to set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); + + /// Method to set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetGlobalPercentageSpeedDifference(float const percentage); + + /// Method to set collision detection rules between vehicles. + void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision); + + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + void SetForceLaneChange(const ActorPtr &actor, const bool direction); + + /// Enable/disable automatic lane change on a vehicle. + void SetAutoLaneChange(const ActorPtr &actor, const bool enable); + + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); + + /// Method to specify the % chance of ignoring collisions with any walker. + void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc); + + /// Method to specify the % chance of ignoring collisions with any vehicle. + void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc); + + /// Method to specify the % chance of running any traffic light + void SetPercentageRunningLight(const ActorPtr &actor, const float perc); + + /// Method to specify the % chance of running any traffic sign + void SetPercentageRunningSign(const ActorPtr &actor, const float perc); + + /// Method to switch traffic manager into synchronous execution. + void SetSynchronousMode(bool mode); + + /// Method to set Tick timeout for synchronous execution. + void SetSynchronousModeTimeOutInMiliSecond(double time); + + /// Method to provide synchronous tick + bool SynchronousTick(); + + /// Method to reset all traffic lights. + void ResetAllTrafficLights(); + + /// Get CARLA episode information. + carla::client::detail::EpisodeProxy& GetEpisodeProxy(); + + /// Method to check server is alive or not. + void HealthCheckRemoteTM(); + +private: + + /// Remote client using the IP and port information it connects to + /// as remote RPC traffic manager server. + TrafficManagerClient client; + + /// CARLA client connection object. + carla::client::detail::EpisodeProxy episodeProxyTM; + + std::condition_variable _cv; + + std::mutex _mutex; + + bool _keep_alive = true; +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h b/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h new file mode 100644 index 000000000..af113e311 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h @@ -0,0 +1,195 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include +#include + +#include "carla/Exception.h" +#include "carla/client/Actor.h" +#include "carla/Version.h" +#include "carla/rpc/Server.h" +#include "carla/trafficmanager/TrafficManagerBase.h" + +namespace carla { +namespace traffic_manager { + +using ActorPtr = carla::SharedPtr; + +class TrafficManagerServer { +public: + + TrafficManagerServer(const TrafficManagerServer &) = default; + TrafficManagerServer(TrafficManagerServer &&) = default; + + TrafficManagerServer &operator=(const TrafficManagerServer &) = default; + TrafficManagerServer &operator=(TrafficManagerServer &&) = default; + + /// Here RPCPort is the traffic manager local instance RPC server port where + /// it can listen to remote traffic managers and apply the changes to + /// local instance through a TrafficManagerBase pointer. + TrafficManagerServer( + uint16_t &RPCPort, + carla::traffic_manager::TrafficManagerBase* tm) + : _RPCPort(RPCPort) { + + uint16_t counter = 0; + while(counter < MIN_TRY_COUNT) { + try { + + /// Create server instance. + server = new ::rpc::server(RPCPort); + + } catch(std::exception& e) { + /// Update port number and try again. + std::this_thread::sleep_for(500ms); + } + + /// If server created. + if(server != nullptr) { + break; + } + counter ++; + } + + /// If server still not created throw a runtime exception. + if(server == nullptr) { + + carla::throw_exception(std::runtime_error( + "trying to create rpc server for traffic manager; " + "but the system failed to create because of bind error.")); + } else { + /// If the server creation was successful we are + /// binding a lambda function to the name "register_vehicle". + server->bind("register_vehicle", [=](std :: vector _actor_list) { + std::vector actor_list; + for (auto &&actor : _actor_list) { + actor_list.emplace_back(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy())); + } + tm->RegisterVehicles(actor_list); + }); + + + /// Binding a lambda function to the name "unregister_vehicle". + server->bind("unregister_vehicle", [=](std :: vector _actor_list) { + std::vector actor_list; + for (auto &&actor : _actor_list) { + actor_list.emplace_back(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy())); + } + tm->UnregisterVehicles(actor_list); + }); + + /// Method to set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + server->bind("set_percentage_speed_difference", [=](carla::rpc::Actor actor, const float percentage) { + tm->SetPercentageSpeedDifference(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage); + }); + + /// Method to set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + server->bind("set_global_percentage_speed_difference", [=](const float percentage) { + tm->SetGlobalPercentageSpeedDifference(percentage); + }); + + /// Method to set collision detection rules between vehicles. + server->bind("set_collision_detection", [=](const carla::rpc::Actor &reference_actor, const carla::rpc::Actor &other_actor, const bool detect_collision) { + const auto reference = carla::client::detail::ActorVariant(reference_actor).Get(tm->GetEpisodeProxy()); + const auto other = carla::client::detail::ActorVariant(other_actor).Get(tm->GetEpisodeProxy()); + tm->SetCollisionDetection(reference, other, detect_collision); + }); + + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + server->bind("set_force_lane_change", [=](carla::rpc::Actor actor, const bool direction) { + tm->SetForceLaneChange(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), direction); + }); + + /// Enable/disable automatic lane change on a vehicle. + server->bind("set_auto_lane_change", [=](carla::rpc::Actor actor, const bool enable) { + tm->SetAutoLaneChange(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), enable); + }); + + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + server->bind("set_distance_to_leading_vehicle", [=](carla::rpc::Actor actor, const float distance) { + tm->SetDistanceToLeadingVehicle(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), distance); + }); + + /// Method to specify the % chance of running any traffic light. + server->bind("set_percentage_running_light", [=](carla::rpc::Actor actor, const float percentage) { + tm->SetPercentageRunningLight(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage); + }); + + /// Method to specify the % chance of running any traffic sign. + server->bind("set_percentage_running_sign", [=](carla::rpc::Actor actor, const float percentage) { + tm->SetPercentageRunningSign(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage); + }); + + /// Method to specify the % chance of ignoring collisions with any walker. + server->bind("set_percentage_ignore_walkers", [=](carla::rpc::Actor actor, const float percentage) { + tm->SetPercentageIgnoreWalkers(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage); + }); + + /// Method to specify the % chance of ignoring collisions with any vehicle. + server->bind("set_percentage_ignore_vehicles", [=](carla::rpc::Actor actor, const float percentage) { + tm->SetPercentageIgnoreVehicles(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage); + }); + + /// Method to set synchronous mode. + server->bind("set_synchronous_mode", [=](const bool mode) { + tm->SetSynchronousMode(mode); + }); + + /// Method to set tick timeout for synchronous execution. + server->bind("set_synchronous_mode_timeout_in_milisecond", [=](const double time) { + tm->SetSynchronousModeTimeOutInMiliSecond(time); + }); + + /// Method to provide synchronous tick. + server->bind("synchronous_tick", [=]() -> bool { + return tm->SynchronousTick(); + }); + + /// Method to reset all traffic lights. + server->bind("reset_all_traffic_lights", [=]() { + tm->ResetAllTrafficLights(); + }); + + /// Method to check server is alive or not. + server->bind("health_check_remote_TM", [=](){}); + + /// Run traffic manager server to respond of any + /// user client in asynchronous mode. + server->async_run(); + } + + } + + ~TrafficManagerServer() { + if(server) { + server->stop(); + delete server; + server = nullptr; + } + } + + uint16_t port() const { + return _RPCPort; + } + +private: + + /// Traffic manager server RPC port + uint16_t _RPCPort; + + /// Server instance + ::rpc::server *server = nullptr; + +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/PythonAPI/carla/setup.py b/PythonAPI/carla/setup.py index e85d5b1cf..71bbc91ec 100755 --- a/PythonAPI/carla/setup.py +++ b/PythonAPI/carla/setup.py @@ -97,7 +97,7 @@ def get_libcarla_extensions(): extra_compile_args = [ '/experimental:external', '/external:I', 'dependencies/include/system', '/DBOOST_ALL_NO_LIB', '/DBOOST_PYTHON_STATIC_LIB', - '/DBOOST_ERROR_CODE_HEADER_ONLY', '/D_WIN32_WINNT=0x0501', + '/DBOOST_ERROR_CODE_HEADER_ONLY', '/D_WIN32_WINNT=0x0600', '/DHAVE_SNPRINTF', '/DLIBCARLA_WITH_PYTHON_SUPPORT', '-DLIBCARLA_IMAGE_WITH_PNG_SUPPORT=true'] else: raise NotImplementedError diff --git a/PythonAPI/carla/source/libcarla/Client.cpp b/PythonAPI/carla/source/libcarla/Client.cpp index 0b8005ff3..d0cd001b8 100644 --- a/PythonAPI/carla/source/libcarla/Client.cpp +++ b/PythonAPI/carla/source/libcarla/Client.cpp @@ -32,8 +32,8 @@ static void ApplyBatchCommands( bool do_tick) { using CommandType = carla::rpc::Command; std::vector cmds{ - boost::python::stl_input_iterator(commands), - boost::python::stl_input_iterator()}; + boost::python::stl_input_iterator(commands), + boost::python::stl_input_iterator()}; self.ApplyBatch(std::move(cmds), do_tick); } @@ -42,71 +42,108 @@ static auto ApplyBatchCommandsSync( const boost::python::object &commands, bool do_tick) { using CommandType = carla::rpc::Command; - std::vector cmds{ - boost::python::stl_input_iterator(commands), - boost::python::stl_input_iterator()}; + std::vector cmds { + boost::python::stl_input_iterator(commands), + boost::python::stl_input_iterator() + }; + boost::python::list result; auto responses = self.ApplyBatchSync(cmds, do_tick); for (auto &response : responses) { result.append(std::move(response)); } + // check for autopilot command - carla::traffic_manager::TrafficManager *tm = nullptr; - std::vector vehicles_to_enable; - std::vector vehicles_to_disable; - for (size_t i=0; i vehicles_to_enable(cmds.size(), nullptr); + std::vector vehicles_to_disable(cmds.size(), nullptr); + carla::client::World world = self.GetWorld(); - bool isAutopilot = false; - bool autopilotValue = false; + std::atomic vehicles_to_enable_index; + std::atomic vehicles_to_disable_index; - // check SpawnActor command - if (cmds[i].command.type() == typeid(carla::rpc::Command::SpawnActor)) { - // check inside 'do_after' - auto &spawn = boost::get(cmds[i].command); - for (auto &cmd : spawn.do_after) { - if (cmd.command.type() == typeid(carla::rpc::Command::SetAutopilot)) { - autopilotValue = boost::get(cmd.command).enabled; - isAutopilot = true; + vehicles_to_enable_index.store(0); + vehicles_to_disable_index.store(0); + + auto ProcessCommand = [&](size_t min_index, size_t max_index) { + for (size_t i = min_index; i < max_index; ++i) { + if (!responses[i].HasError()) { + + bool isAutopilot = false; + bool autopilotValue = false; + + CommandType::CommandType& cmd_type = cmds[i].command; + const boost::typeindex::type_info& cmd_type_info = cmd_type.type(); + + // check SpawnActor command + if (cmd_type_info == typeid(carla::rpc::Command::SpawnActor)) { + // check inside 'do_after' + auto &spawn = boost::get(cmd_type); + for (auto &cmd : spawn.do_after) { + if (cmd.command.type() == typeid(carla::rpc::Command::SetAutopilot)) { + autopilotValue = boost::get(cmd.command).enabled; + isAutopilot = true; + } } } - } - - // check SetAutopilot command - if (cmds[i].command.type() == typeid(carla::rpc::Command::SetAutopilot)) { - autopilotValue = boost::get(cmds[i].command).enabled; - isAutopilot = true; - } - - // check if found any SetAutopilot command - if (isAutopilot) { - // get the id - carla::rpc::ActorId id = static_cast(responses[i].Get()); - // get traffic manager instance - if (!tm) { - tm = &carla::traffic_manager::TrafficManager::GetInstance( - carla::traffic_manager::TrafficManager::GetUniqueLocalClient()); + // check SetAutopilot command + else if (cmd_type_info == typeid(carla::rpc::Command::SetAutopilot)) { + autopilotValue = boost::get(cmd_type).enabled; + isAutopilot = true; } - // get all actors - carla::SharedPtr actor; - if (tm) { - actor = tm->GetWorld().GetActor(id); - } - // check to enable or disable - if (actor) { - if (autopilotValue) { - vehicles_to_enable.push_back(actor); - } else { - vehicles_to_disable.push_back(actor); + + // check if found any SetAutopilot command + if (isAutopilot) { + // get the id + carla::rpc::ActorId id = static_cast(responses[i].Get()); + + // get all actors + carla::SharedPtr actor; + actor = world.GetActor(id); + + // check to enable or disable + if (actor) { + if (autopilotValue) { + size_t index = vehicles_to_enable_index.fetch_add(1); + vehicles_to_enable[index] = actor; + } else { + size_t index = vehicles_to_disable_index.fetch_add(1); + vehicles_to_disable[index] = actor; + } } } } } + }; + + const size_t TaskLimit = 50; + size_t num_commands = cmds.size(); + size_t num_batches = num_commands / TaskLimit; + + std::vector t(num_batches+1); + + for(size_t n = 0; n < num_batches; n++) { + t[n] = new std::thread(ProcessCommand, n * TaskLimit, (n+1) * TaskLimit); } + t[num_batches] = new std::thread(ProcessCommand, num_batches * TaskLimit, num_commands); + + for(size_t n = 0; n <= num_batches; n++) { + if(t[n]->joinable()){ + t[n]->join(); + } + delete t[n]; + } + + // Fix vector size + vehicles_to_enable.resize(vehicles_to_enable_index.load()); + vehicles_to_disable.resize(vehicles_to_disable_index.load()); + // Release memory + vehicles_to_enable.shrink_to_fit(); + vehicles_to_disable.shrink_to_fit(); + // check if any autopilot command was sent - if ((vehicles_to_enable.size() || vehicles_to_disable.size()) && tm) { - tm->RegisterVehicles(vehicles_to_enable); - tm->UnregisterVehicles(vehicles_to_disable); + if ((vehicles_to_enable.size() || vehicles_to_disable.size())) { + self.GetInstanceTM().RegisterVehicles(vehicles_to_enable); + self.GetInstanceTM().UnregisterVehicles(vehicles_to_disable); } return result; @@ -136,5 +173,6 @@ void export_client() { .def("set_replayer_ignore_hero", &cc::Client::SetReplayerIgnoreHero, (arg("ignore_hero"))) .def("apply_batch", &ApplyBatchCommands, (arg("commands"), arg("do_tick")=false)) .def("apply_batch_sync", &ApplyBatchCommandsSync, (arg("commands"), arg("do_tick")=false)) + .def("get_trafficmanager", CONST_CALL_WITHOUT_GIL_1(cc::Client, GetInstanceTM, uint16_t), (arg("port")=TM_DEFAULT_PORT)) ; } diff --git a/PythonAPI/carla/source/libcarla/TrafficManager.cpp b/PythonAPI/carla/source/libcarla/TrafficManager.cpp index ee8ceebcc..343190420 100644 --- a/PythonAPI/carla/source/libcarla/TrafficManager.cpp +++ b/PythonAPI/carla/source/libcarla/TrafficManager.cpp @@ -15,26 +15,17 @@ void export_trafficmanager() { namespace cc = carla::client; using namespace boost::python; - using ActorList = std::vector>; - using Parameters = std::vector; - class_("TM_ActorList").def(vector_indexing_suite()); - - class_("TM_Parameters").def(vector_indexing_suite()); - - class_("TrafficManager", no_init) - .def("register_vehicles", &carla::traffic_manager::TrafficManager::RegisterVehicles) - .def("unregister_vehicles", &carla::traffic_manager::TrafficManager::UnregisterVehicles) - .def("set_vehicle_max_speed_difference", &carla::traffic_manager::TrafficManager::SetPercentageSpeedDifference) - .def("set_global_max_speed_difference", &carla::traffic_manager::TrafficManager::SetGlobalPercentageSpeedDifference) - .def("set_collision_detection", &carla::traffic_manager::TrafficManager::SetCollisionDetection) + class_("TrafficManager", no_init) + .def("vehicle_percentage_speed_difference", &carla::traffic_manager::TrafficManager::SetPercentageSpeedDifference) + .def("global_percentage_speed_difference", &carla::traffic_manager::TrafficManager::SetGlobalPercentageSpeedDifference) + .def("collision_detection", &carla::traffic_manager::TrafficManager::SetCollisionDetection) .def("force_lane_change", &carla::traffic_manager::TrafficManager::SetForceLaneChange) - .def("set_auto_lane_change", &carla::traffic_manager::TrafficManager::SetAutoLaneChange) - .def("set_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetDistanceToLeadingVehicle) + .def("auto_lane_change", &carla::traffic_manager::TrafficManager::SetAutoLaneChange) + .def("distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetDistanceToLeadingVehicle) .def("reset_traffic_lights", &carla::traffic_manager::TrafficManager::ResetAllTrafficLights) - .def("ignore_actors_percentage", &carla::traffic_manager::TrafficManager::SetPercentageIgnoreActors) - .def("ignore_lights_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningLight); - - def("GetTrafficManager", &carla::traffic_manager::TrafficManager::GetInstance, return_value_policy()); - + .def("ignore_walkers_percentage", &carla::traffic_manager::TrafficManager::SetPercentageIgnoreWalkers) + .def("ignore_vehicles_percentage", &carla::traffic_manager::TrafficManager::SetPercentageIgnoreVehicles) + .def("ignore_lights_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningLight) + .def("ignore_signs_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningSign); } diff --git a/PythonAPI/examples/manual_control.py b/PythonAPI/examples/manual_control.py index 6d16013d3..8498c9331 100755 --- a/PythonAPI/examples/manual_control.py +++ b/PythonAPI/examples/manual_control.py @@ -306,7 +306,12 @@ class KeyboardControl(object): if self._is_quit_shortcut(event.key): return True elif event.key == K_BACKSPACE: - world.restart() + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() elif event.key == K_F1: world.hud.toggle_info() elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): diff --git a/PythonAPI/examples/spawn_npc.py b/PythonAPI/examples/spawn_npc.py index 103846678..0c6f517e5 100755 --- a/PythonAPI/examples/spawn_npc.py +++ b/PythonAPI/examples/spawn_npc.py @@ -68,6 +68,16 @@ def main(): metavar='PATTERN', default='walker.pedestrian.*', help='pedestrians filter (default: "walker.pedestrian.*")') + argparser.add_argument( + '-tm_p', '--tm_port', + metavar='P', + default=8000, + type=int, + help='port to communicate with TM (default: 8000)') + argparser.add_argument( + '--sync', + action='store_true', + help='Synchronous mode execution') args = argparser.parse_args() logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) @@ -76,11 +86,25 @@ def main(): walkers_list = [] all_id = [] client = carla.Client(args.host, args.port) - client.set_timeout(2.0) + client.set_timeout(10.0) try: + traffic_manager = client.get_trafficmanager(args.tm_port) world = client.get_world() + + synchronous_master = False + + if args.sync: + settings = world.get_settings() + if not settings.synchronous_mode: + synchronous_master = True + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + else: + synchronous_master = False + blueprints = world.get_blueprint_library().filter(args.filterv) blueprintsWalkers = world.get_blueprint_library().filter(args.filterw) @@ -88,6 +112,8 @@ def main(): blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] blueprints = [x for x in blueprints if not x.id.endswith('isetta')] blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] + blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] + blueprints = [x for x in blueprints if not x.id.endswith('t2')] spawn_points = world.get_map().get_spawn_points() number_of_spawn_points = len(spawn_points) @@ -121,7 +147,7 @@ def main(): blueprint.set_attribute('role_name', 'autopilot') batch.append(SpawnActor(blueprint, transform).then(SetAutopilot(FutureActor, True))) - for response in client.apply_batch_sync(batch): + for response in client.apply_batch_sync(batch, synchronous_master): if response.error: logging.error(response.error) else: @@ -188,7 +214,10 @@ def main(): all_actors = world.get_actors(all_id) # wait for a tick to ensure client receives the last transform of the walkers we have just created - world.wait_for_tick() + if not args.sync or not synchronous_master: + world.wait_for_tick() + else: + world.tick() # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) # set how many pedestrians can cross the road @@ -203,11 +232,23 @@ def main(): print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) + # example of how to use parameters + traffic_manager.global_percentage_speed_difference(30.0) + while True: - world.wait_for_tick() + if args.sync and synchronous_master: + world.tick() + else: + world.wait_for_tick() finally: + if args.sync and synchronous_master: + settings = world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + world.apply_settings(settings) + print('\ndestroying %d vehicles' % len(vehicles_list)) client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) diff --git a/PythonAPI/examples/tm_spawn_npc.py b/PythonAPI/examples/tm_spawn_npc.py deleted file mode 100644 index 05726dde8..000000000 --- a/PythonAPI/examples/tm_spawn_npc.py +++ /dev/null @@ -1,223 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -"""Spawn NPCs into the simulation using the Traffic Manager interface""" - -import time -import random -import glob -import argparse -import logging -import sys -import os -try: - sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( - sys.version_info.major, - sys.version_info.minor, - 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) -except IndexError: - pass -import carla - -def main(): - argparser = argparse.ArgumentParser( - description=__doc__) - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '-n', '--number-of-vehicles', - metavar='N', - default=10, - type=int, - help='number of vehicles (default: 10)') - argparser.add_argument( - '-w', '--number-of-walkers', - metavar='W', - default=50, - type=int, - help='number of walkers (default: 50)') - argparser.add_argument( - '--safe', - action='store_true', - help='avoid spawning vehicles prone to accidents') - argparser.add_argument( - '--filterv', - metavar='PATTERN', - default='vehicle.*', - help='vehicles filter (default: "vehicle.*")') - argparser.add_argument( - '--filterw', - metavar='PATTERN', - default='walker.pedestrian.*', - help='pedestrians filter (default: "walker.pedestrian.*")') - - args = argparser.parse_args() - - logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) - - vehicles_list = [] - walkers_list = [] - all_id = [] - client = carla.Client(args.host, args.port) - client.set_timeout(2.0) - - try: - world = client.get_world() - blueprints = world.get_blueprint_library().filter(args.filterv) - blueprintsWalkers = world.get_blueprint_library().filter(args.filterw) - - if args.safe: - blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] - blueprints = [x for x in blueprints if not x.id.endswith('isetta')] - blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] - - spawn_points = world.get_map().get_spawn_points() - number_of_spawn_points = len(spawn_points) - - if args.number_of_vehicles < number_of_spawn_points: - random.shuffle(spawn_points) - elif args.number_of_vehicles > number_of_spawn_points: - msg = 'Requested %d vehicles, but could only find %d spawn points' - logging.warning(msg, args.number_of_vehicles, number_of_spawn_points) - args.number_of_vehicles = number_of_spawn_points - - # @todo cannot import these directly. - SpawnActor = carla.command.SpawnActor - - # -------------- - # Spawn vehicles - # -------------- - - for n, transform in enumerate(spawn_points): - if n >= args.number_of_vehicles: - break - blueprint = random.choice(blueprints) - if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color').recommended_values) - blueprint.set_attribute('color', color) - if blueprint.has_attribute('driver_id'): - driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) - blueprint.set_attribute('driver_id', driver_id) - blueprint.set_attribute('role_name', 'autopilot') - vehicle = world.try_spawn_actor(blueprint, transform) - vehicles_list.append(vehicle) - - # ------------- - # Spawn Walkers - # ------------- - # some settings - percentagePedestriansRunning = 0.0 # how many pedestrians will run - percentagePedestriansCrossing = 0.0 # 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): - spawn_point = carla.Transform() - loc = world.get_random_location_from_navigation() - if (loc != None): - spawn_point.location = loc - spawn_points.append(spawn_point) - # 2. we spawn the walker object - batch = [] - walker_speed = [] - for spawn_point in spawn_points: - walker_bp = random.choice(blueprintsWalkers) - # set as not invencible - if walker_bp.has_attribute('is_invincible'): - walker_bp.set_attribute('is_invincible', 'false') - # set the max speed - if walker_bp.has_attribute('speed'): - if (random.random() > percentagePedestriansRunning): - # walking - walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) - else: - # running - walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) - else: - print("Walker has no speed") - walker_speed.append(0.0) - batch.append(SpawnActor(walker_bp, spawn_point)) - results = client.apply_batch_sync(batch, True) - walker_speed2 = [] - for i in range(len(results)): - if results[i].error: - logging.error(results[i].error) - else: - walkers_list.append({"id": results[i].actor_id}) - walker_speed2.append(walker_speed[i]) - walker_speed = walker_speed2 - # 3. we spawn the walker controller - batch = [] - walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') - for i in range(len(walkers_list)): - batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) - results = client.apply_batch_sync(batch, True) - for i in range(len(results)): - if results[i].error: - logging.error(results[i].error) - else: - walkers_list[i]["con"] = results[i].actor_id - # 4. we put altogether the walkers and controllers id to get the objects from their id - for i in range(len(walkers_list)): - all_id.append(walkers_list[i]["con"]) - all_id.append(walkers_list[i]["id"]) - all_actors = world.get_actors(all_id) - - # wait for a tick to ensure client receives the last transform of the walkers we have just created - world.wait_for_tick() - - # 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(percentagePedestriansCrossing) - for i in range(0, len(all_id), 2): - # start walker - all_actors[i].start() - # set walk to random point - all_actors[i].go_to_location(world.get_random_location_from_navigation()) - # max speed - all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) - - print('Spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) - - time.sleep(1) - - for v in vehicles_list: - v.set_autopilot(True) - - while True: - time.sleep(1) - - finally: - - print('Destroying %d vehicles.\n' % len(vehicles_list)) - client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) - - # stop walker controllers (list is [controller, actor, controller, actor ...]) - for i in range(0, len(all_id), 2): - all_actors[i].stop() - - print('\ndestroying %d walkers' % len(walkers_list)) - client.apply_batch([carla.command.DestroyActor(x) for x in all_id]) - - -if __name__ == '__main__': - - try: - main() - except KeyboardInterrupt: - pass - finally: - print('Done.\n') diff --git a/PythonAPI/test/smoke/test_snapshot.py b/PythonAPI/test/smoke/test_snapshot.py index 60fc0b699..8c1976829 100644 --- a/PythonAPI/test/smoke/test_snapshot.py +++ b/PythonAPI/test/smoke/test_snapshot.py @@ -13,11 +13,20 @@ from . import SyncSmokeTest class TestSnapshot(SyncSmokeTest): def test_spawn_points(self): self.world = self.client.reload_world() + + # Check why the world settings aren't applied after a reload + self.settings = self.world.get_settings() + settings = carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=0.05) + self.world.apply_settings(settings) + spawn_points = self.world.get_map().get_spawn_points()[:20] vehicles = self.world.get_blueprint_library().filter('vehicle.*') batch = [(random.choice(vehicles), t) for t in spawn_points] batch = [carla.command.SpawnActor(*args) for args in batch] - response = self.client.apply_batch_sync(batch) + response = self.client.apply_batch_sync(batch, True) self.assertFalse(any(x.error for x in response)) ids = [x.actor_id for x in response] diff --git a/PythonAPI/util/performance_benchmark.py b/PythonAPI/util/performance_benchmark.py index 18b9d6453..6492de25e 100755 --- a/PythonAPI/util/performance_benchmark.py +++ b/PythonAPI/util/performance_benchmark.py @@ -8,14 +8,14 @@ """ This is a benchmarking script for CARLA. It serves to analyze the performance of CARLA in different scenarios and -conditions. +conditions, for both sensors and traffic. Please, make sure you install the following dependencies: * python -m pip install -U py-cpuinfo * python -m pip install psutil * python -m pip install python-tr - + * python -m pip install gpuinfo """ @@ -24,12 +24,10 @@ Please, make sure you install the following dependencies: import sys - if sys.version_info[0] < 3: print('This script is only available for Python 3') sys.exit(1) - from tr import tr import argparse import cpuinfo @@ -40,8 +38,10 @@ import os import psutil import pygame import shutil -import subprocess +import GPUtil import threading +import time +import logging try: sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( @@ -58,26 +58,36 @@ import carla # ====================================================================================================================== sensors_callback = [] -# ====================================================================================================================== -# -- Tunable parameters ------------------------------------------------------------------------------------------------ -# ====================================================================================================================== -number_locations = 5 -number_ticks = 30 -actor_list = ['vehicle.*'] +def define_weather(): + list_weather = [] + if args.tm: + weather00 = { 'parameter' : carla.WeatherParameters.ClearNoon, 'name': 'ClearNoon'} -def weathers(): - list_weathers = [carla.WeatherParameters.ClearNoon, - carla.WeatherParameters.CloudyNoon, - carla.WeatherParameters.SoftRainSunset - ] + list_weather.append(weather00) - return list_weathers + else: + weather00 = { 'parameter' : carla.WeatherParameters.ClearNoon, 'name' : 'ClearNoon'} + weather01 = { 'parameter' : carla.WeatherParameters.CloudyNoon, 'name' : 'CloudyNoon'} + weather02 = { 'parameter' : carla.WeatherParameters.SoftRainSunset, 'name' : 'SoftRainSunset'} + + list_weather.append(weather00) + list_weather.append(weather01) + list_weather.append(weather02) + + return list_weather def define_sensors(): - list_sensor_specs = [] + list_sensor_specs = [] + if args.tm: + sensors00 = [{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, + 'width': 300, 'height': 200, 'fov': 100, 'label': '1. cam-300x200'}] + + list_sensor_specs.append(sensors00) + + else: sensors00 = [{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'width': 300, 'height': 200, 'fov': 100, 'label': '1. cam-300x200'}] @@ -102,7 +112,28 @@ def define_sensors(): list_sensor_specs.append(sensors03) list_sensor_specs.append(sensors04) - return list_sensor_specs + return list_sensor_specs + +def define_environments(): + list_env_specs = [] + + if args.tm: + env00 = {'vehicles': 10, 'walkers': 0} + env01 = {'vehicles': 50, 'walkers': 50} + env02 = {'vehicles': 250, 'walkers': 0} + env03 = {'vehicles': 150, 'walkers': 50} + + list_env_specs.append(env00) + list_env_specs.append(env01) + list_env_specs.append(env02) + list_env_specs.append(env03) + + else: + env00 = {'vehicles': 1, 'walkers': 0} + + list_env_specs.append(env00) + + return list_env_specs class CallBack(object): @@ -120,105 +151,212 @@ class CallBack(object): return self._current_fps -def create_ego_vehicle(world, ego_vehicle, spawn_point, list_sensor_spec): - global sensors_callback +def create_environment(world, sensors, n_vehicles, n_walkers, spawn_points, client): + global sensors_callback + sensors_ret = [] + blueprint_library = world.get_blueprint_library() - if ego_vehicle: - ego_vehicle.set_transform(spawn_point) - sensors = None + # setup sensors + for sensor_spec in sensors: + bp = blueprint_library.find(sensor_spec['type']) + + if sensor_spec['type'].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(sensor_spec['width'])) + bp.set_attribute('image_size_y', str(sensor_spec['height'])) + bp.set_attribute('fov', str(sensor_spec['fov'])) + sensor_location = carla.Location( + x=sensor_spec['x'], + y=sensor_spec['y'], + z=sensor_spec['z']) + sensor_rotation = carla.Rotation( + pitch=sensor_spec['pitch'], + roll=sensor_spec['roll'], + yaw=sensor_spec['yaw']) + + elif sensor_spec['type'].startswith('sensor.lidar'): + bp.set_attribute('range', '200') + bp.set_attribute('rotation_frequency', '10') + bp.set_attribute('channels', '32') + bp.set_attribute('upper_fov', '15') + bp.set_attribute('lower_fov', '-30') + bp.set_attribute('points_per_second', '500000') + sensor_location = carla.Location( + x=sensor_spec['x'], + y=sensor_spec['y'], + z=sensor_spec['z']) + sensor_rotation = carla.Rotation( + pitch=sensor_spec['pitch'], + roll=sensor_spec['roll'], + yaw=sensor_spec['yaw']) + + elif sensor_spec['type'].startswith('sensor.other.gnss'): + sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'], z=sensor_spec['z']) + sensor_rotation = carla.Rotation() + + # create sensor + sensor_transform = carla.Transform(sensor_location, sensor_rotation) + sensor = world.spawn_actor(bp, sensor_transform) + + # add callbacks + sc = CallBack() + sensor.listen(sc) + + sensors_callback.append(sc) + sensors_ret.append(sensor) + + vehicles_list = [] + walkers_list = [] + all_id = [] + + blueprint = world.get_blueprint_library().filter('vehicle.audi.a2')[0] + walker_bp = world.get_blueprint_library().filter("walker.pedestrian.0001")[0] + + # @todo cannot import these directly. + SpawnActor = carla.command.SpawnActor + SetAutopilot = carla.command.SetAutopilot + FutureActor = carla.command.FutureActor + + # -------------- + # Spawn vehicles + # -------------- + batch = [] + for num, transform in enumerate(spawn_points): + if num >= n_vehicles: + break + blueprint.set_attribute('role_name', 'autopilot') + batch.append(SpawnActor(blueprint, transform).then(SetAutopilot(FutureActor, True))) + + for response in client.apply_batch_sync(batch, False): + if response.error: + logging.error(response.error) else: - sensors = [] - blueprint_library = world.get_blueprint_library() - blueprint = blueprint_library.filter('vehicle.lincoln.mkz2017')[0] - ego_vehicle = world.try_spawn_actor(blueprint, spawn_point) + vehicles_list.append(response.actor_id) - # setup sensors - for sensor_spec in list_sensor_spec: - bp = blueprint_library.find(sensor_spec['type']) - if sensor_spec['type'].startswith('sensor.camera'): - bp.set_attribute('image_size_x', str(sensor_spec['width'])) - bp.set_attribute('image_size_y', str(sensor_spec['height'])) - bp.set_attribute('fov', str(sensor_spec['fov'])) - sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'], z=sensor_spec['z']) - sensor_rotation = carla.Rotation( - pitch=sensor_spec['pitch'], - roll=sensor_spec['roll'], - yaw=sensor_spec['yaw']) - elif sensor_spec['type'].startswith('sensor.lidar'): - bp.set_attribute('range', '200') - bp.set_attribute('rotation_frequency', '10') - bp.set_attribute('channels', '32') - bp.set_attribute('upper_fov', '15') - bp.set_attribute('lower_fov', '-30') - bp.set_attribute('points_per_second', '500000') + # ------------- + # Spawn Walkers + # ------------- + # some settings + percentagePedestriansRunning = 0.0 # how many pedestrians will run + percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road + # 1. take all the random locations to spawn + spawn_points = [] + for i in range(n_walkers): + spawn_point = carla.Transform() + loc = world.get_random_location_from_navigation() + if (loc != None): + spawn_point.location = loc + spawn_points.append(spawn_point) + # 2. we spawn the walker object + batch = [] + walker_speed = [] + for spawn_point in spawn_points: + # set as not invincible + if walker_bp.has_attribute('is_invincible'): + walker_bp.set_attribute('is_invincible', 'false') + # set the max speed + if walker_bp.has_attribute('speed'): + # walking + walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) + else: + print("Walker has no speed") + walker_speed.append(0.0) + batch.append(SpawnActor(walker_bp, spawn_point)) + results = client.apply_batch_sync(batch, True) + walker_speed2 = [] + for i in range(len(results)): + if results[i].error: + logging.error(results[i].error) + else: + walkers_list.append({"id": results[i].actor_id}) + walker_speed2.append(walker_speed[i]) + walker_speed = walker_speed2 + # 3. we spawn the walker controller + batch = [] + walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') + for i in range(len(walkers_list)): + batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) + results = client.apply_batch_sync(batch, True) + for i in range(len(results)): + if results[i].error: + logging.error(results[i].error) + else: + walkers_list[i]["con"] = results[i].actor_id + # 4. we put altogether the walkers and controllers id to get the objects from their id + for i in range(len(walkers_list)): + all_id.append(walkers_list[i]["con"]) + all_id.append(walkers_list[i]["id"]) + all_actors = world.get_actors(all_id) - sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'], z=sensor_spec['z']) - sensor_rotation = carla.Rotation( - pitch=sensor_spec['pitch'], - roll=sensor_spec['roll'], - yaw=sensor_spec['yaw']) - elif sensor_spec['type'].startswith('sensor.other.gnss'): - sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'], z=sensor_spec['z']) - sensor_rotation = carla.Rotation() + # wait for a tick to ensure client receives the last transform of the walkers we have just created + world.wait_for_tick() - # create sensor - sensor_transform = carla.Transform(sensor_location, sensor_rotation) - sensor = world.spawn_actor(bp, sensor_transform, ego_vehicle) + # 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(percentagePedestriansCrossing) + for i in range(0, len(all_id), 2): + # start walker + all_actors[i].start() + # set walk to random point + all_actors[i].go_to_location(world.get_random_location_from_navigation()) + # max speed + all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) - # add callbacks - sc = CallBack() - sensor.listen(sc) + print('Spawned %d vehicles and %d walkers.' % (len(vehicles_list), len(walkers_list))) - sensors_callback.append(sc) - sensors.append(sensor) - return ego_vehicle, sensors + return vehicles_list, walkers_list, all_id, all_actors, sensors_ret # ====================================================================================================================== # -- Benchmarking functions -------------------------------------------------------------------------------------------- # ====================================================================================================================== -def run_benchmark(world, sensor_specs_list, number_locations, number_ticks, actor_list, debug=False): - global sensors_callback +def run_benchmark(world, sensors, n_vehicles, n_walkers, client, debug=False): + global sensors_callback - spawn_points = world.get_map().get_spawn_points() - n = min(number_locations, len(spawn_points)) + spawn_points = world.get_map().get_spawn_points() + n = min(n_vehicles, len(spawn_points)) + list_fps = [] + sensor_list = None - ego_vehicle = None - list_fps = [] - sensor_list = None - for i in range(n): - spawn_point = spawn_points[i] - ego_vehicle, sensors = create_ego_vehicle(world, ego_vehicle, spawn_point, sensor_specs_list) - if sensors: - sensor_list = sensors - ego_vehicle.set_autopilot(True) + vehicles_list, walkers_list, all_id, all_actors, sensors_ret = create_environment(world, sensors, n, n_walkers, spawn_points, client) - ticks = 0 - while ticks < number_ticks: - _ = world.wait_for_tick(1000.0) - if debug: - print("== Samples {} / {}".format(ticks + 1, number_ticks)) + if sensors_ret: + sensor_list = sensors_ret - min_fps = float('inf') - for sc in sensors_callback: - fps = sc.get_fps() - if fps < min_fps: - min_fps = fps - if math.isinf(min_fps): - min_fps = 0 - list_fps.append(min_fps) + ticks = 0 + while ticks < args.ticks: + _ = world.wait_for_tick() + if debug: + print("== Samples {} / {}".format(ticks + 1, args.ticks)) - ticks += 1 + min_fps = float('inf') + for sc in sensors_callback: + fps = sc.get_fps() + if fps < min_fps: + min_fps = fps + if math.isinf(min_fps): + min_fps = 0 + list_fps.append(min_fps) - for sensor in sensor_list: - sensor.stop() - sensor.destroy() - sensors_callback.clear() - ego_vehicle.destroy() + ticks += 1 - return list_fps + for sensor in sensor_list: + sensor.stop() + sensor.destroy() + sensors_callback.clear() + + print('Destroying %d vehicles.\n' % len(vehicles_list)) + client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) + + # stop walker controllers (list is [controller, actor, controller, actor ...]) + for i in range(0, len(all_id), 2): + all_actors[i].stop() + + print('\ndestroying %d walkers' % len(walkers_list)) + client.apply_batch([carla.command.DestroyActor(x) for x in all_id]) + + return list_fps def compute_mean_std(list_values): @@ -231,108 +369,121 @@ def compute_mean_std(list_values): def serialize_records(records, system_specs, filename): - with open(filename, 'w+') as fd: - s = "| Sensors | Town | Weather | Samples | Mean fps | Std fps |\n" - s += "| ----------- | ----------- | ----------- | ----------- | ----------- | ----------- |\n" + with open(filename, 'w+') as fd: + s = "| Town | Sensors | Weather | # of Vehicles | # of Walkers | Samples | Mean FPS | Std FPS |\n" + s += "| ----------- | ----------- | ----------- | ----------- | ----------- | ----------- | ----------- |\n" + fd.write(s) + + for sensor_key in sorted(records.keys()): + list_records = records[sensor_key] + for record in list_records: + s = "| {} | {} | {} | {} | {} | {} | {:03.2f} | {:03.2f} |\n".format(record['town'], + record['sensors'], + record['weather'], + record['n_vehicles'], + record['n_walkers'], + record['samples'], + record['fps_mean'], + record['fps_std']) fd.write(s) - for sensor_key in sorted(records.keys()): - list_records = records[sensor_key] - for record in list_records: - s = "| {} | {} | {} | {} | {:03.2f} | {:03.2f} |\n".format(record['sensors'], - record['town'], - record['weather'], - record['samples'], - record['fps_mean'], - record['fps_std']) - fd.write(s) + s = "\n| Global mean FPS | Global std FPS |\n" + s += "| **{:03.2f}** | **{:03.2f}** |\n".format(*get_total(records)) + fd.write(s) - s = "| | | | | **{:03.2f}** | **{:03.2f}** |\n".format(*get_total(records)) - fd.write(s) - - s = "Table: {}.\n".format(system_specs) - fd.write(s) + s = "Table: {}.\n".format(system_specs) + fd.write(s) def get_total(records): - record_vals = [item for sublist in records.values() for item in sublist] - total_mean_fps = sum([r['fps_mean'] for r in record_vals]) / len(record_vals) - total_mean_std = sum([r['fps_std'] for r in record_vals]) / len(record_vals) - return total_mean_fps, total_mean_std + record_vals = [item for sublist in records.values() for item in sublist] + total_mean_fps = sum([r['fps_mean'] for r in record_vals]) / len(record_vals) + total_mean_std = sum([r['fps_std'] for r in record_vals]) / len(record_vals) + return total_mean_fps, total_mean_std def get_system_specs(): - str_system = "" - cpu_info = cpuinfo.get_cpu_info() - str_system += "CPU {} {}. ".format(cpu_info['brand'], cpu_info['family']) + str_system = "" + cpu_info = cpuinfo.get_cpu_info() + str_system += "CPU {} {}. ".format(cpu_info['brand'], cpu_info['family']) - memory_info = psutil.virtual_memory() - str_system += "{:03.2f} GB RAM memory. ".format(memory_info.total / (1024 * 1024 * 1024)) + memory_info = psutil.virtual_memory() + str_system += "{:03.2f} GB RAM memory. ".format(memory_info.total / (1024 * 1024 * 1024)) + nvidia_cmd = shutil.which("nvidia-smi") + if nvidia_cmd: + str_system += "GPU " + gpu_info = GPUtil.getGPUs() + for gpu in gpu_info: + str_system += "{} ".format(gpu.name) - nvidia_cmd = shutil.which("nvidia-smi") - if nvidia_cmd: - gpu_info = subprocess.check_output([nvidia_cmd]) - gpu_info_ext = subprocess.check_output([nvidia_cmd, '-L']) - for line in gpu_info.decode('ascii').split("\n"): - if "CarlaUE4" in line: - gpu_id = tr(' ', '', line, 's').split(" ")[1] - for gpu_line in gpu_info_ext.decode('ascii').split("\n"): - gpu_line_id = gpu_line.split(" ")[1].split(":")[0] - if gpu_line_id == gpu_id: - gpu_model = gpu_line.split(":")[1].split("(")[0] - str_system += "GPU {}".format(gpu_model) - break - - return str_system + return str_system def main(args): - client = carla.Client(args.host, int(args.port)) - client.set_timeout(60.0) - pygame.init() + client = carla.Client(args.host, int(args.port)) + client.set_timeout(60.0) + pygame.init() - records = {} - for town in sorted(client.get_available_maps()): - world = client.load_world(town) + records = {} + maps = [m.replace('/Game/Carla/Maps/', '') for m in client.get_available_maps()] - # spectator pointing to the sky to reduce rendering impact - spectator = world.get_spectator() - spectator.set_transform(carla.Transform(carla.Location(z=500), carla.Rotation(pitch=90))) + for town in sorted(maps): + world = client.load_world(town) - for weather in weathers(): - world.set_weather(weather) - for sensors in define_sensors(): - list_fps = run_benchmark(world, sensors, number_locations, number_ticks, actor_list) - mean, std = compute_mean_std(list_fps) + # set to async mode + settings = world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + world.apply_settings(settings) - sensor_str = "" - for sensor in sensors: - sensor_str += (sensor['label'] + " ") + # spectator pointing to the sky to reduce rendering impact + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(z=500), carla.Rotation(pitch=90))) - record = {'sensors': sensor_str, - 'weather': weather, - 'town': town, - 'samples': number_locations * number_ticks, - 'fps_mean': mean, - 'fps_std': std} + for weather in define_weather(): + world.set_weather(weather["parameter"]) + for env in define_environments(): + for sensors in define_sensors(): + list_fps = run_benchmark(world, sensors, env["vehicles"], env["walkers"], client) + mean, std = compute_mean_std(list_fps) + sensor_str = "" + for sensor in sensors: + sensor_str += (sensor['label'] + " ") - if sensor_str not in records: - records[sensor_str] = [] - records[sensor_str].append(record) - print(record) + record = { + 'town': town, + 'sensors': sensor_str, + 'weather': weather["name"], + 'n_vehicles': env["vehicles"], + 'n_walkers': env["walkers"], + 'samples': args.ticks, + 'fps_mean': mean, + 'fps_std': std + } - system_specs = get_system_specs() - serialize_records(records, system_specs, args.file) - pygame.quit() + env_str = str(env["vehicles"]) + str(env["walkers"]) + + if env_str not in records: + records[env_str] = [] + records[env_str].append(record) + print(record) + + system_specs = get_system_specs() + serialize_records(records, system_specs, args.file) + pygame.quit() if __name__ == '__main__': - description = "Benchmark CARLA performance in your platform for different towns and sensor configurations\n" + description = "Benchmark CARLA performance in your platform for different towns and sensor or traffic configurations.\n" - parser = argparse.ArgumentParser(description=description) - parser.add_argument('--host', default='localhost', help='IP of the host server (default: localhost)') - parser.add_argument('--port', default='2000', help='TCP port to listen to (default: 2000)') - parser.add_argument('--file', type=str, help='Write results into a txt file', default="benchmark.md") - args = parser.parse_args() + parser = argparse.ArgumentParser(description=description) + parser.add_argument('--host', default='localhost', help='IP of the host server (default: localhost)') + parser.add_argument('--port', default='2000', help='TCP port to listen to (default: 2000)') + parser.add_argument('--file', type=str, help='Write results into a txt file', default="benchmark.md") + parser.add_argument('--tm', action='store_true', help='Switch to traffic manager benchmark') + parser.add_argument('--ticks', default=100, help='Number of ticks for each scenario (default: 100)') + + args = parser.parse_args() + + main(args) - main(args) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index 5368b3491..cdb96aaf3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -41,6 +41,7 @@ #include #include +#include template using R = carla::rpc::Response; @@ -71,6 +72,9 @@ public: BindActions(); } + /// Map of pairs < port , ip > with all the Traffic Managers active in the simulation + std::map TrafficManagerInfo; + carla::rpc::Server Server; carla::streaming::Server StreamingServer; @@ -152,6 +156,47 @@ void FCarlaServer::FPimpl::BindActions() namespace cr = carla::rpc; namespace cg = carla::geom; + /// Looks for a Traffic Manager running on port + BIND_SYNC(is_traffic_manager_running) << [this] (uint16_t port) ->R + { + return (TrafficManagerInfo.find(port) != TrafficManagerInfo.end()); + }; + + /// Gets a pair filled with the of the Trafic Manager running on port. + /// If there is no Traffic Manager running the pair will be ("", 0) + BIND_SYNC(get_traffic_manager_running) << [this] (uint16_t port) ->R> + { + auto it = TrafficManagerInfo.find(port); + if(it != TrafficManagerInfo.end()) { + return std::pair(it->second, it->first); + } + return std::pair("",0); + }; + + /// Add a new Traffic Manager running on + BIND_SYNC(add_traffic_manager_running) << [this] (std::pair trafficManagerInfo) ->R + { + uint16_t port = trafficManagerInfo.second; + auto it = TrafficManagerInfo.find(port); + if(it == TrafficManagerInfo.end()) { + TrafficManagerInfo.insert( + std::pair(port, trafficManagerInfo.first)); + return true; + } + return false; + + }; + + BIND_SYNC(destroy_traffic_manager) << [this] (uint16_t port) ->R + { + auto it = TrafficManagerInfo.find(port); + if(it != TrafficManagerInfo.end()) { + TrafficManagerInfo.erase(it); + return true; + } + return false; + }; + BIND_ASYNC(version) << [] () -> R { return carla::version(); diff --git a/Util/BuildTools/Setup.bat b/Util/BuildTools/Setup.bat index fa9240b60..21bd84260 100644 --- a/Util/BuildTools/Setup.bat +++ b/Util/BuildTools/Setup.bat @@ -208,7 +208,8 @@ set CMAKE_CONFIG_FILE="%INSTALLATION_DIR%CMakeLists.txt.in" >>"%CMAKE_CONFIG_FILE%" echo set(CMAKE_CXX_STANDARD 14) >>"%CMAKE_CONFIG_FILE%" echo set(CMAKE_CXX_STANDARD_REQUIRED ON) >>"%CMAKE_CONFIG_FILE%" echo. ->>"%CMAKE_CONFIG_FILE%" echo add_definitions(-D_WIN32_WINNT=0x0501) +>>"%CMAKE_CONFIG_FILE%" echo add_definitions(-D_WIN32_WINNT=0x0600) +>>"%CMAKE_CONFIG_FILE%" echo add_definitions(-DHAVE_SNPRINTF) >>"%CMAKE_CONFIG_FILE%" echo STRING (REGEX REPLACE "/RTC(su|[1su])" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") >>"%CMAKE_CONFIG_FILE%" echo. >>"%CMAKE_CONFIG_FILE%" echo add_definitions(-DBOOST_ERROR_CODE_HEADER_ONLY) diff --git a/mkdocs.yml b/mkdocs.yml index 02d0422a0..1a59ba32d 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -7,7 +7,7 @@ extra_css: [extra.css] nav: - Home: 'index.md' -- Getting started: +- Getting started: - 'Introduction': 'getting_started/introduction.md' - 'Quick start': 'getting_started/quickstart.md' - Building CARLA: @@ -27,7 +27,7 @@ nav: - 'Recorder': 'recorder_and_playback.md' - 'Rendering options': 'rendering_options.md' - 'Synchrony and time-step': 'simulation_time_and_synchrony.md' -- References: +- References: - 'Python API reference': 'python_api.md' - 'Code recipes': 'python_cookbook.md' - 'Blueprint Library': 'bp_library.md'