p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Doterop/traffic manager (#2468) 

* Initial implementation of t.m. performance script

* Remove redundant getLocation() calls

* Demo for inter-client communication via Carla server

* WIP: To do: For client usage needed to be changed.

* Instead or client instance episodeProxy is passed to TM

* Instead or client instance episodeProxy is passed to TM

* parmeter improvements (walkers,cars,signs,lights)

* adding section id to map setup

* fix manual_control to reload car with autopilot on

* Instead of client instance episodeProxy is passed to TM.

* WIP: Only added vehicle register via RPC server / client call in TM to
local instance from remote instance. Similarly other APIs we need to
added.

* Added all TM APIs

* carla client now can provide TM instance if required.

* While getting TM instance if server given data is not valid new TM
instace is created. Need to decide what to do with earlier registed
vehicle with previous TM.

* Updated with HealthCheckRemoteTM() function to check TM server status
instead of ResetAllTrafficLights() for any new user client.

* Dynamic port selection added to TM server instace (in case of multiple
TM serve run, it required).

* Fixed change map error

* If registered TM at carla server is not present then remote TM should be
freed and new local TM instance is created.

* Created Process for Local TM.
It cheaks with total registered vehicles in it, if no register vehicles
for 5 sec, it closes it self.

In python spawn_npc.py Client Timeout set to 10 sec. as creating Process
is system dependent call and takes time.

User has to unregister vehicle before destroying it (if activated with
TM). Also neet to check TM unregister function to remove vehicles from
register list.

* Fix for collision ignore bug

* Update TM exit function check to compare registered vehicles with world
actors that if any valid vehicle present cointinue to run the TM else
stop.

* Print removal

* Splited work of main for loop in ApplyBatchCommandsSync

* WIP! Trying to get client directly

* WIP! Trying to access episode properly without getting it from TM ctr

* Changes to BatchControlStage for Sync Mode
Changes to Parameters for Synch Mode

* Added port support for TM. Multiclient MultiTM

* Added additions for RPC Synchronous Tick to TM.
Error handeling and code organization.

* Exposed API for Sync Mode

* TMServer notifies Server that it is gonna be destroyed

* Exposed Python API for Sync Mode

* Add TM as separate process and synchronous tick calls

* SetSynchronousModeTimeOutInMiliSecond method added

* TM shutsdown and informs to the connected clients

* WIP! Map change issues again

* Updated Traffic Manager for:
1) Multi-client, Multi-TM (as required) can be reistered to server.
2) TM is not a PROCESS separated from the user-client.
3) TM will exit if user-client exit.
4) If server closed, TM also get closed by catching runtime err.

* Server side changes ...

* Fixed possible stall on TM destruction

* fixing collision stage + cybertruck not safe

* Merge branch 'master' into 'soumyadeep/traffic_manager'

* WIP! Disconnection of server has to be properly handled by clients

* format update

* Fix bug unsignalized junctions

* # WARNING: head commit changed in the meantime

Merge branch 'doterop/traffic_manager' into soumyadeep/traffic_manager
Updates for Syncronized Tick.

* Updated runtime exception in Episode for smooth exit if TN server closed
for any other user client.

* Better Exception handeling

* Merged with jackbart94/tm_reduce_getloc_calls

* Merged soumyadeep/traffic_manager.
Removed World getter and recovered Episode as a parameter of TM ctr.
Fixed syntax errors.

* Initial implementation of t.m. performance script

* Remove redundant getLocation() calls

* Demo for inter-client communication via Carla server

* WIP: To do: For client usage needed to be changed.

* Instead or client instance episodeProxy is passed to TM

* Instead or client instance episodeProxy is passed to TM

* parmeter improvements (walkers,cars,signs,lights)

* fix manual_control to reload car with autopilot on

* Instead of client instance episodeProxy is passed to TM.

* WIP: Only added vehicle register via RPC server / client call in TM to
local instance from remote instance. Similarly other APIs we need to
added.

* Added all TM APIs

* carla client now can provide TM instance if required.

* While getting TM instance if server given data is not valid new TM
instace is created. Need to decide what to do with earlier registed
vehicle with previous TM.

* Updated with HealthCheckRemoteTM() function to check TM server status
instead of ResetAllTrafficLights() for any new user client.

* Dynamic port selection added to TM server instace (in case of multiple
TM serve run, it required).

* Fixed change map error

* If registered TM at carla server is not present then remote TM should be
freed and new local TM instance is created.

* Created Process for Local TM.
It cheaks with total registered vehicles in it, if no register vehicles
for 5 sec, it closes it self.

In python spawn_npc.py Client Timeout set to 10 sec. as creating Process
is system dependent call and takes time.

User has to unregister vehicle before destroying it (if activated with
TM). Also neet to check TM unregister function to remove vehicles from
register list.

* Fix for collision ignore bug

* Update TM exit function check to compare registered vehicles with world
actors that if any valid vehicle present cointinue to run the TM else
stop.

* Splited work of main for loop in ApplyBatchCommandsSync

* WIP! Trying to get client directly

* WIP! Trying to access episode properly without getting it from TM ctr

* Changes to BatchControlStage for Sync Mode
Changes to Parameters for Synch Mode

* Added port support for TM. Multiclient MultiTM

* Added additions for RPC Synchronous Tick to TM.
Error handeling and code organization.

* Exposed API for Sync Mode

* TMServer notifies Server that it is gonna be destroyed

* Exposed Python API for Sync Mode

* Add TM as separate process and synchronous tick calls

* SetSynchronousModeTimeOutInMiliSecond method added

* TM shutsdown and informs to the connected clients

* WIP! Map change issues again

* Updated Traffic Manager for:
1) Multi-client, Multi-TM (as required) can be reistered to server.
2) TM is not a PROCESS separated from the user-client.
3) TM will exit if user-client exit.
4) If server closed, TM also get closed by catching runtime err.

* Fixed possible stall on TM destruction

* fixing collision stage + cybertruck not safe

* Merge branch 'master' into 'soumyadeep/traffic_manager'

* WIP! Disconnection of server has to be properly handled by clients

* format update

* Fix bug unsignalized junctions

* # WARNING: head commit changed in the meantime

Merge branch 'doterop/traffic_manager' into soumyadeep/traffic_manager
Updates for Syncronized Tick.

* Updated runtime exception in Episode for smooth exit if TN server closed
for any other user client.

* Better Exception handeling

* Merged with jackbart94/tm_reduce_getloc_calls

* Merged soumyadeep/traffic_manager.
Removed World getter and recovered Episode as a parameter of TM ctr.
Fixed syntax errors.

* Rebased with master

* Changes after rebase

* Solving tab errors

* Updated Changelog

* Removing Destroyed vehicle's from TM Server

* Fixed uint compatibility with Windows

* Merged soumyadeep/traffic_manager. Discarded destroyed actors

* -Removed unnecessary files

* restoring docs from rebase

* Fix windows compilation

* refactoring stage-related code

* more code refactoring

* When map change the simulation doesn't throw exception anymore

* Fixed incorrect episode

* Fixed map change and TM remote detached thread destruction

* Syntax and comments fixes

* Missed change on previous commit

* Fixed compile minor compile issue

* Cleaned and fixed some issues after merge

* fix to sync localization bugs
modified PID parameters
revamping spawn_npc

* deleted tm_spawn_npc

* fixes spawn error in sync mode

* Redoing TM sync logic

* finished performance benchmark for tm

* deprecated wrapped methods:
register_vehicle
unregister_vehicle

* New TM management

* Fixed sync mode on TM

* Cleaned TM of prints and unussed functions

* collision stage checks for (0,0,0) to ignore.
in memory map has an # between keys to avoid possible mixup.
fixed spawn_npc with new sync mode

* changelog

* added more connection retries

* fixed changelog + comments (see reviewable)

* Moved socket include's to single header

* Added missing line at the end of the file

* Fixed syntax errors

* final commit

* Minor correction in comment

* update copyright year to 2020 + removed break

* restoring unwanted changes

* patch for smoke test error

* Removed "todo" for pylint

Co-authored-by: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com>
Co-authored-by: Praveen Kumar <35625166+pravinblaze@users.noreply.github.com>
Co-authored-by: Soumyadeep <soumyadeep.dhar@kpit.com>
Co-authored-by: joel-mb <joel.moriana@gmail.com>
Co-authored-by: Sekhar Barua <58979936+sekhar2912@users.noreply.github.com>
Co-authored-by: bernat <bernatx@gmail.com>
Co-authored-by: Marc Garcia Puig <marcgpuig@gmail.com>
This commit is contained in:
doterop 2020-02-28 19:58:13 +01:00 committed by GitHub
parent 069b63873c
commit 85b192530d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
61 changed files with 2932 additions and 1215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
CHANGELOG.md
View File

@ -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
@ -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

17
LibCarla/source/carla/Sockets.h Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#pragma once
#if _WIN32
#include <winsock2.h> ///< socket
#include <Ws2tcpip.h>
#else
#include <sys/socket.h> ///< socket
#include <netinet/in.h> ///< sockaddr_in
#include <arpa/inet.h> ///< getsockname
#include <unistd.h> ///< close
#endif

22
LibCarla/source/carla/client/Client.h
View File

@ -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<detail::Simulator> _simulator;
};
inline Client::Client(

2
LibCarla/source/carla/client/Vehicle.cpp
View File

@ -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 {

6
LibCarla/source/carla/client/World.h
View File

@ -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;

18
LibCarla/source/carla/client/detail/Client.cpp
View File

@ -102,6 +102,22 @@ namespace detail {
const size_t worker_threads)
: _pimpl(std::make_unique<Pimpl>(host, port, worker_threads)) {}
bool Client::IsTrafficManagerRunning(uint16_t port) const {
return _pimpl->CallAndWait<bool>("is_traffic_manager_running", port);
}
std::pair<std::string, uint16_t> Client::GetTrafficManagerRunning(uint16_t port) const {
return _pimpl->CallAndWait<std::pair<std::string, uint16_t>>("get_traffic_manager_running", port);
};
bool Client::AddTrafficManagerRunning(std::pair<std::string, uint16_t> trafficManagerInfo) const {
return _pimpl->CallAndWait<bool>("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;
}

14
LibCarla/source/carla/client/detail/Client.h
View File

@ -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 <IP, port> of the Trafic Manager running on port.
/// If there is no Traffic Manager running the pair will be ("", 0)
std::pair<std::string, uint16_t> GetTrafficManagerRunning(uint16_t port) const;
/// Informs the server that a Traffic Manager is running on <IP, port>
bool AddTrafficManagerRunning(std::pair<std::string, uint16_t> 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();

35
LibCarla/source/carla/client/detail/Episode.cpp
View File

@ -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 <exception>
@ -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<target_t &>(data);
@ -51,11 +54,28 @@ namespace detail {
std::weak_ptr<Episode> 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<const EpisodeState>(CastData(*data));
auto prev = self->GetState();
/// 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);
@ -63,8 +83,9 @@ namespace detail {
}
} while (!self->_state.compare_exchange(&prev, next));
if (next->GetEpisodeId() != prev->GetEpisodeId()) {
self->OnEpisodeStarted();
/// Episode change
if(episode_changed) {
self->OnEpisodeChanged();
}
// Notify waiting threads and do the callbacks.
@ -79,6 +100,7 @@ namespace detail {
// Call user callbacks.
self->_on_tick_callbacks.Call(next);
}
}
});
}
@ -118,6 +140,11 @@ namespace detail {
_actors.Clear();
_on_tick_callbacks.Clear();
_navigation.reset();
traffic_manager::TrafficManager::Release();
}
void Episode::OnEpisodeChanged() {
traffic_manager::TrafficManager::Reset();
}
} // namespace detail

11
LibCarla/source/carla/client/detail/Episode.h
View File

@ -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<const EpisodeState> _state;
AtomicSharedPtr<WalkerNavigation> _navigation;
std::string _pending_exceptions_msg;
CachedActorList _actors;
CallbackList<WorldSnapshot> _on_tick_callbacks;
@ -104,6 +113,8 @@ namespace detail {
RecurrentSharedFuture<WorldSnapshot> _snapshot;
const streaming::Token _token;
bool _pending_exceptions = false;
};
} // namespace detail

5
LibCarla/source/carla/client/detail/EpisodeProxy.cpp
View File

@ -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;
}

3
LibCarla/source/carla/client/detail/Simulator.cpp
View File

@ -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 <exception>
@ -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;
}

32
LibCarla/source/carla/client/detail/Simulator.h
View File

@ -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 <IP, port> of the Trafic Manager running on port.
/// If there is no Traffic Manager running the pair will be ("", 0)
std::pair<std::string, uint16_t> GetTrafficManagerRunning(uint16_t port) const {
return _client.GetTrafficManagerRunning(port);
}
/// Informs that a Traffic Manager is running on <IP, port>
bool AddTrafficManagerRunning(std::pair<std::string, uint16_t> trafficManagerInfo) const {
return _client.AddTrafficManagerRunning(trafficManagerInfo);
}
void DestroyTrafficManager(uint16_t port) const {
_client.DestroyTrafficManager(port);
}
void AddPendingException(std::string e) {
_episode->AddPendingException(e);
}
SharedPtr<BlueprintLibrary> GetBlueprintLibrary();
SharedPtr<Actor> GetSpectator();

20
LibCarla/source/carla/trafficmanager/AtomicActorSet.h
View File

@ -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<ActorId> GetIDList() {
std::lock_guard<std::mutex> lock(modification_mutex);
std::vector<ActorId> 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<ActorPtr> actor_list) {
std::lock_guard<std::mutex> lock(modification_mutex);
@ -76,8 +86,16 @@ namespace traffic_manager {
bool Contains(ActorId id) {
std::lock_guard<std::mutex> lock(modification_mutex);
return actor_set.find(id) != actor_set.end();
}
size_t Size() {
std::lock_guard<std::mutex> lock(modification_mutex);
return actor_set.size();
}
};
} // namespace traffic_manager

2
LibCarla/source/carla/trafficmanager/AtomicMap.h
View File

@ -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.

46
LibCarla/source/carla/trafficmanager/BatchControlStage.cpp
View File

@ -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 <https://opensource.org/licenses/MIT>.
#include "BatchControlStage.h"
#include "carla/trafficmanager/BatchControlStage.h"
namespace carla {
namespace traffic_manager {
@ -12,10 +12,12 @@ namespace traffic_manager {
BatchControlStage::BatchControlStage(
std::string stage_name,
std::shared_ptr<PlannerToControlMessenger> messenger,
cc::Client &carla_client)
carla::client::detail::EpisodeProxy &episode_proxy,
Parameters &parameters)
: 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;
@ -28,10 +30,10 @@ namespace traffic_manager {
// 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,7 +42,7 @@ 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);
}
}
@ -49,28 +51,44 @@ namespace traffic_manager {
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()) {
if (data_frame != nullptr && number_of_vehicles != (*data_frame.get()).size()) {
number_of_vehicles = static_cast<uint64_t>((*data_frame.get()).size());
// Allocating array for command batching.
commands = std::make_shared<std::vector<cr::Command>>(number_of_vehicles);
}
// Allocating array for command batching.
commands = std::make_shared<std::vector<carla::rpc::Command>>(number_of_vehicles);
}
}
void BatchControlStage::DataSender() {
messenger->Pop();
bool synch_mode = parameters.GetSynchronousMode();
if (commands != nullptr) {
carla_client.ApplyBatch(*commands.get());
// Run asynchronous mode commands.
episode_proxy_bcs.Lock()->ApplyBatch(*commands.get(), false);
}
// Limiting updates to 100 frames per second.
if (!synch_mode) {
std::this_thread::sleep_for(10ms);
} else {
std::unique_lock<std::mutex> 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

34
LibCarla/source/carla/trafficmanager/BatchControlStage.h
View File

@ -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,22 +6,26 @@
#pragma once
#include <atomic>
#include <chrono>
#include <memory>
#include <mutex>
#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 Motion Planner Stage class and communicates these signals to
@ -34,19 +38,28 @@ namespace traffic_manager {
std::shared_ptr<PlannerToControlFrame> data_frame;
/// Pointer to a messenger from Motion Planner.
std::shared_ptr<PlannerToControlMessenger> messenger;
/// Reference to carla client connection object.
cc::Client &carla_client;
/// Reference to CARLA client connection object.
carla::client::detail::EpisodeProxy episode_proxy_bcs;
/// Array to hold command batch.
std::shared_ptr<std::vector<cr::Command>> commands;
std::shared_ptr<std::vector<carla::rpc::Command>> commands;
/// Number of vehicles registered with the traffic manager.
uint64_t number_of_vehicles;
/// Parameter object for changing synchronous behaviour.
Parameters &parameters;
/// Step runner flag.
std::atomic<bool> 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:
BatchControlStage(
std::string stage_name,
BatchControlStage(std::string stage_name,
std::shared_ptr<PlannerToControlMessenger> messenger,
cc::Client &carla_client);
carla::client::detail::EpisodeProxy &episode_proxy,
Parameters &parameters);
~BatchControlStage();
void DataReceiver() override;
@ -55,6 +68,7 @@ namespace traffic_manager {
void DataSender() override;
bool RunStep();
};
} // namespace traffic_manager

109
LibCarla/source/carla/trafficmanager/CollisionStage.cpp
View File

@ -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;
@ -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<unsigned>(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,52 +76,45 @@ 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;
// Continue only if random number is lower than our %, default is 0.
if (parameters.GetPercentageIgnoreActors(boost::shared_ptr<cc::Actor>(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();
// Collision checks increase with speed (Official formula used)
try {
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();
// 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);
// Temporary fix to (0,0,0) bug
if (other_location.x != 0 && other_location.y != 0 && other_location.z != 0){
if (!(other_location.x == 0 && other_location.y == 0 && other_location.z == 0)) {
if (actor_id != ego_actor_id &&
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 (safe_point_junction != nullptr){
if(parameters.GetCollisionDetection(ego_actor, actor) &&
!IsLocationAfterJunctionSafe(ego_actor, actor, safe_point_junction)){
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");
}
}
}
CollisionToPlannerData &message = current_planner_frame->at(i);
@ -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<uint64_t>((*localization_frame.get()).size());
// Allocating output arrays to be shared with motion planner stage.
planner_frame_a = std::make_shared<CollisionToPlannerFrame>(number_of_vehicles);
planner_frame_b = std::make_shared<CollisionToPlannerFrame>(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<cc::Vehicle>(reference_vehicle);
const auto other_vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(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){

17
LibCarla/source/carla/trafficmanager/CollisionStage.h
View File

@ -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<SimpleWaypoint>;
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);

6
LibCarla/source/carla/trafficmanager/InMemoryMap.cpp
View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<int, int> 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) {

2
LibCarla/source/carla/trafficmanager/InMemoryMap.h
View File

@ -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.

163
LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
View File

@ -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 <https://opensource.org/licenses/MIT>.
#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 &registered_actors,
InMemoryMap &local_map,
Parameters &parameters,
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<BufferList>();
// Initializing maximum idle time to null.
maximum_idle_time = std::make_pair(nullptr, 0.0);
// Initializing srand.
srand(static_cast<unsigned>(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<cc::Vehicle>(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);
}
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,12 +295,54 @@ namespace LocalizationConstants {
}
void LocalizationStage::DataReceiver() {
bool is_deleted_actors_present = false;
std::set<uint32_t> world_actor_id;
std::vector<ActorPtr> 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<carla::client::detail::ActorVariant> 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();
@ -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<uint64_t>(actor_list.size());
// Allocating output frames to be shared with the motion planner stage.
planner_frame_a = std::make_shared<LocalizationToPlannerFrame>(number_of_vehicles);
planner_frame_b = std::make_shared<LocalizationToPlannerFrame>(number_of_vehicles);
@ -316,7 +366,6 @@ namespace LocalizationConstants {
traffic_light_frame_a = std::make_shared<LocalizationToTrafficLightFrame>(number_of_vehicles);
traffic_light_frame_b = std::make_shared<LocalizationToTrafficLightFrame>(number_of_vehicles);
}
}
void LocalizationStage::DataSender() {
@ -365,27 +414,44 @@ namespace LocalizationConstants {
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.*");
auto Filter = [&](auto &actors, auto &wildcard_pattern) {
std::vector<carla::client::detail::ActorVariant> 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_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.get()) {
const auto unregistered_id = actor->GetId();
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});
unregistered_actors.insert({unregistered_id, actor.Get(episode_proxy_ls)});
}
}
// Scanning for pedestrians.
for (auto walker: *world_walker.get()) {
const auto unregistered_id = walker->GetId();
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});
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<cc::Vehicle>(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<SimpleWaypointPtr> next_waypoints = waypoint_buffer.back()->GetNextWaypoint();
uint selection_index = 0u;
uint64_t selection_index = 0u;
if (next_waypoints.size() > 1) {
selection_index = static_cast<uint>(rand()) % next_waypoints.size();
selection_index = static_cast<uint64_t>(rand()) % next_waypoints.size();
}
waypoint_buffer.push_back(next_waypoints.at(selection_index));
@ -610,7 +675,7 @@ 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<SimpleWaypointPtr> 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<uint>(rand()) % next_waypoints.size();
selection_index = static_cast<uint64_t>(rand()) % next_waypoints.size();
}
waypoint_buffer.push_back(next_waypoints.at(selection_index));

19
LibCarla/source/carla/trafficmanager/LocalizationStage.h
View File

@ -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 <mutex>
#include <unordered_map>
#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 &parameters;
/// 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<BufferList> 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();
@ -147,8 +154,8 @@ namespace traffic_manager {
AtomicActorSet &registered_actors,
InMemoryMap &local_map,
Parameters &parameters,
cc::DebugHelper &debug_helper,
cc::World& world);
carla::client::DebugHelper &debug_helper,
carla::client::detail::EpisodeProxy &episodeProxy);
~LocalizationStage();

29
LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
View File

@ -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<float>::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<cc::Vehicle>(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<float>::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<GeoGridId>& 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,14 +104,16 @@ namespace LocalizationConstants {
void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer& buffer) {
// Add actor entry if not present.
if (!buffer.empty()) {
// 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<GeoGridId>& 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);
@ -119,11 +123,10 @@ namespace LocalizationConstants {
}
}
// Clear all grids current actor is tracking.
// 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()) {
uint64_t buffer_size = buffer.size();
uint64_t step_size = static_cast<uint64_t>(std::floor(buffer_size/BUFFER_STEP_THROUGH));
for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) {

6
LibCarla/source/carla/trafficmanager/LocalizationUtils.h
View File

@ -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

2
LibCarla/source/carla/trafficmanager/Messenger.h
View File

@ -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.

2
LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h
View File

@ -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.

11
LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp
View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<cc::Vehicle> 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);
}

7
LibCarla/source/carla/trafficmanager/MotionPlannerStage.h
View File

@ -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:

19
LibCarla/source/carla/trafficmanager/PIDController.cpp
View File

@ -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 <https://opensource.org/licenses/MIT>.
#include "PIDController.h"
#include "carla/trafficmanager/PIDController.h"
#include <algorithm>
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<float> 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<float> &longitudinal_parameters,
const std::vector<float> &lateral_parameters) const {
// Calculating dt for updating the integral component.
const chr::duration<float> 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 +

2
LibCarla/source/carla/trafficmanager/PIDController.h
View File

@ -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.

86
LibCarla/source/carla/trafficmanager/Parameters.cpp
View File

@ -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,16 +6,24 @@
#include <random>
#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<int, std::milli>(10);
}
Parameters::~Parameters() {}
void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
float new_percentage = std::min(100.0f, percentage);
percentage_difference_from_speed_limit.AddEntry({actor->GetId(), new_percentage});
}
@ -25,16 +33,11 @@ void Parameters::SetGlobalPercentageSpeedDifference(const float 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<AtomicActorSet> 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<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
if (!actor_set->Contains(other_id)) {
@ -77,6 +79,22 @@ void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float
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<double, std::milli>(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();
@ -150,11 +168,25 @@ void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float pe
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_ignore_actors.AddEntry(entry);
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_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;

65
LibCarla/source/carla/trafficmanager/Parameters.h
View File

@ -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 <atomic>
#include <chrono>
#include "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
#include "carla/Memory.h"
@ -45,21 +48,28 @@ namespace traffic_manager {
AtomicMap<ActorId, bool> auto_lane_change;
/// Map containing % of running a traffic light.
AtomicMap<ActorId, float> perc_run_traffic_light;
/// Map containing % of ignoring actors.
AtomicMap<ActorId, float> perc_ignore_actors;
/// Map containing % of running a traffic sign.
AtomicMap<ActorId, float> perc_run_traffic_sign;
/// Map containing % of ignoring walkers.
AtomicMap<ActorId, float> perc_ignore_walkers;
/// Map containing % of ignoring vehicles.
AtomicMap<ActorId, float> perc_ignore_vehicles;
/// Synchronous mode switch.
std::atomic<bool> 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,
@ -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<double, std::milli> synchronous_time_out;
};

2
LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp
View File

@ -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.

2
LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h
View File

@ -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.

21
LibCarla/source/carla/trafficmanager/PipelineStage.cpp
View File

@ -1,32 +1,41 @@
// 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 <https://opensource.org/licenses/MIT>.
#include "PipelineStage.h"
#include "carla/trafficmanager/PipelineStage.h"
namespace carla {
namespace traffic_manager {
PipelineStage::PipelineStage(std::string stage_name)
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();
worker_thread.release();
Stop();
}
void PipelineStage::Start() {
run_stage.store(true);
if(worker_thread) {
Stop();
}
worker_thread = std::make_unique<std::thread>(&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::Update() {
@ -34,14 +43,12 @@ namespace traffic_manager {
// 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();
}

7
LibCarla/source/carla/trafficmanager/PipelineStage.h
View File

@ -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<std::thread> worker_thread;
protected:
/// Flag to start/stop stage.
std::atomic<bool> 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();

4
LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp
View File

@ -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 <https://opensource.org/licenses/MIT>.
#include "SimpleWaypoint.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {

2
LibCarla/source/carla/trafficmanager/SimpleWaypoint.h
View File

@ -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.

58
LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp
View File

@ -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 <iostream>
#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<unsigned>(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<cc::Vehicle>(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<cc::Actor>(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<cc::Actor>(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<cc::Actor>(ego_actor)) <= (rand() % 101)) {
std::lock_guard<std::mutex> 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<uint64_t>((*localization_frame.get()).size());
// Allocating output frames.
planner_frame_a = std::make_shared<TrafficLightToPlannerFrame>(number_of_vehicles);
planner_frame_b = std::make_shared<TrafficLightToPlannerFrame>(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);
}

2
LibCarla/source/carla/trafficmanager/TrafficLightStage.h
View File

@ -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.

360
LibCarla/source/carla/trafficmanager/TrafficManager.cpp
View File

@ -1,246 +1,212 @@
// 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 <https://opensource.org/licenses/MIT>.
#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 {
std::map<uint16_t, std::unique_ptr<TrafficManagerBase>> TrafficManager::_tm_map;
std::mutex TrafficManager::_mutex;
TrafficManager::TrafficManager(
std::vector<float> longitudinal_PID_parameters,
std::vector<float> longitudinal_highway_PID_parameters,
std::vector<float> lateral_PID_parameters,
std::vector<float> 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()) {
carla::client::detail::EpisodeProxy episode_proxy,
uint16_t port)
: _port(port) {
const WorldMap world_map = world.GetMap();
local_map = std::make_shared<traffic_manager::InMemoryMap>(world_map);
local_map->SetUp();
parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit);
localization_collision_messenger = std::make_shared<LocalizationToCollisionMessenger>();
localization_traffic_light_messenger = std::make_shared<LocalizationToTrafficLightMessenger>();
collision_planner_messenger = std::make_shared<CollisionToPlannerMessenger>();
localization_planner_messenger = std::make_shared<LocalizationToPlannerMessenger>();
traffic_light_planner_messenger = std::make_shared<TrafficLightToPlannerMessenger>();
planner_control_messenger = std::make_shared<PlannerToControlMessenger>();
localization_stage = std::make_unique<LocalizationStage>(
"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<CollisionStage>(
"Collision stage",
localization_collision_messenger, collision_planner_messenger,
parameters, debug_helper);
traffic_light_stage = std::make_unique<TrafficLightStage>(
"Traffic light stage",
localization_traffic_light_messenger, traffic_light_planner_messenger,
parameters, debug_helper);
planner_stage = std::make_unique<MotionPlannerStage>(
"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<BatchControlStage>(
"Batch control stage",
planner_control_messenger, client_connection);
Start();
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);
}
}
}
TrafficManager::~TrafficManager() {
Stop();
void TrafficManager::Release() {
std::lock_guard<std::mutex> lock(_mutex);
for(auto& tm : _tm_map) {
tm.second->Release();
TrafficManagerBase *base_ptr = tm.second.release();
delete base_ptr;
}
_tm_map.clear();
}
std::unique_ptr<TrafficManager> TrafficManager::singleton_pointer = nullptr;
void TrafficManager::Reset() {
std::lock_guard<std::mutex> lock(_mutex);
for(auto& tm : _tm_map) {
tm.second->Reset();
}
}
TrafficManager& TrafficManager::GetInstance(cc::Client &client_connection) {
void TrafficManager::Tick() {
std::lock_guard<std::mutex> lock(_mutex);
for(auto& tm : _tm_map) {
tm.second->SynchronousTick();
}
}
if (singleton_pointer == nullptr) {
void TrafficManager::CreateTrafficManagerServer(
carla::client::detail::EpisodeProxy episode_proxy,
uint16_t port) {
// Get local IP details.
auto GetLocalIP = [=](const uint16_t sport)-> std::pair<std::string, uint16_t> {
std::pair<std::string, uint16_t> 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<sockaddr*>(&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<struct sockaddr*> (&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, uint16_t>(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;
};
/// Define local constants
const std::vector<float> longitudinal_param = {2.0f, 0.05f, 0.07f};
const std::vector<float> longitudinal_highway_param = {4.0f, 0.02f, 0.03f};
const std::vector<float> lateral_param = {10.0f, 0.02f, 1.0f};
const std::vector<float> lateral_highway_param = {9.0f, 0.02f, 1.0f};
const std::vector<float> lateral_param = {9.0f, 0.02f, 1.0f};
const std::vector<float> lateral_highway_param = {7.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
);
std::pair<std::string, uint16_t> serverTM;
singleton_pointer = std::unique_ptr<TrafficManager>(tm_ptr);
}
/// 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);
return *singleton_pointer.get();
}
/// Get TM server info (Local IP & PORT)
serverTM = GetLocalIP(port);
std::unique_ptr<cc::Client> TrafficManager::singleton_local_client = nullptr;
/// Set this client as the TM to server
episode_proxy.Lock()->AddTrafficManagerRunning(serverTM);
cc::Client& TrafficManager::GetUniqueLocalClient() {
#if DEBUG_PRINT_TM
/// Print status
std::cout << "NEW@: Registered TM at "
<< serverTM.first << ":"
<< serverTM.second << " ..... SUCCESS."
<< std::endl;
#endif
if (singleton_local_client == nullptr) {
cc::Client* client = new cc::Client("localhost", 2000);
singleton_local_client = std::unique_ptr<cc::Client>(client);
}
return *singleton_local_client.get();
}
void TrafficManager::RegisterVehicles(const std::vector<ActorPtr> &actor_list) {
registered_actors.Insert(actor_list);
}
void TrafficManager::UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
registered_actors.Remove(actor_list);
}
void TrafficManager::Start() {
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();
/// Set the pointer of the instance
_tm_map.insert(std::make_pair(port, std::unique_ptr<TrafficManagerBase>(tm_ptr)));
}
void TrafficManager::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
parameters.SetPercentageSpeedDifference(actor, percentage);
}
bool TrafficManager::CreateTrafficManagerClient(
carla::client::detail::EpisodeProxy episode_proxy,
uint16_t port) {
void TrafficManager::SetGlobalPercentageSpeedDifference(const float percentage) {
parameters.SetGlobalPercentageSpeedDifference(percentage);
}
bool result = false;
void TrafficManager::SetCollisionDetection(
const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision) {
if(episode_proxy.Lock()->IsTrafficManagerRunning(port)) {
parameters.SetCollisionDetection(reference_actor, other_actor, detect_collision);
}
/// Get TM server info (Remote IP & PORT)
std::pair<std::string, uint16_t> serverTM =
episode_proxy.Lock()->GetTrafficManagerRunning(port);
void TrafficManager::SetForceLaneChange(const ActorPtr &actor, const bool direction) {
/// Set remote TM server IP and port
TrafficManagerRemote* tm_ptr = new(std::nothrow)
TrafficManagerRemote(serverTM, episode_proxy);
parameters.SetForceLaneChange(actor, direction);
}
/// Try to connect to remote TM server
try {
void TrafficManager::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
/// Check memory allocated or not
if(tm_ptr != nullptr) {
parameters.SetAutoLaneChange(actor, enable);
}
#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();
void TrafficManager::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
/// Set the pointer of the instance
_tm_map.insert(std::make_pair(port, std::unique_ptr<TrafficManagerBase>(tm_ptr)));
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<TLGroup> list_of_all_groups;
TLGroup tl_to_freeze;
std::vector<carla::ActorId> 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<cc::TrafficLight>(tl)->GetGroupTrafficLights();
list_of_all_groups.push_back(tl_group);
for (uint64_t i=0u; i<tl_group.size(); i++) {
list_of_ids.push_back(tl_group.at(i).get()->GetId());
if(i!=0u) {
tl_to_freeze.push_back(tl_group.at(i));
}
}
result = true;
}
}
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);});
/// 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
}
while (!CheckAllFrozen(tl_to_freeze)) {
for (auto& tln : tl_to_freeze) {
tln->SetState(TLS::Red);
tln->Freeze(true);
}
}
return result;
}
} // namespace traffic_manager

256
LibCarla/source/carla/trafficmanager/TrafficManager.h
View File

@ -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 <algorithm>
#include <memory>
#include <mutex>
#include <random>
#include <unordered_set>
#include <vector>
@ -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,125 +34,201 @@
#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<carla::client::Actor>;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using TLS = carla::rpc::TrafficLightState;
using TLGroup = std::vector<carla::SharedPtr<cc::TrafficLight>>;
/// The function of this class is to integrate all the various stages of
/// This class integrates all the various stages of
/// the traffic manager appropriately using messengers.
class TrafficManager {
private:
/// PID controller parameters.
std::vector<float> longitudinal_PID_parameters;
std::vector<float> longitudinal_highway_PID_parameters;
std::vector<float> lateral_PID_parameters;
std::vector<float> lateral_highway_PID_parameters;
/// Set of all actors registered with traffic manager.
AtomicActorSet registered_actors;
/// Pointer to local map cache.
std::shared_ptr<InMemoryMap> 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<CollisionToPlannerMessenger> collision_planner_messenger;
std::shared_ptr<LocalizationToCollisionMessenger> localization_collision_messenger;
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_traffic_light_messenger;
std::shared_ptr<LocalizationToPlannerMessenger> localization_planner_messenger;
std::shared_ptr<PlannerToControlMessenger> planner_control_messenger;
std::shared_ptr<TrafficLightToPlannerMessenger> traffic_light_planner_messenger;
/// Pointers to the stage objects of traffic manager.
std::unique_ptr<CollisionStage> collision_stage;
std::unique_ptr<BatchControlStage> control_stage;
std::unique_ptr<LocalizationStage> localization_stage;
std::unique_ptr<MotionPlannerStage> planner_stage;
std::unique_ptr<TrafficLightStage> traffic_light_stage;
/// Static pointer to singleton object.
static std::unique_ptr<TrafficManager> singleton_pointer;
/// Static pointer to singleton client connected to localhost, 2000.
static std::unique_ptr<cc::Client> singleton_local_client;
/// Parameterization object.
Parameters parameters;
/// Private constructor for singleton lifecycle management.
TrafficManager(
std::vector<float> longitudinal_PID_parameters,
std::vector<float> longitudinal_highway_PID_parameters,
std::vector<float> lateral_PID_parameters,
std::vector<float> lateral_highway_PID_parameters,
float perc_decrease_from_limit,
cc::Client &client_connection);
/// To start the TrafficManager.
void Start();
/// To stop the TrafficManager.
void Stop();
public:
/// Public constructor for singleton life cycle management.
explicit TrafficManager(
carla::client::detail::EpisodeProxy episode_proxy,
uint16_t port = TM_DEFAULT_PORT);
/// Static method for singleton lifecycle management.
static TrafficManager& GetInstance(cc::Client &client_connection);
TrafficManager(const TrafficManager& other) {
_port = other._port;
}
/// Static method to get unique client connected to (localhost, 2000).
static cc::Client& GetUniqueLocalClient();
TrafficManager(TrafficManager &&) = default;
TrafficManager &operator=(const TrafficManager &) = default;
TrafficManager &operator=(TrafficManager &&) = default;
static void Release();
static void Reset();
static void Tick();
/// This method registers a vehicle with the traffic manager.
void RegisterVehicles(const std::vector<ActorPtr> &actor_list);
void RegisterVehicles(const std::vector<ActorPtr> &actor_list) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->RegisterVehicles(actor_list);
}
}
/// This method unregisters a vehicle from traffic manager.
void UnregisterVehicles(const std::vector<ActorPtr> &actor_list);
void UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->UnregisterVehicles(actor_list);
}
}
/// Set target velocity specific to a vehicle.
void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage);
/// 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);
}
}
/// Set global target velocity.
void SetGlobalPercentageSpeedDifference(float const percentage);
/// 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 collision detection rules between vehicles.
void SetCollisionDetection(
const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision);
/// 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);
}
}
/// 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);
void SetForceLaneChange(const ActorPtr &actor, const bool direction) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->SetForceLaneChange(actor, direction);
}
}
/// Enable/disable automatic lane change on a vehicle.
void SetAutoLaneChange(const ActorPtr &actor, const bool enable);
void SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->SetAutoLaneChange(actor, enable);
}
}
/// Method to specify how much distance a vehicle should maintain to
/// the leading vehicle.
void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance);
void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->SetDistanceToLeadingVehicle(actor, distance);
}
}
/// 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 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 the % chance of running a red light
void SetPercentageRunningLight(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) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->SetPercentageIgnoreVehicles(actor, perc);
}
}
/// Method to check if traffic lights are frozen.
bool CheckAllFrozen(TLGroup tl_to_freeze);
/// 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 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 reset all traffic lights.
void ResetAllTrafficLights();
void ResetAllTrafficLights() {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->ResetAllTrafficLights();
}
}
/// Return the world object
const cc::World &GetWorld() { return world; };
/// 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);
}
}
/// Destructor.
~TrafficManager();
/// 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);
}
}
/// 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<std::mutex> lock(_mutex);
auto it = _tm_map.find(port);
if (it != _tm_map.end()) {
_mutex.unlock();
return it->second.get();
}
return nullptr;
}
static std::map<uint16_t, std::unique_ptr<TrafficManagerBase>> _tm_map;
static std::mutex _mutex;
uint16_t _port = TM_DEFAULT_PORT;
};

105
LibCarla/source/carla/trafficmanager/TrafficManagerBase.h Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#pragma once
#include <memory>
#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<carla::client::Actor>;
/// 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<ActorPtr> &actor_list) = 0;
/// This method unregisters a vehicle from traffic manager.
virtual void UnregisterVehicles(const std::vector<ActorPtr> &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

180
LibCarla/source/carla/trafficmanager/TrafficManagerClient.h Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#pragma once
#include "carla/client/Actor.h"
#include <rpc/client.h>
#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<carla::rpc::Actor> &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<carla::rpc::Actor> &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<bool>();
}
/// 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

277
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<float> longitudinal_PID_parameters,
std::vector<float> longitudinal_highway_PID_parameters,
std::vector<float> lateral_PID_parameters,
std::vector<float> 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<carla::traffic_manager::TrafficManagerBase *>(this))) {
parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit);
Start();
}
TrafficManagerLocal::~TrafficManagerLocal() {
episodeProxyTM.Lock()->DestroyTrafficManager(server.port());
Release();
}
void TrafficManagerLocal::Start() {
const carla::SharedPtr<cc::Map> world_map = episodeProxyTM.Lock()->GetCurrentMap();
local_map = std::make_shared<traffic_manager::InMemoryMap>(world_map);
local_map->SetUp();
localization_collision_messenger = std::make_shared<LocalizationToCollisionMessenger>();
localization_traffic_light_messenger = std::make_shared<LocalizationToTrafficLightMessenger>();
collision_planner_messenger = std::make_shared<CollisionToPlannerMessenger>();
localization_planner_messenger = std::make_shared<LocalizationToPlannerMessenger>();
traffic_light_planner_messenger = std::make_shared<TrafficLightToPlannerMessenger>();
planner_control_messenger = std::make_shared<PlannerToControlMessenger>();
localization_stage = std::make_unique<LocalizationStage>(
"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<CollisionStage>(
"Collision stage",
localization_collision_messenger, collision_planner_messenger,
parameters, debug_helper);
traffic_light_stage = std::make_unique<TrafficLightStage>(
"Traffic light stage",
localization_traffic_light_messenger, traffic_light_planner_messenger,
parameters, debug_helper);
planner_stage = std::make_unique<MotionPlannerStage>(
"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<BatchControlStage>(
"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<ActorPtr>& actor_list) {
registered_actors.Insert(actor_list);
}
void TrafficManagerLocal::UnregisterVehicles(const std::vector<ActorPtr>& 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<carla::client::detail::ActorVariant> 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<TLGroup> list_of_all_groups;
TLGroup tl_to_freeze;
std::vector<carla::ActorId> 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<cc::TrafficLight>(tl.Get(episodeProxyTM))->GetGroupTrafficLights();
list_of_all_groups.push_back(tl_group);
for (uint64_t i=0u; i<tl_group.size(); i++) {
list_of_ids.push_back(tl_group.at(i).get()->GetId());
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<ActorId> TrafficManagerLocal::GetRegisteredVehiclesIDs() {
return registered_actors.GetIDList();
}
} // namespace traffic_manager
} // namespace carla

183
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#pragma once
#include <algorithm>
#include <memory>
#include <random>
#include <unordered_set>
#include <vector>
#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<carla::client::Actor>;
using TLS = carla::rpc::TrafficLightState;
using TLGroup = std::vector<carla::SharedPtr<carla::client::TrafficLight>>;
/// 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<float> longitudinal_PID_parameters;
std::vector<float> longitudinal_highway_PID_parameters;
std::vector<float> lateral_PID_parameters;
std::vector<float> lateral_highway_PID_parameters;
/// Set of all actors registered with traffic manager.
AtomicActorSet registered_actors;
/// Pointer to local map cache.
std::shared_ptr<InMemoryMap> 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<CollisionToPlannerMessenger> collision_planner_messenger;
std::shared_ptr<LocalizationToCollisionMessenger> localization_collision_messenger;
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_traffic_light_messenger;
std::shared_ptr<LocalizationToPlannerMessenger> localization_planner_messenger;
std::shared_ptr<PlannerToControlMessenger> planner_control_messenger;
std::shared_ptr<TrafficLightToPlannerMessenger> traffic_light_planner_messenger;
/// Pointers to the stage objects of traffic manager.
std::unique_ptr<CollisionStage> collision_stage;
std::unique_ptr<BatchControlStage> control_stage;
std::unique_ptr<LocalizationStage> localization_stage;
std::unique_ptr<MotionPlannerStage> planner_stage;
std::unique_ptr<TrafficLightStage> 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<float> longitudinal_PID_parameters,
std::vector<float> longitudinal_highway_PID_parameters,
std::vector<float> lateral_PID_parameters,
std::vector<float> 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<ActorPtr> &actor_list);
/// This method unregisters a vehicle from traffic manager.
void UnregisterVehicles(const std::vector<ActorPtr> &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<ActorId> GetRegisteredVehiclesIDs();
};
} // namespace traffic_manager
} // namespace carla

193
LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#include <carla/client/Client.h>
#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<std::string, uint16_t> &_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<std::mutex> 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<ActorPtr> &_actor_list) {
std::vector<carla::rpc::Actor> actor_list;
for (auto &&actor : _actor_list) {
actor_list.emplace_back(actor->Serialize());
}
client.RegisterVehicle(actor_list);
}
void TrafficManagerRemote::UnregisterVehicles(const std::vector<ActorPtr> &_actor_list) {
std::vector<carla::rpc::Actor> 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

126
LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#pragma once
#include <condition_variable>
#include <memory>
#include <mutex>
#include <vector>
#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<carla::client::Actor>;
using TLS = carla::rpc::TrafficLightState;
using TLGroup = std::vector<carla::SharedPtr<carla::client::TrafficLight>>;
/// 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<std::string, uint16_t> &_serverTM, carla::client::detail::EpisodeProxy &episodeProxy);
/// Destructor.
virtual ~TrafficManagerRemote();
/// This method registers a vehicle with the traffic manager.
void RegisterVehicles(const std::vector<ActorPtr> &actor_list);
/// This method unregisters a vehicle from traffic manager.
void UnregisterVehicles(const std::vector<ActorPtr> &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

195
LibCarla/source/carla/trafficmanager/TrafficManagerServer.h Normal file
View File

@ -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 <https://opensource.org/licenses/MIT>.
#pragma once
#include <vector>
#include <memory>
#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<carla::client::Actor>;
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 <carla::rpc::Actor> _actor_list) {
std::vector<ActorPtr> 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 <carla::rpc::Actor> _actor_list) {
std::vector<ActorPtr> 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

2
PythonAPI/carla/setup.py
View File

@ -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

84
PythonAPI/carla/source/libcarla/Client.cpp
View File

@ -44,26 +44,40 @@ static auto ApplyBatchCommandsSync(
using CommandType = carla::rpc::Command;
std::vector<CommandType> cmds {
boost::python::stl_input_iterator<CommandType>(commands),
boost::python::stl_input_iterator<CommandType>()};
boost::python::stl_input_iterator<CommandType>()
};
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<carla::traffic_manager::ActorPtr> vehicles_to_enable;
std::vector<carla::traffic_manager::ActorPtr> vehicles_to_disable;
for (size_t i=0; i<cmds.size(); ++i) {
std::vector<carla::traffic_manager::ActorPtr> vehicles_to_enable(cmds.size(), nullptr);
std::vector<carla::traffic_manager::ActorPtr> vehicles_to_disable(cmds.size(), nullptr);
carla::client::World world = self.GetWorld();
std::atomic<size_t> vehicles_to_enable_index;
std::atomic<size_t> vehicles_to_disable_index;
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 (cmds[i].command.type() == typeid(carla::rpc::Command::SpawnActor)) {
if (cmd_type_info == typeid(carla::rpc::Command::SpawnActor)) {
// check inside 'do_after'
auto &spawn = boost::get<carla::rpc::Command::SpawnActor>(cmds[i].command);
auto &spawn = boost::get<carla::rpc::Command::SpawnActor>(cmd_type);
for (auto &cmd : spawn.do_after) {
if (cmd.command.type() == typeid(carla::rpc::Command::SetAutopilot)) {
autopilotValue = boost::get<carla::rpc::Command::SetAutopilot>(cmd.command).enabled;
@ -71,10 +85,9 @@ static auto ApplyBatchCommandsSync(
}
}
}
// check SetAutopilot command
if (cmds[i].command.type() == typeid(carla::rpc::Command::SetAutopilot)) {
autopilotValue = boost::get<carla::rpc::Command::SetAutopilot>(cmds[i].command).enabled;
else if (cmd_type_info == typeid(carla::rpc::Command::SetAutopilot)) {
autopilotValue = boost::get<carla::rpc::Command::SetAutopilot>(cmd_type).enabled;
isAutopilot = true;
}
@ -82,31 +95,55 @@ static auto ApplyBatchCommandsSync(
if (isAutopilot) {
// get the id
carla::rpc::ActorId id = static_cast<carla::rpc::ActorId>(responses[i].Get());
// get traffic manager instance
if (!tm) {
tm = &carla::traffic_manager::TrafficManager::GetInstance(
carla::traffic_manager::TrafficManager::GetUniqueLocalClient());
}
// get all actors
carla::SharedPtr<carla::client::Actor> actor;
if (tm) {
actor = tm->GetWorld().GetActor(id);
}
actor = world.GetActor(id);
// check to enable or disable
if (actor) {
if (autopilotValue) {
vehicles_to_enable.push_back(actor);
size_t index = vehicles_to_enable_index.fetch_add(1);
vehicles_to_enable[index] = actor;
} else {
vehicles_to_disable.push_back(actor);
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<std::thread*> 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))
;
}

29
PythonAPI/carla/source/libcarla/TrafficManager.cpp
View File

@ -15,26 +15,17 @@
void export_trafficmanager() {
namespace cc = carla::client;
using namespace boost::python;
using ActorList = std::vector<carla::SharedPtr<cc::Actor>>;
using Parameters = std::vector<float>;
class_<ActorList>("TM_ActorList").def(vector_indexing_suite<ActorList>());
class_<Parameters>("TM_Parameters").def(vector_indexing_suite<Parameters>());
class_<carla::traffic_manager::TrafficManager, boost::noncopyable>("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_<carla::traffic_manager::TrafficManager>("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<reference_existing_object>());
.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);
}

5
PythonAPI/examples/manual_control.py
View File

@ -306,6 +306,11 @@ class KeyboardControl(object):
if self._is_quit_shortcut(event.key):
return True
elif event.key == K_BACKSPACE:
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()

45
PythonAPI/examples/spawn_npc.py
View File

@ -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
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:
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])

223
PythonAPI/examples/tm_spawn_npc.py
View File

@ -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 <https://opensource.org/licenses/MIT>.
"""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')

11
PythonAPI/test/smoke/test_snapshot.py
View File

@ -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]

305
PythonAPI/util/performance_benchmark.py
View File

@ -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 = []
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'}]
@ -104,6 +114,27 @@ def define_sensors():
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):
def __init__(self):
@ -120,30 +151,28 @@ class CallBack(object):
return self._current_fps
def create_ego_vehicle(world, ego_vehicle, spawn_point, list_sensor_spec):
def create_environment(world, sensors, n_vehicles, n_walkers, spawn_points, client):
global sensors_callback
if ego_vehicle:
ego_vehicle.set_transform(spawn_point)
sensors = None
else:
sensors = []
sensors_ret = []
blueprint_library = world.get_blueprint_library()
blueprint = blueprint_library.filter('vehicle.lincoln.mkz2017')[0]
ego_vehicle = world.try_spawn_actor(blueprint, spawn_point)
# setup sensors
for sensor_spec in list_sensor_spec:
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_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')
@ -151,55 +180,155 @@ def create_ego_vehicle(world, ego_vehicle, spawn_point, list_sensor_spec):
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_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, ego_vehicle)
sensor = world.spawn_actor(bp, sensor_transform)
# add callbacks
sc = CallBack()
sensor.listen(sc)
sensors_callback.append(sc)
sensors.append(sensor)
sensors_ret.append(sensor)
return ego_vehicle, sensors
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:
vehicles_list.append(response.actor_id)
# -------------
# 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)
# 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.' % (len(vehicles_list), len(walkers_list)))
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):
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))
ego_vehicle = None
n = min(n_vehicles, len(spawn_points))
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)
if sensors_ret:
sensor_list = sensors_ret
ticks = 0
while ticks < number_ticks:
_ = world.wait_for_tick(1000.0)
while ticks < args.ticks:
_ = world.wait_for_tick()
if debug:
print("== Samples {} / {}".format(ticks + 1, number_ticks))
print("== Samples {} / {}".format(ticks + 1, args.ticks))
min_fps = float('inf')
for sc in sensors_callback:
@ -216,7 +345,16 @@ def run_benchmark(world, sensor_specs_list, number_locations, number_ticks, acto
sensor.stop()
sensor.destroy()
sensors_callback.clear()
ego_vehicle.destroy()
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
@ -232,22 +370,25 @@ 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"
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['sensors'],
record['town'],
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)
s = "| | | | | **{:03.2f}** | **{:03.2f}** |\n".format(*get_total(records))
s = "\n| Global mean FPS | Global std FPS |\n"
s += "| **{:03.2f}** | **{:03.2f}** |\n".format(*get_total(records))
fd.write(s)
s = "Table: {}.\n".format(system_specs)
@ -268,20 +409,12 @@ def get_system_specs():
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:
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
str_system += "GPU "
gpu_info = GPUtil.getGPUs()
for gpu in gpu_info:
str_system += "{} ".format(gpu.name)
return str_system
@ -292,33 +425,47 @@ def main(args):
pygame.init()
records = {}
for town in sorted(client.get_available_maps()):
maps = [m.replace('/Game/Carla/Maps/', '') for m in client.get_available_maps()]
for town in sorted(maps):
world = client.load_world(town)
# set to async mode
settings = world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
# 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 weather in weathers():
world.set_weather(weather)
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, number_locations, number_ticks, actor_list)
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'] + " ")
record = {'sensors': sensor_str,
'weather': weather,
record = {
'town': town,
'samples': number_locations * number_ticks,
'sensors': sensor_str,
'weather': weather["name"],
'n_vehicles': env["vehicles"],
'n_walkers': env["walkers"],
'samples': args.ticks,
'fps_mean': mean,
'fps_std': std}
'fps_std': std
}
if sensor_str not in records:
records[sensor_str] = []
records[sensor_str].append(record)
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()
@ -327,12 +474,16 @@ def main(args):
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")
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)

45
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp
View File

@ -41,6 +41,7 @@
#include <compiler/enable-ue4-macros.h>
#include <vector>
#include <map>
template <typename T>
using R = carla::rpc::Response<T>;
@ -71,6 +72,9 @@ public:
BindActions();
}
/// Map of pairs < port , ip > with all the Traffic Managers active in the simulation
std::map<uint16_t, std::string> 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<bool>
{
return (TrafficManagerInfo.find(port) != TrafficManagerInfo.end());
};
/// Gets a pair filled with the <IP, port> 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<std::pair<std::string, uint16_t>>
{
auto it = TrafficManagerInfo.find(port);
if(it != TrafficManagerInfo.end()) {
return std::pair<std::string, uint16_t>(it->second, it->first);
}
return std::pair<std::string, uint16_t>("",0);
};
/// Add a new Traffic Manager running on <IP, port>
BIND_SYNC(add_traffic_manager_running) << [this] (std::pair<std::string, uint16_t> trafficManagerInfo) ->R<bool>
{
uint16_t port = trafficManagerInfo.second;
auto it = TrafficManagerInfo.find(port);
if(it == TrafficManagerInfo.end()) {
TrafficManagerInfo.insert(
std::pair<uint16_t, std::string>(port, trafficManagerInfo.first));
return true;
}
return false;
};
BIND_SYNC(destroy_traffic_manager) << [this] (uint16_t port) ->R<bool>
{
auto it = TrafficManagerInfo.find(port);
if(it != TrafficManagerInfo.end()) {
TrafficManagerInfo.erase(it);
return true;
}
return false;
};
BIND_ASYNC(version) << [] () -> R<std::string>
{
return carla::version();

3
Util/BuildTools/Setup.bat
View File

@ -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)