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

Traffic manager 2.0 (#2833) 

* Traffic Manager 2.0

* WIP: new class structure for TM 2.0
Yet to use the new classes in TrafficManagerLocal

* WIP: new class structure integration

* Fix for python api build.

* Fix for hybrid mode crash.

* Fixed incorrect collision cache.
Fixed crash upon map change.
Minor reformatting.

* Fixed collisions with unregistered actors.
Fixed collisions among multiple traffic manager instances.
Fixed vehicle destruction upon being stuck for too long.

* Fixed vehicle wobble on steep roads

* Waypoint buffer extension at junction entrance.

* WIP: Revamped intersection anticipation.

* Implemented waypoint occupancy tracking.

* * considering buffer independent waypoints
for determining blocked junction exit.

* considering longitudinal extreme points
for localizing unregistered actors.

* Removed debug statements

* Intersection anticipation for vehicles moving across path.

* Fixed intersection anticipation in Town03, Town04.

* Safe interval length check for intersection anticipation.

* Distance check from safe interval for blocked exit.

* Removed debug statements

* * Intersection anticipation exception for roundabout in Town03.
* Updated comments.

* codacity fix

* more codacity fixes

* formatting and minor changes

* bad indentation

* Removed debug statements and updated comments.

* fixes unused variable error in make examples

* Increased minimum obstacle distance for lane change
to avoid partial lane changes.

* Removing crude stopping logic at junction entrance
for blocked junction case.

* Ignoring small intersection segments
for intersection anticipation.

* Fixed issue in unblocking mechanism
due to incorrect clock initialization.

* Fixing intersection entrance identification.

* Fixed incorrect safe space after junction detection.

* Fixed collision negotiation conditions
to work well inside intersections
and avoid deadlocks in roundabout turns.

* Replaced in file constants of InMemoryMap
with definitions from Constants.h

Updated comments

* Added a check to avoid collision considerations
in case  of traffic light hazard in motion planner.

* Re-organised include statements for ALSM.h/.cpp files.

Fixed a double initialization precision.

* WIP: Refactoring ALSM::Update

* Refactored ALSM::Update into shorter functions.

* ALSM: Corrected numeric comparision precision
and container access optimizations.

* Initial review changes

* CollisionStage: addressed pr comments.

* Constants.h, DataStructures.h: addressed pr review comments.

* InMemorMap: review comment addressal.

* LocalizationStage: Review comment addressal

* New class for random generation instead of rand()

* Removed unused code

* MotionPlanStage: re-organized include statements.

* MotionPlanStage: review comment addressal.

* SimulationState, DataStructures: minor refactoring.

* SnippetProfiler, TrackTraffic: Review comment addressal.

* Refactored include statements for
SimpleWaypoint, SnippetProfiler, TrackTraffic files.

* TrafficLightStage: review comment addressal.

* Using sleep instead of continue to time hybrid mode.

* Changing fixed array allocation with dynamic resizing.

* Refactored include statements for TrafficManager

* Removed clamp macro

* Added const to Networking constants

Co-authored-by: Jacopo Bartiromo <jackbart94@gmail.com>
Co-authored-by: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com>
This commit is contained in:
Praveen Kumar 2020-06-05 03:32:10 +05:30 committed by GitHub
parent 66166399bb
commit 1e98335808
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
53 changed files with 3372 additions and 4112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,6 +13,8 @@
#include "carla/rpc/VehiclePhysicsControl.h"
#include "carla/trafficmanager/TrafficManager.h"
using carla::traffic_manager::constants::Networking::TM_DEFAULT_PORT;
namespace carla {
namespace traffic_manager {

357
LibCarla/source/carla/trafficmanager/ALSM.cpp Normal file
View File

@ -0,0 +1,357 @@
#include "boost/pointer_cast.hpp"
#include "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
#include "carla/client/Walker.h"
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/LocalizationUtils.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
#include "carla/trafficmanager/ALSM.h"
namespace carla {
namespace traffic_manager {
ALSM::ALSM(
AtomicActorSet &registered_vehicles,
BufferMap &buffer_map,
TrackTraffic &track_traffic,
const Parameters &parameters,
const cc::World &world,
const LocalMapPtr &local_map,
SimulationState &simulation_state,
LocalizationStage &localization_stage,
CollisionStage &collision_stage,
TrafficLightStage &traffic_light_stage,
MotionPlanStage &motion_plan_stage)
: registered_vehicles(registered_vehicles),
buffer_map(buffer_map),
track_traffic(track_traffic),
parameters(parameters),
world(world),
local_map(local_map),
simulation_state(simulation_state),
localization_stage(localization_stage),
collision_stage(collision_stage),
traffic_light_stage(traffic_light_stage),
motion_plan_stage(motion_plan_stage) {}
void ALSM::Update() {
bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
std::set<ActorId> world_vehicle_ids;
std::set<ActorId> world_pedestrian_ids;
std::vector<ActorId> unregistered_list_to_be_deleted;
current_timestamp = world.GetSnapshot().GetTimestamp();
ActorList world_actors = world.GetActors();
// Find destroyed actors and perform clean up.
const ALSM::DestroyeddActors destroyed_actors = IdentifyDestroyedActors(world_actors);
const ActorIdSet &destroyed_registered = destroyed_actors.first;
for (const auto &deletion_id: destroyed_registered) {
RemoveActor(deletion_id, true);
}
const ActorIdSet &destroyed_unregistered = destroyed_actors.second;
for (auto deletion_id : destroyed_unregistered) {
RemoveActor(deletion_id, false);
}
// Invalidate hero actor if it is not alive anymore.
if (hybrid_physics_mode && hero_actors.size() != 0u) {
ActorIdSet hero_actors_to_delete;
for (auto &hero_actor_info: hero_actors) {
if(destroyed_unregistered.find(hero_actor_info.first) != destroyed_unregistered.end()) {
hero_actors_to_delete.insert(hero_actor_info.first);
}
}
for (auto &deletion_id: hero_actors_to_delete) {
hero_actors.erase(deletion_id);
}
}
// Scan for new unregistered actors.
ALSM::ActorVector new_actors = IdentifyNewActors(world_actors);
for (const ActorPtr &actor: new_actors) {
unregistered_actors.insert({actor->GetId(), actor});
// Identify any new hero vehicle if the system is in hybrid physics mode.
if (hybrid_physics_mode) {
if (actor->GetTypeId().front() == 'v') {
ActorId hero_actor_id = actor->GetId();
for (auto&& attribute: actor->GetAttributes()) {
if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") {
hero_actors.insert({hero_actor_id, actor});
}
}
}
}
}
// Update dynamic state and static attributes for all registered vehicles.
ALSM::IdleInfo max_idle_time = std::make_pair(0u, current_timestamp.elapsed_seconds);
UpdateRegisteredActorsData(hybrid_physics_mode, max_idle_time);
// Destroy registered vehicle if stuck at a location for too long.
if (IsVehicleStuck(max_idle_time.first)
&& (current_timestamp.elapsed_seconds - elapsed_last_actor_destruction) > DELTA_TIME_BETWEEN_DESTRUCTIONS) {
registered_vehicles.Destroy(max_idle_time.first);
RemoveActor(max_idle_time.first, true);
elapsed_last_actor_destruction = current_timestamp.elapsed_seconds;
}
// Update dynamic state and static attributes for unregistered actors.
UpdateUnregisteredActorsData();
}
std::vector<ActorPtr> ALSM::IdentifyNewActors(const ActorList &actor_list) {
std::vector<ActorPtr> new_actors;
for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
ActorPtr actor = *iter;
ActorId actor_id = actor->GetId();
if (!registered_vehicles.Contains(actor_id)
&& unregistered_actors.find(actor_id) == unregistered_actors.end()) {
new_actors.push_back(actor);
}
}
return new_actors;
}
ALSM::DestroyeddActors ALSM::IdentifyDestroyedActors(const ActorList &actor_list) {
ALSM::DestroyeddActors destroyed_actors;
ActorIdSet &deleted_registered = destroyed_actors.first;
ActorIdSet &deleted_unregistered = destroyed_actors.second;
// Building hash set of actors present in current frame.
ActorIdSet current_actors;
for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
current_actors.insert((*iter)->GetId());
}
// Searching for destroyed registered actors.
std::vector<ActorId> registered_ids = registered_vehicles.GetIDList();
for (const ActorId &actor_id : registered_ids) {
if (current_actors.find(actor_id) == current_actors.end()) {
deleted_registered.insert(actor_id);
}
}
// Searching for destroyed unregistered actors.
for (const auto &actor_info: unregistered_actors) {
const ActorId &actor_id = actor_info.first;
if (current_actors.find(actor_id) == current_actors.end()
|| registered_vehicles.Contains(actor_id)) {
deleted_unregistered.insert(actor_id);
}
}
return destroyed_actors;
}
void ALSM::UpdateRegisteredActorsData(const bool hybrid_physics_mode, ALSM::IdleInfo &max_idle_time) {
std::vector<ActorPtr> vehicle_list = registered_vehicles.GetList();
bool hero_actor_present = hybrid_physics_mode && hero_actors.size() != 0u;
float physics_radius = parameters.GetHybridPhysicsRadius();
float physics_radius_square = SQUARE(physics_radius);
for (const Actor &vehicle : vehicle_list) {
ActorId actor_id = vehicle->GetId();
cg::Transform vehicle_transform = vehicle->GetTransform();
cg::Location vehicle_location = vehicle_transform.location;
cg::Rotation vehicle_rotation = vehicle_transform.rotation;
cg::Vector3D vehicle_velocity = vehicle->GetVelocity();
// Initializing idle times.
if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) {
idle_time.insert({actor_id, current_timestamp.elapsed_seconds});
}
// Check if current actor is in range of hero actor and enable physics in hybrid mode.
bool in_range_of_hero_actor = false;
if (hero_actor_present) {
for (auto &hero_actor_info: hero_actors) {
const ActorId &hero_actor_id = hero_actor_info.first;
if (simulation_state.ContainsActor(hero_actor_id)) {
const cg::Location &hero_location = simulation_state.GetLocation(hero_actor_id);
if (cg::Math::DistanceSquared(vehicle_location, hero_location) < physics_radius_square) {
in_range_of_hero_actor = true;
break;
}
}
}
}
bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true;
vehicle->SetSimulatePhysics(enable_physics);
bool state_entry_present = simulation_state.ContainsActor(actor_id);
// If physics is disabled, calculate velocity based on change in position.
if (!enable_physics) {
cg::Location previous_location;
if (state_entry_present) {
previous_location = simulation_state.GetLocation(actor_id);
} else {
previous_location = vehicle_location;
}
cg::Vector3D displacement = (vehicle_location - previous_location);
vehicle_velocity = displacement * INV_HYBRID_DT;
}
// Updated kinematic state object.
auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(vehicle);
KinematicState kinematic_state{vehicle_location, vehicle_rotation,
vehicle_velocity, vehicle_ptr->GetSpeedLimit(), enable_physics};
// Updated traffic light state object.
TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
// Update simulation state.
if (state_entry_present) {
simulation_state.UpdateKinematicState(actor_id, kinematic_state);
simulation_state.UpdateTrafficLightState(actor_id, tl_state);
}
else {
cg::Vector3D dimensions = vehicle_ptr->GetBoundingBox().extent;
StaticAttributes attributes{ActorType::Vehicle, dimensions.x, dimensions.y, dimensions.z};
simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
}
// Updating idle time when necessary.
UpdateIdleTime(max_idle_time, actor_id);
}
}
void ALSM::UpdateUnregisteredActorsData() {
for (auto &actor_info: unregistered_actors) {
const ActorId actor_id = actor_info.first;
const ActorPtr actor_ptr = actor_info.second;
const std::string type_id = actor_ptr->GetTypeId();
const cg::Transform actor_transform = actor_ptr->GetTransform();
const cg::Location actor_location = actor_transform.location;
const cg::Rotation actor_rotation = actor_transform.rotation;
const cg::Vector3D actor_velocity = actor_ptr->GetVelocity();
KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true};
TrafficLightState tl_state;
ActorType actor_type = ActorType::Any;
cg::Vector3D dimensions;
std::vector<SimpleWaypointPtr> nearest_waypoints;
bool state_entry_not_present = !simulation_state.ContainsActor(actor_id);
if (type_id.front() == 'v') {
auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(actor_ptr);
kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit();
tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
if (state_entry_not_present) {
dimensions = vehicle_ptr->GetBoundingBox().extent;
actor_type = ActorType::Vehicle;
StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
} else {
simulation_state.UpdateKinematicState(actor_id, kinematic_state);
simulation_state.UpdateTrafficLightState(actor_id, tl_state);
}
// Identify occupied waypoints.
cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector();
std::vector<cg::Location> corners = {actor_location + cg::Location(extent.x * heading_vector),
actor_location,
actor_location + cg::Location(-extent.x * heading_vector)};
for (cg::Location &vertex: corners) {
SimpleWaypointPtr nearest_waypoint = local_map->GetWaypointInVicinity(vertex);
if (nearest_waypoint == nullptr) {nearest_waypoint = local_map->GetPedWaypoint(vertex);}
if (nearest_waypoint == nullptr) {nearest_waypoint = local_map->GetWaypoint(actor_location);}
nearest_waypoints.push_back(nearest_waypoint);
}
}
else if (type_id.front() == 'w') {
auto walker_ptr = boost::static_pointer_cast<cc::Walker>(actor_ptr);
if (state_entry_not_present) {
dimensions = walker_ptr->GetBoundingBox().extent;
actor_type = ActorType::Pedestrian;
StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
} else {
simulation_state.UpdateKinematicState(actor_id, kinematic_state);
}
// Identify occupied waypoints.
SimpleWaypointPtr nearest_waypoint = local_map->GetPedWaypoint(actor_location);
if (nearest_waypoint == nullptr) {nearest_waypoint = local_map->GetWaypoint(actor_location);}
nearest_waypoints.push_back(nearest_waypoint);
}
track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints);
}
}
void ALSM::UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id) {
if (idle_time.find(actor_id) != idle_time.end()) {
double &idle_duration = idle_time.at(actor_id);
TrafficLightState tl_state = simulation_state.GetTLS(actor_id);
if (simulation_state.GetVelocity(actor_id).SquaredLength() > SQUARE(STOPPED_VELOCITY_THRESHOLD)
|| (tl_state.at_traffic_light && tl_state.tl_state != TLS::Green)) {
idle_duration = current_timestamp.elapsed_seconds;
}
// Checking maximum idle time.
if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) {
max_idle_time = std::make_pair(actor_id, idle_duration);
}
}
}
bool ALSM::IsVehicleStuck(const ActorId& actor_id) {
if (idle_time.find(actor_id) != idle_time.end()) {
double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id);
if (delta_idle_time >= BLOCKED_TIME_THRESHOLD) {
return true;
}
}
return false;
}
void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
if (registered_actor) {
registered_vehicles.Remove({actor_id});
buffer_map.erase(actor_id);
idle_time.erase(actor_id);
localization_stage.RemoveActor(actor_id);
collision_stage.RemoveActor(actor_id);
traffic_light_stage.RemoveActor(actor_id);
motion_plan_stage.RemoveActor(actor_id);
}
else {
unregistered_actors.erase(actor_id);
hero_actors.erase(actor_id);
}
track_traffic.DeleteActor(actor_id);
simulation_state.RemoveActor(actor_id);
}
void ALSM::Reset() {
unregistered_actors.clear();
idle_time.clear();
hero_actors.clear();
elapsed_last_actor_destruction = 0.0;
current_timestamp = world.GetSnapshot().GetTimestamp();
}
} // namespace traffic_manager
} // namespace carla

106
LibCarla/source/carla/trafficmanager/ALSM.h Normal file
View File

@ -0,0 +1,106 @@
#pragma once
#include <memory>
#include "carla/client/ActorList.h"
#include "carla/client/Timestamp.h"
#include "carla/client/World.h"
#include "carla/Memory.h"
#include "carla/trafficmanager/AtomicActorSet.h"
#include "carla/trafficmanager/CollisionStage.h"
#include "carla/trafficmanager/DataStructures.h"
#include "carla/trafficmanager/InMemoryMap.h"
#include "carla/trafficmanager/LocalizationStage.h"
#include "carla/trafficmanager/MotionPlanStage.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/SimulationState.h"
#include "carla/trafficmanager/TrafficLightStage.h"
namespace carla {
namespace traffic_manager {
using namespace constants::HybridMode;
using namespace constants::VehicleRemoval;
namespace chr = std::chrono;
namespace cg = carla::geom;
namespace cc = carla::client;
using ActorList = carla::SharedPtr<cc::ActorList>;
using ActorMap = std::unordered_map<ActorId, ActorPtr>;
using IdleTimeMap = std::unordered_map<ActorId, double>;
using LocalMapPtr = std::shared_ptr<InMemoryMap>;
/// ALSM: Agent Lifecycle and State Managerment
/// This class has functionality to update the local cache of kinematic states
/// and manage memory and cleanup for varying number of vehicles in the simulation.
class ALSM {
private:
AtomicActorSet &registered_vehicles;
// Structure containing vehicles in the simulator not registered with the traffic manager.
ActorMap unregistered_actors;
BufferMap &buffer_map;
// Structure keeping track of duration of vehicles stuck in a location.
IdleTimeMap idle_time;
// Structure containing vehicles with attribute role_name with value hero.
ActorMap hero_actors;
TrackTraffic &track_traffic;
const Parameters &parameters;
const cc::World &world;
const LocalMapPtr &local_map;
SimulationState &simulation_state;
LocalizationStage &localization_stage;
CollisionStage &collision_stage;
TrafficLightStage &traffic_light_stage;
MotionPlanStage &motion_plan_stage;
// Time elapsed since last vehicle destruction due to being idle for too long.
double elapsed_last_actor_destruction {0.0};
cc::Timestamp current_timestamp;
// Updates the duration for which a registered vehicle is stuck at a location.
void UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id);
// Method to determine if a vehicle is stuck at a place for too long.
bool IsVehicleStuck(const ActorId& actor_id);
// Removes an actor from traffic manager and performs clean up of associated data
// from various stages tracking the said vehicle.
void RemoveActor(const ActorId actor_id, const bool registered_actor);
using ActorVector = std::vector<ActorPtr>;
// Method to identify actors newly spawned in the simulation since last tick.
ActorVector IdentifyNewActors(const ActorList &actor_list);
using DestroyeddActors = std::pair<ActorIdSet, ActorIdSet>;
// Method to identify actors deleted in the last frame.
// Arrays of registered and unregistered actors are returned separately.
DestroyeddActors IdentifyDestroyedActors(const ActorList &actor_list);
using IdleInfo = std::pair<ActorId, double>;
void UpdateRegisteredActorsData(const bool hybrid_physics_mode, IdleInfo &max_idle_time);
void UpdateUnregisteredActorsData();
public:
ALSM(AtomicActorSet &registered_vehicles,
BufferMap &buffer_map,
TrackTraffic &track_traffic,
const Parameters &parameters,
const cc::World &world,
const LocalMapPtr &local_map,
SimulationState &simulation_state,
LocalizationStage &localization_stage,
CollisionStage &collision_stage,
TrafficLightStage &traffic_light_stage,
MotionPlanStage &motion_plan_stage);
void Update();
void Reset();
};
} // namespace traffic_manager
} // namespace carla

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

@ -61,22 +61,27 @@ namespace traffic_manager {
++state_counter;
}
void Remove(std::vector<ActorPtr> actor_list) {
void Remove(std::vector<ActorId> actor_id_list) {
std::lock_guard<std::mutex> lock(modification_mutex);
for (auto& actor: actor_list) {
actor_set.erase(actor->GetId());
for (auto& actor_id: actor_id_list) {
if (actor_set.find(actor_id) != actor_set.end()){
actor_set.erase(actor_id);
}
}
++state_counter;
}
void Destroy(ActorPtr actor) {
void Destroy(ActorId actor_id) {
std::lock_guard<std::mutex> lock(modification_mutex);
actor_set.erase(actor->GetId());
if (actor_set.find(actor_id) != actor_set.end()) {
ActorPtr actor = actor_set.at(actor_id);
actor->Destroy();
actor_set.erase(actor_id);
++state_counter;
}
}
int GetState() {
@ -96,6 +101,12 @@ namespace traffic_manager {
return actor_set.size();
}
void Clear() {
std::lock_guard<std::mutex> lock(modification_mutex);
return actor_set.clear();
}
};
} // namespace traffic_manager

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

@ -17,7 +17,7 @@ namespace traffic_manager {
private:
std::mutex map_mutex;
mutable std::mutex map_mutex;
std::unordered_map<Key, Value> map;
public:
@ -30,13 +30,13 @@ namespace traffic_manager {
map.insert(entry);
}
bool Contains(const Key &key) {
bool Contains(const Key &key) const {
std::lock_guard<std::mutex> lock(map_mutex);
return map.find(key) != map.end();
}
Value &GetValue(const Key &key) {
const Value &GetValue(const Key &key) const {
std::lock_guard<std::mutex> lock(map_mutex);
return map.at(key);

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

@ -1,135 +0,0 @@
// 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/BatchControlStage.h"
namespace carla {
namespace traffic_manager {
BatchControlStage::BatchControlStage(
std::string stage_name,
std::shared_ptr<PlannerToControlMessenger> messenger,
carla::client::detail::EpisodeProxy &episode_proxy,
Parameters &parameters)
: PipelineStage(stage_name),
messenger(messenger),
episode_proxy_bcs(episode_proxy),
parameters(parameters) {
// Initializing number of vehicles to zero in the beginning.
number_of_vehicles = 0u;
}
BatchControlStage::~BatchControlStage() {}
void BatchControlStage::Action() {
// Looping over registered actors.
for (uint64_t i = 0u; i < number_of_vehicles && data_frame != nullptr; ++i) {
carla::rpc::VehicleControl vehicle_control;
const PlannerToControlData &element = data_frame->at(i);
if (!element.actor || !element.actor->IsAlive()) {
continue;
}
const carla::ActorId actor_id = element.actor->GetId();
// Apply actuation from controller if physics enabled.
if (element.physics_enabled) {
vehicle_control.throttle = element.throttle;
vehicle_control.brake = element.brake;
vehicle_control.steer = element.steer;
commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
}
// Apply target transform for physics-less vehicles.
else {
const cg::Transform vehicle_transform = element.actor->GetTransform();
const cg::Location vehicle_location = vehicle_transform.location;
const cg::Vector3D vehicle_heading = vehicle_transform.GetForwardVector();
const cg::Transform &teleportation_transform = element.transform;
const cg::Location &teleportation_location_candidate = teleportation_transform.location;
const cg::Vector3D teleprotation_relative_position = teleportation_location_candidate - vehicle_location;
cg::Transform corrected_teleportation_transform = teleportation_transform;
// Validate if teleportation transform is ahead of the vehicle's current position.
// If not, then teleport to vehicle's current position.
if (cg::Math::Dot(teleprotation_relative_position, vehicle_heading) < 0.0f) {
corrected_teleportation_transform = vehicle_transform;
}
commands->at(i) = carla::rpc::Command::ApplyTransform(actor_id, corrected_teleportation_transform);
}
}
}
void BatchControlStage::DataReceiver() {
data_frame = messenger->Peek();
// Allocating new containers for the changed number of registered vehicles.
if (data_frame != nullptr && number_of_vehicles != (*data_frame.get()).size()) {
number_of_vehicles = static_cast<uint64_t>((*data_frame.get()).size());
// 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();
// Asynchronous mode.
if (!synch_mode) {
// Apply batch command through an asynchronous RPC call.
if (commands != nullptr) {
episode_proxy_bcs.Lock()->ApplyBatch(*commands.get(), false);
}
// Applying an infinitesimal sleep statement for providing a system cancellation point.
std::this_thread::sleep_for(1us);
}
// Synchronous mode.
else {
std::unique_lock<std::mutex> lock(step_execution_mutex);
// TODO: Re-introduce timeout while waiting for RunStep() call.
while (!run_step.load()) {
step_execution_notifier.wait_for(lock, 1ms, [this]() {return run_step.load();});
}
// Apply batch command in synchronous RPC call.
if (commands != nullptr) {
episode_proxy_bcs.Lock()->ApplyBatchSync(*commands.get(), true);
}
// Set flag to false, unblock RunStep() call and release mutex lock.
run_step.store(false);
step_complete_notifier.notify_one();
lock.unlock();
}
}
bool BatchControlStage::RunStep() {
bool synch_mode = parameters.GetSynchronousMode();
if (synch_mode) {
std::unique_lock<std::mutex> lock(step_execution_mutex);
// Set flag to true, notify DataSender and wait for a return notification
run_step.store(true);
step_execution_notifier.notify_one();
while (run_step.load()) {
step_complete_notifier.wait_for(lock, 1ms, [this]() {return !run_step.load();});
}
}
return true;
}
} // namespace traffic_manager
} // namespace carla

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

@ -1,76 +0,0 @@
// 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 <atomic>
#include <chrono>
#include <memory>
#include <mutex>
#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 {
using namespace std::literals::chrono_literals;
/// This class receives actuation signals (throttle, brake, steer)
/// from Motion Planner Stage class and communicates these signals to
/// the simulator in batches to control vehicles' movement.
class BatchControlStage : public PipelineStage {
private:
/// Pointer to frame received from Motion Planner.
std::shared_ptr<PlannerToControlFrame> data_frame;
/// Pointer to a messenger from Motion Planner.
std::shared_ptr<PlannerToControlMessenger> messenger;
/// Reference to CARLA client connection object.
carla::client::detail::EpisodeProxy episode_proxy_bcs;
/// Array to hold command batch.
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 {false};
/// Mutex for progressing synchronous execution.
std::mutex step_execution_mutex;
/// Condition variables for progressing synchronous execution.
std::condition_variable step_execution_notifier;
std::condition_variable step_complete_notifier;
public:
BatchControlStage(std::string stage_name,
std::shared_ptr<PlannerToControlMessenger> messenger,
carla::client::detail::EpisodeProxy &episode_proxy,
Parameters &parameters);
~BatchControlStage();
void DataReceiver() override;
void Action() override;
void DataSender() override;
bool RunStep();
};
} // namespace traffic_manager
} // namespace carla

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

@ -1,275 +1,373 @@
// 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 "CollisionStage.h"
#include "carla/geom/Math.h"
#define MAX(__a, __b) ((__a) > (__b) ? (__a) : (__b))
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/LocalizationUtils.h"
#include "carla/trafficmanager/CollisionStage.h"
namespace carla {
namespace traffic_manager {
namespace CollisionStageConstants {
using Point2D = bg::model::point<double, 2, bg::cs::cartesian>;
using TLS = carla::rpc::TrafficLightState;
static const float VERTICAL_OVERLAP_THRESHOLD = 4.0f;
static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f;
static const float BOUNDARY_EXTENSION_MAXIMUM = 50.0f;
static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f;
static const float LOCKING_DISTANCE_PADDING = 4.0f;
static const float MAX_LOCKING_EXTENSION = 10.0f;
static const float MAX_COLLISION_RADIUS = 100.0f;
static const float MIN_COLLISION_RADIUS = 15.0f;
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 SQUARE_ROOT_OF_TWO = 1.414f;
static const float COS_10_DEGREES = 0.9848f;
} // namespace CollisionStageConstants
using namespace CollisionStageConstants;
using namespace constants::Collision;
using constants::WaypointSelection::JUNCTION_LOOK_AHEAD;
CollisionStage::CollisionStage(
std::string stage_name,
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
Parameters &parameters,
const std::vector<ActorId> &vehicle_id_list,
const SimulationState &simulation_state,
const BufferMap &buffer_map,
const TrackTraffic &track_traffic,
const Parameters &parameters,
CollisionFrame &output_array,
cc::DebugHelper &debug_helper)
: PipelineStage(stage_name),
localization_messenger(localization_messenger),
planner_messenger(planner_messenger),
: vehicle_id_list(vehicle_id_list),
simulation_state(simulation_state),
buffer_map(buffer_map),
track_traffic(track_traffic),
parameters(parameters),
debug_helper(debug_helper) {
// Initializing clock for checking unregistered actors periodically.
last_world_actors_pass_instance = chr::system_clock::now();
// Initializing output array selector.
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() {
// Clearing the old chache.
vehicle_cache.clear();
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) {
LocalizationToCollisionData &data = localization_frame->at(i);
if (!data.actor->IsAlive()) {
continue;
}
const Actor ego_actor = data.actor;
const ActorId ego_actor_id = ego_actor->GetId();
const cg::Location ego_location = ego_actor->GetLocation();
const cg::Vector3D ego_velocity = data.velocity;
const SimpleWaypointPtr& closest_point = data.closest_waypoint;
const SimpleWaypointPtr& junction_look_ahead = data.junction_look_ahead_waypoint;
using OverlappingActorInfo = std::vector<std::tuple<ActorId, Actor, cg::Vector3D>>;
const OverlappingActorInfo &collision_candidates = data.overlapping_actors;
output_array(output_array),
debug_helper(debug_helper) {}
void CollisionStage::Update(const unsigned long index) {
ActorId obstacle_id = 0u;
bool collision_hazard = false;
float available_distance_margin = std::numeric_limits<float>::infinity();
cg::Vector3D obstacle_velocity;
const SimpleWaypointPtr safe_point_junction = data.safe_point_after_junction;
try {
const ActorId ego_actor_id = vehicle_id_list.at(index);
if (simulation_state.ContainsActor(ego_actor_id)) {
const cg::Location ego_location = simulation_state.GetLocation(ego_actor_id);
const Buffer &ego_buffer = buffer_map.at(ego_actor_id);
const unsigned long look_ahead_index = GetTargetWaypoint(ego_buffer, JUNCTION_LOOK_AHEAD).second;
ActorIdSet overlapping_actors = track_traffic.GetOverlappingVehicles(ego_actor_id);
std::vector<ActorId> collision_candidate_ids;
// Run through vehicles with overlapping paths and filter them;
float collision_radius_square = SQUARE(MAX_COLLISION_RADIUS);
for (ActorId overlapping_actor_id : overlapping_actors) {
// If actor is within maximum collision avoidance and vertical overlap range.
const cg::Location &overlapping_actor_location = simulation_state.GetLocation(overlapping_actor_id);
if (overlapping_actor_id != ego_actor_id
&& cg::Math::DistanceSquared(overlapping_actor_location, ego_location) < collision_radius_square
&& std::abs(ego_location.z - overlapping_actor_location.z) < VERTICAL_OVERLAP_THRESHOLD) {
collision_candidate_ids.push_back(overlapping_actor_id);
}
}
// Sorting collision candidates in accending order of distance to current vehicle.
std::sort(collision_candidate_ids.begin(), collision_candidate_ids.end(),
[this, &ego_location](const ActorId &a_id_1, const ActorId &a_id_2) {
const cg::Location &e_loc = ego_location;
const cg::Location &loc_1 = simulation_state.GetLocation(a_id_1);
const cg::Location &loc_2 = simulation_state.GetLocation(a_id_2);
return (cg::Math::DistanceSquared(e_loc, loc_1) < cg::Math::DistanceSquared(e_loc, loc_2));
});
// Check every actor in the vicinity if it poses a collision hazard.
for (auto actor_info = collision_candidates.begin();
actor_info != collision_candidates.end() && !collision_hazard;
++actor_info) {
for (auto iter = collision_candidate_ids.begin();
iter != collision_candidate_ids.end() && !collision_hazard;
++iter) {
const ActorId other_actor_id = *iter;
const ActorType other_actor_type = simulation_state.GetType(other_actor_id);
ActorId other_actor_id;
Actor other_actor;
cg::Vector3D other_velocity;
std::tie(other_actor_id, other_actor, other_velocity) = *actor_info;
if (!other_actor->IsAlive()) continue;
const auto other_actor_type = other_actor->GetTypeId();
const cg::Location other_location = other_actor->GetLocation();
// Collision checks increase with speed
float collision_distance = std::pow(floor(ego_velocity.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_actor_id != ego_actor_id &&
(cg::Math::DistanceSquared(ego_location, other_location)
< std::pow(MAX_COLLISION_RADIUS, 2)) &&
(std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) {
if (parameters.GetCollisionDetection(ego_actor, other_actor)) {
std::pair<bool, float> negotiation_result = NegotiateCollision(ego_actor, other_actor, ego_location,
other_location, closest_point, junction_look_ahead,
ego_velocity, other_velocity);
if ((safe_point_junction != nullptr
&& !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction,
other_location, other_velocity))
|| negotiation_result.first)
{
if ((other_actor_type[0] == 'v' && parameters.GetPercentageIgnoreVehicles(ego_actor) <= (rand() % 101)) ||
(other_actor_type[0] == 'w' && parameters.GetPercentageIgnoreWalkers(ego_actor) <= (rand() % 101)))
{
if (parameters.GetCollisionDetection(ego_actor_id, other_actor_id)
&& buffer_map.find(ego_actor_id) != buffer_map.end()
&& simulation_state.ContainsActor(other_actor_id)) {
std::pair<bool, float> negotiation_result = NegotiateCollision(ego_actor_id,
other_actor_id,
look_ahead_index);
if (negotiation_result.first) {
if ((other_actor_type == ActorType::Vehicle
&& parameters.GetPercentageIgnoreVehicles(ego_actor_id) <= pgen.next())
|| (other_actor_type == ActorType::Pedestrian
&& parameters.GetPercentageIgnoreWalkers(ego_actor_id) <= pgen.next())) {
collision_hazard = true;
obstacle_id = other_actor_id;
available_distance_margin = negotiation_result.second;
obstacle_velocity = other_velocity;
}
}
}
}
}
CollisionHazardData &output_element = output_array.at(index);
output_element.hazard_actor_id = obstacle_id;
output_element.hazard = collision_hazard;
output_element.available_distance_margin = available_distance_margin;
}
} catch (const std::exception&) {
carla::log_info("Actor might not be alive \n");
void CollisionStage::RemoveActor(const ActorId actor_id) {
collision_locks.erase(actor_id);
}
CollisionToPlannerData &message = current_planner_frame->at(i);
message.hazard = collision_hazard;
message.distance_to_other_vehicle = available_distance_margin;
message.other_vehicle_velocity = obstacle_velocity;
void CollisionStage::Reset() {
collision_locks.clear();
}
float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id) {
const float velocity = cg::Math::Dot(simulation_state.GetVelocity(actor_id), simulation_state.GetHeading(actor_id));
float bbox_extension;
// Using a linear function to calculate boundary length.
bbox_extension = BOUNDARY_EXTENSION_RATE * velocity + BOUNDARY_EXTENSION_MINIMUM;
// If a valid collision lock present, change boundary length to maintain lock.
if (collision_locks.find(actor_id) != collision_locks.end()) {
const CollisionLock &lock = collision_locks.at(actor_id);
float lock_boundary_length = static_cast<float>(lock.distance_to_lead_vehicle + LOCKING_DISTANCE_PADDING);
// Only extend boundary track vehicle if the leading vehicle
// if it is not further than velocity dependent extension by MAX_LOCKING_EXTENSION.
if ((lock_boundary_length - lock.initial_lock_distance) < MAX_LOCKING_EXTENSION) {
bbox_extension = lock_boundary_length;
}
}
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.
vehicle_id_to_index.clear();
uint64_t index = 0u;
for (auto &element: *localization_frame.get()) {
vehicle_id_to_index.insert({element.actor->GetId(), index++});
return bbox_extension;
}
// Allocating new containers for the changed number
// of registered vehicles.
if (number_of_vehicles != (*localization_frame.get()).size()) {
LocationVector CollisionStage::GetBoundary(const ActorId actor_id) {
const ActorType actor_type = simulation_state.GetType(actor_id);
const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
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);
}
float forward_extension = 0.0f;
if (actor_type == ActorType::Pedestrian) {
// Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
forward_extension = simulation_state.GetVelocity(actor_id).Length() * WALKER_TIME_EXTENSION;
}
// Cleaning geodesic boundaries from the last iteration.
geodesic_boundaries.clear();
cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id);
float bbox_x = dimensions.x;
float bbox_y = dimensions.y;
const cg::Vector3D x_boundary_vector = heading_vector * (bbox_x + forward_extension);
const auto perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f).MakeUnitVector();
const cg::Vector3D y_boundary_vector = perpendicular_vector * (bbox_y + forward_extension);
// Four corners of the vehicle in top view clockwise order (left-handed system).
const cg::Location location = simulation_state.GetLocation(actor_id);
LocationVector bbox_boundary = {
location + cg::Location(x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
location + cg::Location(x_boundary_vector + y_boundary_vector),
};
return bbox_boundary;
}
void CollisionStage::DataSender() {
LocationVector CollisionStage::GetGeodesicBoundary(const ActorId actor_id) {
LocationVector geodesic_boundary;
localization_messenger->Pop();
if (geodesic_boundary_map.find(actor_id) != geodesic_boundary_map.end()) {
geodesic_boundary = geodesic_boundary_map.at(actor_id);
} else {
const LocationVector bbox = GetBoundary(actor_id);
planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b);
frame_selector = !frame_selector;
if (buffer_map.find(actor_id) != buffer_map.end()) {
float bbox_extension = GetBoundingBoxExtention(actor_id);
const float specific_lead_distance = parameters.GetDistanceToLeadingVehicle(actor_id);
bbox_extension = std::max(specific_lead_distance, bbox_extension);
const float bbox_extension_square = SQUARE(bbox_extension);
LocationVector left_boundary;
LocationVector right_boundary;
cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id);
const float width = dimensions.y;
const float length = dimensions.x;
const Buffer &waypoint_buffer = buffer_map.at(actor_id);
const TargetWPInfo target_wp_info = GetTargetWaypoint(waypoint_buffer, length);
const SimpleWaypointPtr boundary_start = target_wp_info.first;
const uint64_t boundary_start_index = target_wp_info.second;
// At non-signalized junctions, we extend the boundary across the junction
// and in all other situations, boundary length is velocity-dependent.
SimpleWaypointPtr boundary_end = nullptr;
SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index);
bool reached_distance = false;
for (uint64_t j = boundary_start_index; !reached_distance && (j < waypoint_buffer.size()); ++j) {
if (boundary_start->DistanceSquared(current_point) > bbox_extension_square || j == waypoint_buffer.size() - 1) {
reached_distance = true;
}
if (boundary_end == nullptr
|| cg::Math::Dot(boundary_end->GetForwardVector(), current_point->GetForwardVector()) < COS_10_DEGREES
|| reached_distance) {
const cg::Vector3D heading_vector = current_point->GetForwardVector();
const cg::Location location = current_point->GetLocation();
cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
perpendicular_vector = perpendicular_vector.MakeUnitVector();
// Direction determined for the left-handed system.
const cg::Vector3D scaled_perpendicular = perpendicular_vector * width;
left_boundary.push_back(location + cg::Location(scaled_perpendicular));
right_boundary.push_back(location + cg::Location(-1.0f * scaled_perpendicular));
boundary_end = current_point;
}
std::pair<bool, float> 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,
const cg::Vector3D reference_velocity,
const cg::Vector3D other_velocity) {
current_point = waypoint_buffer.at(j);
}
// Vehicle IDs
const ActorId reference_vehicle_id = reference_vehicle->GetId();
const ActorId other_vehicle_id = other_vehicle->GetId();
// Reversing right boundary to construct clockwise (left-hand system)
// boundary. This is so because both left and right boundary vectors have
// the closest point to the vehicle at their starting index for the right
// boundary,
// we want to begin at the farthest point to have a clockwise trace.
std::reverse(right_boundary.begin(), right_boundary.end());
geodesic_boundary.insert(geodesic_boundary.end(), right_boundary.begin(), right_boundary.end());
geodesic_boundary.insert(geodesic_boundary.end(), bbox.begin(), bbox.end());
geodesic_boundary.insert(geodesic_boundary.end(), left_boundary.begin(), left_boundary.end());
} else {
geodesic_boundary = bbox;
}
geodesic_boundary_map.insert({actor_id, geodesic_boundary});
}
return geodesic_boundary;
}
Polygon CollisionStage::GetPolygon(const LocationVector &boundary) {
traffic_manager::Polygon boundary_polygon;
for (const cg::Location &location : boundary) {
bg::append(boundary_polygon.outer(), Point2D(location.x, location.y));
}
bg::append(boundary_polygon.outer(), Point2D(boundary.front().x, boundary.front().y));
return boundary_polygon;
}
GeometryComparison CollisionStage::GetGeometryBetweenActors(const ActorId reference_vehicle_id,
const ActorId other_actor_id) {
std::pair<ActorId, ActorId> key_parts;
if (reference_vehicle_id < other_actor_id) {
key_parts = {reference_vehicle_id, other_actor_id};
} else {
key_parts = {other_actor_id, reference_vehicle_id};
}
uint64_t actor_id_key = 0u;
actor_id_key |= key_parts.first;
actor_id_key <<= 32;
actor_id_key |= key_parts.second;
GeometryComparison comparision_result{-1.0, -1.0, -1.0, -1.0};
if (geometry_cache.find(actor_id_key) != geometry_cache.end()) {
comparision_result = geometry_cache.at(actor_id_key);
double mref_veh_other = comparision_result.reference_vehicle_to_other_geodesic;
comparision_result.reference_vehicle_to_other_geodesic = comparision_result.other_vehicle_to_reference_geodesic;
comparision_result.other_vehicle_to_reference_geodesic = mref_veh_other;
} else {
const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle_id));
const Polygon other_polygon = GetPolygon(GetBoundary(other_actor_id));
const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle_id));
const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_actor_id));
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);
const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
comparision_result = {reference_vehicle_to_other_geodesic,
other_vehicle_to_reference_geodesic,
inter_geodesic_distance,
inter_bbox_distance};
geometry_cache.insert({actor_id_key, comparision_result});
}
return comparision_result;
}
std::pair<bool, float> CollisionStage::NegotiateCollision(const ActorId reference_vehicle_id,
const ActorId other_actor_id,
const uint64_t reference_junction_look_ahead_index) {
// Output variables for the method.
bool hazard = false;
float available_distance_margin = std::numeric_limits<float>::infinity();
const cg::Location reference_location = simulation_state.GetLocation(reference_vehicle_id);
const cg::Location other_location = simulation_state.GetLocation(other_actor_id);
// Ego and other vehicle heading.
const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector();
const cg::Vector3D reference_heading = simulation_state.GetHeading(reference_vehicle_id);
// Vector from ego position to position of the other vehicle.
const float vector_magnitude_epsilon = 2 * std::numeric_limits<float>::epsilon();
const float vector_magnitude_epsilon = 2.0f * std::numeric_limits<float>::epsilon();
cg::Vector3D reference_to_other = other_location - reference_location;
reference_to_other = reference_to_other.MakeSafeUnitVector(vector_magnitude_epsilon);
// Other vehicle heading.
const cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector();
const cg::Vector3D other_heading = simulation_state.GetHeading(other_actor_id);
// Vector from other vehicle position to ego position.
cg::Vector3D other_to_reference = reference_location - other_location;
other_to_reference = other_to_reference.MakeSafeUnitVector(vector_magnitude_epsilon);
// Obtain cc::Vehicle pointers and calculate half diagonal length of vehicle bounding box.
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 * SQUARE_ROOT_OF_TWO;
float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * SQUARE_ROOT_OF_TWO;
float reference_vehicle_length = simulation_state.GetDimensions(reference_vehicle_id).x * SQUARE_ROOT_OF_TWO;
float other_vehicle_length = simulation_state.GetDimensions(other_actor_id).x * SQUARE_ROOT_OF_TWO;
float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location);
float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id,
reference_velocity, reference_heading);
float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle_id, other_velocity, other_heading);
float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id);
float other_bounding_box_extension = GetBoundingBoxExtention(other_actor_id);
// Calculate minimum distance between vehicle to consider collision negotiation.
float inter_vehicle_length = reference_vehicle_length + other_vehicle_length;
float ego_detection_range = std::pow(ego_bounding_box_extension + inter_vehicle_length, 2.0f);
float cross_detection_range = std::pow(ego_bounding_box_extension + inter_vehicle_length
+ other_bounding_box_extension, 2.0f);
float ego_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length);
float cross_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length + other_bounding_box_extension);
// Conditions to consider collision negotiation.
bool other_vehicle_in_ego_range = inter_vehicle_distance < ego_detection_range;
bool other_vehicles_in_cross_detection_range = inter_vehicle_distance < cross_detection_range;
bool other_vehicle_in_front = cg::Math::Dot(reference_heading, reference_to_other) > 0;
float reference_heading_to_other_dot = cg::Math::Dot(reference_heading, reference_to_other);
bool other_vehicle_in_front = reference_heading_to_other_dot > 0;
const Buffer &reference_vehicle_buffer = buffer_map.at(reference_vehicle_id);
SimpleWaypointPtr closest_point = reference_vehicle_buffer.front();
bool ego_inside_junction = closest_point->CheckJunction();
bool ego_at_traffic_light = reference_vehicle_ptr->IsAtTrafficLight();
bool ego_stopped_by_light = reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green;
bool ego_at_junction_entrance = !closest_point->CheckJunction() && junction_look_ahead->CheckJunction();
TrafficLightState reference_tl_state = simulation_state.GetTLS(reference_vehicle_id);
bool ego_at_traffic_light = reference_tl_state.at_traffic_light;
bool ego_stopped_by_light = reference_tl_state.tl_state != TLS::Green;
SimpleWaypointPtr look_ahead_point = reference_vehicle_buffer.at(reference_junction_look_ahead_index);
bool ego_at_junction_entrance = !closest_point->CheckJunction() && look_ahead_point->CheckJunction();
// Conditions to consider collision negotiation.
if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light)
&& ((ego_inside_junction && other_vehicles_in_cross_detection_range)
|| (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) {
GeometryComparisonCache cache = GetGeometryBetweenActors(reference_vehicle, other_vehicle,
reference_location, other_location,
reference_velocity, other_velocity);
GeometryComparison geometry_comparison = GetGeometryBetweenActors(reference_vehicle_id, other_actor_id);
// Conditions for collision negotiation.
bool geodesic_path_bbox_touching = cache.inter_geodesic_distance < 0.1;
bool vehicle_bbox_touching = cache.inter_bbox_distance < 0.1;
bool ego_path_clear = cache.other_vehicle_to_reference_geodesic > 0.1;
bool other_path_clear = cache.reference_vehicle_to_other_geodesic > 0.1;
bool ego_path_priority = cache.reference_vehicle_to_other_geodesic < cache.other_vehicle_to_reference_geodesic;
bool ego_angular_priority = cg::Math::Dot(reference_heading, reference_to_other)
< cg::Math::Dot(other_heading, other_to_reference);
bool geodesic_path_bbox_touching = geometry_comparison.inter_geodesic_distance < 0.1;
bool vehicle_bbox_touching = geometry_comparison.inter_bbox_distance < 0.1;
bool ego_path_clear = geometry_comparison.other_vehicle_to_reference_geodesic > 0.1;
bool other_path_clear = geometry_comparison.reference_vehicle_to_other_geodesic > 0.1;
bool ego_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic < geometry_comparison.other_vehicle_to_reference_geodesic;
bool other_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic > geometry_comparison.other_vehicle_to_reference_geodesic;
bool ego_angular_priority = reference_heading_to_other_dot< cg::Math::Dot(other_heading, other_to_reference);
// Whichever vehicle's path is farthest away from the other vehicle gets priority to move.
if (geodesic_path_bbox_touching
&& ((!vehicle_bbox_touching
&& (!ego_path_clear || (ego_path_clear && other_path_clear && !ego_angular_priority && !ego_path_priority)))
|| (vehicle_bbox_touching && !ego_angular_priority && !ego_path_priority))) {
bool lower_priority = !ego_path_priority && (other_path_priority || !ego_angular_priority);
bool blocked_by_other_or_lower_priority = !ego_path_clear || (other_path_clear && lower_priority);
bool yield_pre_crash = !vehicle_bbox_touching && blocked_by_other_or_lower_priority;
bool yield_post_crash = vehicle_bbox_touching && !ego_angular_priority;
if (geodesic_path_bbox_touching && (yield_pre_crash || yield_post_crash)) {
hazard = true;
const float specific_distance_margin = MAX(parameters.GetDistanceToLeadingVehicle(reference_vehicle),
BOUNDARY_EXTENSION_MINIMUM);
available_distance_margin = static_cast<float>(MAX(cache.reference_vehicle_to_other_geodesic
- specific_distance_margin, 0.0));
const float reference_lead_distance = parameters.GetDistanceToLeadingVehicle(reference_vehicle_id);
const float specific_distance_margin = std::max(reference_lead_distance, BOUNDARY_EXTENSION_MINIMUM);
available_distance_margin = static_cast<float>(std::max(geometry_comparison.reference_vehicle_to_other_geodesic
- static_cast<double>(specific_distance_margin), 0.0));
///////////////////////////////////// Collision locking mechanism /////////////////////////////////
// The idea is, when encountering a possible collision,
@ -280,23 +378,25 @@ namespace CollisionStageConstants {
if (collision_locks.find(reference_vehicle_id) != collision_locks.end()) {
CollisionLock &lock = collision_locks.at(reference_vehicle_id);
// Check if the same vehicle is under lock.
if (other_vehicle_id == lock.lead_vehicle_id) {
if (other_actor_id == lock.lead_vehicle_id) {
// If the body of the lead vehicle is touching the reference vehicle bounding box.
if (cache.other_vehicle_to_reference_geodesic < 0.1) {
if (geometry_comparison.other_vehicle_to_reference_geodesic < 0.1) {
// Distance between the bodies of the vehicles.
lock.distance_to_lead_vehicle = cache.inter_bbox_distance;
lock.distance_to_lead_vehicle = geometry_comparison.inter_bbox_distance;
} else {
// Distance from reference vehicle body to other vehicle path polygon.
lock.distance_to_lead_vehicle = cache.reference_vehicle_to_other_geodesic;
lock.distance_to_lead_vehicle = geometry_comparison.reference_vehicle_to_other_geodesic;
}
} else {
// If possible collision with a new vehicle, re-initialize with new lock entry.
lock = {other_vehicle_id, cache.inter_bbox_distance, cache.inter_bbox_distance};
lock = {geometry_comparison.inter_bbox_distance, geometry_comparison.inter_bbox_distance, other_actor_id};
}
} else {
// Insert and initialize lock entry if not present.
collision_locks.insert({reference_vehicle_id,
{other_vehicle_id, cache.inter_bbox_distance, cache.inter_bbox_distance}});
{geometry_comparison.inter_bbox_distance,
geometry_comparison.inter_bbox_distance,
other_actor_id}});
}
}
}
@ -309,259 +409,20 @@ namespace CollisionStageConstants {
return {hazard, available_distance_margin};
}
traffic_manager::Polygon CollisionStage::GetPolygon(const LocationList &boundary) {
traffic_manager::Polygon boundary_polygon;
for (const cg::Location &location: boundary) {
bg::append(boundary_polygon.outer(), Point2D(location.x, location.y));
}
bg::append(boundary_polygon.outer(), Point2D(boundary.front().x, boundary.front().y));
return boundary_polygon;
void CollisionStage::ClearCycleCache() {
geodesic_boundary_map.clear();
geometry_cache.clear();
}
LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location,
const cg::Vector3D velocity) {
const ActorId actor_id = actor->GetId();
if (geodesic_boundaries.find(actor_id) != geodesic_boundaries.end()) {
return geodesic_boundaries.at(actor_id);
}
const LocationList bbox = GetBoundary(actor, vehicle_location, velocity);
if (vehicle_id_to_index.find(actor_id) != vehicle_id_to_index.end()) {
float bbox_extension = GetBoundingBoxExtention(actor_id, velocity, actor->GetTransform().GetForwardVector());
const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor);
if (specific_distance_margin > 0.0f) {
bbox_extension = MAX(specific_distance_margin, bbox_extension);
}
const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor_id)).buffer;
LocationList left_boundary;
LocationList right_boundary;
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const float width = vehicle->GetBoundingBox().extent.y;
const float length_squared = std::pow(vehicle->GetBoundingBox().extent.x*2, 2.0f);
SimpleWaypointPtr boundary_start = waypoint_buffer.front();
SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
uint64_t boundary_start_index = 0u;
while (boundary_start->DistanceSquared(front_waypoint) < length_squared &&
boundary_start_index < waypoint_buffer.size() -1) {
boundary_start = waypoint_buffer.at(boundary_start_index);
++boundary_start_index;
}
SimpleWaypointPtr boundary_end = nullptr;
SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index);
const auto vehicle_reference = boost::static_pointer_cast<cc::Vehicle>(actor);
// At non-signalized junctions, we extend the boundary across the junction
// and in all other situations, boundary length is velocity-dependent.
bool reached_distance = false;
for (uint64_t j = boundary_start_index; !reached_distance && (j < waypoint_buffer.size()); ++j) {
if (boundary_start->DistanceSquared(current_point) > std::pow(bbox_extension, 2)
|| j == waypoint_buffer.size() - 1) {
reached_distance = true;
}
if (boundary_end == nullptr
|| cg::Math::Dot(boundary_end->GetForwardVector(), current_point->GetForwardVector()) < COS_10_DEGREES
|| reached_distance) {
const cg::Vector3D heading_vector = current_point->GetForwardVector();
const cg::Location location = current_point->GetLocation();
cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, heading_vector.z);
// Direction determined for the left-handed system.
const cg::Vector3D scaled_perpendicular = perpendicular_vector * width;
left_boundary.push_back(location + cg::Location(scaled_perpendicular));
right_boundary.push_back(location + cg::Location(-1.0f * scaled_perpendicular));
boundary_end = current_point;
}
current_point = waypoint_buffer.at(j);
}
// Connecting the geodesic path boundary with the vehicle bounding box.
LocationList geodesic_boundary;
// Reversing right boundary to construct clockwise (left-hand system)
// boundary. This is so because both left and right boundary vectors have
// the closest point to the vehicle at their starting index for the right
// boundary,
// we want to begin at the farthest point to have a clockwise trace.
std::reverse(right_boundary.begin(), right_boundary.end());
geodesic_boundary.insert(geodesic_boundary.end(), right_boundary.begin(), right_boundary.end());
geodesic_boundary.insert(geodesic_boundary.end(), bbox.begin(), bbox.end());
geodesic_boundary.insert(geodesic_boundary.end(), left_boundary.begin(), left_boundary.end());
geodesic_boundaries.insert({actor->GetId(), geodesic_boundary});
return geodesic_boundary;
} else {
geodesic_boundaries.insert({actor->GetId(), bbox});
return bbox;
}
}
float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id,
const cg::Vector3D velocity_vector,
const cg::Vector3D heading_vector) {
const float velocity = cg::Math::Dot(velocity_vector, heading_vector);
float bbox_extension;
// Using a linear function to calculate boundary length.
// min speed : 0kmph -> BOUNDARY_EXTENSION_MINIMUM
// max speed : 100kmph -> BOUNDARY_EXTENSION_MAXIMUM
float slope = (BOUNDARY_EXTENSION_MAXIMUM - BOUNDARY_EXTENSION_MINIMUM) / ARBITRARY_MAX_SPEED;
bbox_extension = slope * velocity + BOUNDARY_EXTENSION_MINIMUM;
// If a valid collision lock present, change boundary length to maintain lock.
if (collision_locks.find(actor_id) != collision_locks.end())
{
CollisionLock &lock = collision_locks.at(actor_id);
float lock_boundary_length = static_cast<float>(lock.distance_to_lead_vehicle
+ LOCKING_DISTANCE_PADDING);
// Only extend boundary track vehicle if the leading vehicle
// if it is not further than velocity dependent extension by MAX_LOCKING_EXTENSION.
if ((lock_boundary_length - lock.initial_lock_distance) < MAX_LOCKING_EXTENSION)
{
bbox_extension = lock_boundary_length;
}
}
return bbox_extension;
}
LocationList CollisionStage::GetBoundary(const Actor &actor,
const cg::Location &location,
const cg::Vector3D velocity) {
const auto actor_type = actor->GetTypeId();
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
cg::BoundingBox bbox;
float forward_extension = 0.0f;
if (actor_type[0] == 'v') {
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
bbox = vehicle->GetBoundingBox();
} else if (actor_type[0] == 'w') {
const auto walker = boost::static_pointer_cast<cc::Walker>(actor);
bbox = walker->GetBoundingBox();
// Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
forward_extension = velocity.Length() * WALKER_TIME_EXTENSION;
}
const cg::Vector3D extent = bbox.extent;
const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, heading_vector.z);
const cg::Vector3D x_boundary_vector = heading_vector * (extent.x + forward_extension);
const cg::Vector3D y_boundary_vector = perpendicular_vector * (extent.y + forward_extension);
// Four corners of the vehicle in top view clockwise order (left-handed
// system).
LocationList bbox_boundary = {
location + cg::Location(x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
location + cg::Location(x_boundary_vector + y_boundary_vector),
};
return bbox_boundary;
}
bool CollisionStage::IsLocationAfterJunctionSafe(const Actor &ego_actor,
const Actor &other_actor,
const SimpleWaypointPtr safe_point,
const cg::Location &other_location,
const cg::Vector3D other_velocity){
bool safe_junction = true;
if (other_velocity.Length() < EPSILON_VELOCITY){
cg::Location safe_location = safe_point->GetLocation();
cg::Vector3D heading_vector = safe_point->GetForwardVector();
cg::BoundingBox bbox;
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(ego_actor);
bbox = vehicle->GetBoundingBox();
const cg::Vector3D extent = bbox.extent;
const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
const cg::Vector3D x_boundary_vector = heading_vector * extent.x;
const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y;
LocationList ego_actor_boundary = {
safe_location + cg::Location(x_boundary_vector - y_boundary_vector),
safe_location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
safe_location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
safe_location + cg::Location(x_boundary_vector + y_boundary_vector),
};
const Polygon reference_polygon = GetPolygon(ego_actor_boundary);
const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location, other_velocity));
const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
if (inter_bbox_distance < INTER_BBOX_DISTANCE_THRESHOLD){
safe_junction = false;
}
}
return safe_junction;
}
void CollisionStage::DrawBoundary(const LocationList &boundary) {
void CollisionStage::DrawBoundary(const LocationVector &boundary) {
cg::Location one_meter_up(0.0f, 0.0f, 1.0f);
for (uint64_t i = 0u; i < boundary.size(); ++i) {
debug_helper.DrawLine(
boundary[i] + cg::Location(0.0f, 0.0f, 1.0f),
boundary[(i + 1) % boundary.size()] + cg::Location(0.0f, 0.0f, 1.0f),
boundary[i] + one_meter_up,
boundary[i+1 == boundary.size()? 0: i+1] + one_meter_up,
0.1f, {255u, 255u, 0u}, 0.05f);
}
}
GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle,
const cg::Location &reference_location, const cg::Location &other_location,
const cg::Vector3D reference_velocity, const cg::Vector3D other_velocity) {
std::string actor_id_key = (reference_vehicle->GetId() < other_vehicle->GetId()) ?
std::to_string(reference_vehicle->GetId()) + "|" + std::to_string(other_vehicle->GetId())
: std::to_string(other_vehicle->GetId()) +"|"+ std::to_string(reference_vehicle->GetId());
GeometryComparisonCache mCache{-1,-1,-1,-1};
if (vehicle_cache.find(actor_id_key) != vehicle_cache.end()) {
mCache = vehicle_cache.at(actor_id_key);
double mref_veh_other = mCache.reference_vehicle_to_other_geodesic;
mCache.reference_vehicle_to_other_geodesic = mCache.other_vehicle_to_reference_geodesic;
mCache.other_vehicle_to_reference_geodesic = mref_veh_other;
return mCache;
}
const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location, reference_velocity));
const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location, other_velocity));
const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location, reference_velocity));
const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location, other_velocity));
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);
const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
GeometryComparisonCache mRetCache = {reference_vehicle_to_other_geodesic,
other_vehicle_to_reference_geodesic,
inter_geodesic_distance,
inter_bbox_distance};
vehicle_cache.insert({actor_id_key, mRetCache});
return mRetCache;
}
} // namespace traffic_manager
} // namespace carla

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

@ -1,166 +1,108 @@
// 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 <cmath>
#include <deque>
#include <stdlib.h>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>
#include <memory>
#include "boost/geometry.hpp"
#include "boost/geometry/geometries/geometries.hpp"
#include "boost/geometry/geometries/point_xy.hpp"
#include "boost/geometry/geometries/polygon.hpp"
#include "boost/pointer_cast.hpp"
#include "carla/client/ActorList.h"
#include "carla/client/Vehicle.h"
#include "carla/client/Walker.h"
#include "carla/client/World.h"
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/geom/Vector3D.h"
#include "carla/Logging.h"
#include "carla/rpc/ActorId.h"
#include "carla/rpc/TrafficLightState.h"
#include "carla/trafficmanager/MessengerAndDataTypes.h"
#include "carla/client/DebugHelper.h"
#include "carla/trafficmanager/DataStructures.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PipelineStage.h"
#include "carla/trafficmanager/RandomGenerator.h"
#include "carla/trafficmanager/SimulationState.h"
#include "carla/trafficmanager/Stage.h"
namespace carla {
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
namespace chr = std::chrono;
namespace bg = boost::geometry;
using ActorId = carla::ActorId;
using Actor = carla::SharedPtr<cc::Actor>;
using Point2D = bg::model::point<double, 2, bg::cs::cartesian>;
using Polygon = bg::model::polygon<bg::model::d2::point_xy<double>>;
using LocationList = std::vector<cg::Location>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using TLS = carla::rpc::TrafficLightState;
/// Structure to hold the Geometry of reference vehicle to other vehicle.
struct GeometryComparisonCache {
struct GeometryComparison {
double reference_vehicle_to_other_geodesic;
double other_vehicle_to_reference_geodesic;
double inter_geodesic_distance;
double inter_bbox_distance;
};
/// Structure to hold data about collision locking with a lead vehicle.
struct CollisionLock {
ActorId lead_vehicle_id;
double distance_to_lead_vehicle;
double initial_lock_distance;
ActorId lead_vehicle_id;
};
using CollisionLockMap = std::unordered_map<ActorId, CollisionLock>;
/// 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 {
namespace cc = carla::client;
namespace bg = boost::geometry;
using Buffer = std::deque<std::shared_ptr<SimpleWaypoint>>;
using BufferMap = std::unordered_map<carla::ActorId, Buffer>;
using LocationVector = std::vector<cg::Location>;
using GeodesicBoundaryMap = std::unordered_map<ActorId, LocationVector>;
using GeometryComparisonMap = std::unordered_map<uint64_t, GeometryComparison>;
using Polygon = bg::model::polygon<bg::model::d2::point_xy<double>>;
/// This class has functionality to detect potential collision with a nearby actor.
class CollisionStage : Stage {
private:
/// Geometry data for the vehicle
std::unordered_map<std::string, GeometryComparisonCache> vehicle_cache;
/// Selection key for switching between output frames.
bool frame_selector;
/// Pointer to data received from localization stage.
std::shared_ptr<LocalizationToCollisionFrame> localization_frame;
/// Pointers to output frames to be shared with motion planner stage.
std::shared_ptr<CollisionToPlannerFrame> planner_frame_a;
std::shared_ptr<CollisionToPlannerFrame> planner_frame_b;
/// Pointers to messenger objects.
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger;
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger;
/// Runtime parameterization object.
Parameters &parameters;
/// Reference to Carla's debug helper object.
const std::vector<ActorId> &vehicle_id_list;
const SimulationState &simulation_state;
const BufferMap &buffer_map;
const TrackTraffic &track_traffic;
const Parameters &parameters;
CollisionFrame &output_array;
cc::DebugHelper &debug_helper;
/// The map used to connect actor ids to the array index of data frames.
std::unordered_map<ActorId, uint64_t> vehicle_id_to_index;
/// An object used to keep track of time between checking for all world
/// actors.
chr::time_point<chr::system_clock, chr::nanoseconds> last_world_actors_pass_instance;
/// Number of vehicles registered with the traffic manager.
uint64_t number_of_vehicles;
/// Structure to hold the geodesic boundaries during one iteration.
std::unordered_map<ActorId, LocationList> geodesic_boundaries;
/// Structure to keep track of collision locking.
std::unordered_map<ActorId, CollisionLock> collision_locks;
/// Snippet profiler for measuring execution time.
SnippetProfiler snippet_profiler;
// Structure keeping track of blocking lead vehicles.
CollisionLockMap collision_locks;
// Structures to cache geodesic boundaries of vehicle and
// comparision between vehicle boundaries
// to avoid repeated computation within a cycle.
GeometryComparisonMap geometry_cache;
GeodesicBoundaryMap geodesic_boundary_map;
RandomGenerator<> pgen;
/// Returns the bounding box corners of the vehicle passed to the method.
LocationList GetBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity);
// Method to determine if a vehicle is on a collision path to another.
std::pair<bool, float> NegotiateCollision(const ActorId reference_vehicle_id,
const ActorId other_actor_id,
const uint64_t reference_junction_look_ahead_index);
/// Returns the extrapolated bounding box of the vehicle along its
/// trajectory.
LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity);
// Method to calculate bounding box extention length ahead of the vehicle.
float GetBoundingBoxExtention(const ActorId actor_id);
/// Method to construct a boost polygon object.
Polygon GetPolygon(const LocationList &boundary);
// Method to calculate polygon points around the vehicle's bounding box.
LocationVector GetBoundary(const ActorId actor_id);
/// The method returns true if ego_vehicle should stop and wait for
/// other_vehicle to pass.
std::pair<bool, float> 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,
const cg::Vector3D reference_velocity,
const cg::Vector3D other_velocity);
// Method to construct polygon points around the path boundary of the vehicle.
LocationVector GetGeodesicBoundary(const ActorId actor_id);
/// Method to calculate the speed dependent bounding box extention for a vehicle.
float GetBoundingBoxExtention(const ActorId actor_id,
const cg::Vector3D velocity_vector,
const cg::Vector3D heading_vector);
Polygon GetPolygon(const LocationVector &boundary);
/// 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, const cg::Location &other_location,
const cg::Vector3D other_velocity);
// Method to compare path boundaries, bounding boxes of vehicles
// and cache the results for reuse in current update cycle.
GeometryComparison GetGeometryBetweenActors(const ActorId reference_vehicle_id,
const ActorId other_actor_id);
/// A simple method used to draw bounding boxes around vehicles
void DrawBoundary(const LocationList &boundary);
/// Method to compute Geometry result between two vehicles
GeometryComparisonCache GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle,
const cg::Location &reference_location, const cg::Location &other_location,
const cg::Vector3D reference_velocity,const cg::Vector3D other_velocity);
// Method to draw path boundary.
void DrawBoundary(const LocationVector &boundary);
public:
CollisionStage(
std::string stage_name,
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
Parameters &parameters,
CollisionStage(const std::vector<ActorId> &vehicle_id_list,
const SimulationState &simulation_state,
const BufferMap &buffer_map,
const TrackTraffic &track_traffic,
const Parameters &parameters,
CollisionFrame &output_array,
cc::DebugHelper& debug_helper);
~CollisionStage();
void Update (const unsigned long index) override;
void DataReceiver() override;
void RemoveActor(const ActorId actor_id) override;
void Action() override;
void DataSender() override;
void Reset() override;
// Method to flush cache for current update cycle.
void ClearCycleCache();
};
} // namespace traffic_manager

132
LibCarla/source/carla/trafficmanager/Constants.h Normal file
View File

@ -0,0 +1,132 @@
/// This file contains various constants used in traffic manager
/// arranged into sensible namespaces for re-usability across
/// different files.
#pragma once
#include <limits>
#include <stdint.h>
#define SQUARE(a) ((a) * (a))
#define RATE(MaxY, MinY, DiffX) (((MaxY) - (MinY)) / (DiffX))
namespace carla {
namespace traffic_manager {
namespace constants {
namespace Networking {
static const uint64_t MIN_TRY_COUNT = 20u;
static const unsigned short TM_DEFAULT_PORT = 8000u;
static const int64_t TM_TIMEOUT = 2000; // ms
} // namespace Networking
namespace VehicleRemoval {
static const float STOPPED_VELOCITY_THRESHOLD = 0.8f;
static const double BLOCKED_TIME_THRESHOLD = 90.0;
static const double DELTA_TIME_BETWEEN_DESTRUCTIONS = 10.0;
} // namespace VehicleRemoval
namespace HybridMode {
static const float HYBRID_MODE_DT = 0.05f;
static const float INV_HYBRID_DT = 1.0f / HYBRID_MODE_DT;
static const float PHYSICS_RADIUS = 50.0f;
} // namespace HybridMode
namespace SpeedThreshold {
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f;
static const float AFTER_JUNCTION_MIN_SPEED = 5.0f / 3.6f;
} // namespace SpeedThreshold
namespace PathBufferUpdate {
static const float MAX_START_DISTANCE = 30.0f;
static const float MINIMUM_HORIZON_LENGTH = 30.0f;
static const float MAXIMUM_HORIZON_LENGTH = 60.0f;
static const float HORIZON_RATE = RATE(MAXIMUM_HORIZON_LENGTH,
MINIMUM_HORIZON_LENGTH,
SpeedThreshold::ARBITRARY_MAX_SPEED);
} // namespace PathBufferUpdate
namespace WaypointSelection {
static const float TARGET_WAYPOINT_TIME_HORIZON = 1.0f;
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 2.5f;
static const float JUNCTION_LOOK_AHEAD = 10.0f;
static const float SAFE_DISTANCE_AFTER_JUNCTION = 10.0f;
static const float MIN_JUNCTION_LENGTH = 5.0f;
static const float MIN_SAFE_INTERVAL_LENGTH = 0.9f * SAFE_DISTANCE_AFTER_JUNCTION;
} // namespace WaypointSelection
namespace LaneChange {
static const float MINIMUM_LANE_CHANGE_DISTANCE = 15.0f;
static const float MAXIMUM_LANE_OBSTACLE_DISTANCE = 50.0f;
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f;
static const float INTER_LANE_CHANGE_DISTANCE = 10.0f;
} // namespace LaneChange
namespace Collision {
static const float BOUNDARY_EXTENSION_MAXIMUM = 50.0f;
static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f;
static const float BOUNDARY_EXTENSION_RATE = RATE(BOUNDARY_EXTENSION_MAXIMUM,
BOUNDARY_EXTENSION_MINIMUM,
SpeedThreshold::ARBITRARY_MAX_SPEED);
static const float COS_10_DEGREES = 0.9848f;
static const float EPSILON_VELOCITY = 0.1f;
static const float LOCKING_DISTANCE_PADDING = 4.0f;
static const float MAX_COLLISION_RADIUS = 100.0f;
static const float MAX_LOCKING_EXTENSION = 10.0f;
static const float WALKER_TIME_EXTENSION = 1.5f;
static const float SQUARE_ROOT_OF_TWO = 1.414f;
static const float VERTICAL_OVERLAP_THRESHOLD = 4.0f;
} // namespace Collision
namespace FrameMemory {
static const uint64_t INITIAL_SIZE = 50u;
static const uint64_t GROWTH_STEP_SIZE = 50u;
static const float INV_GROWTH_STEP_SIZE = 1.0f / static_cast<float>(GROWTH_STEP_SIZE);
} // namespace FrameMemory
namespace Map {
static const float INFINITE_DISTANCE = std::numeric_limits<float>::max();
static const float GRID_SIZE = 4.0f;
static const float PED_GRID_SIZE = 10.0f;
static const float MAX_GEODESIC_GRID_LENGTH = 20.0f;
static const double MAP_RESOLUTION = 0.1;
static const float INV_MAP_RESOLUTION = 10.0f;
} // namespace Map
namespace TrafficLight {
static const uint64_t NO_SIGNAL_PASSTHROUGH_INTERVAL = 5u;
static const double DOUBLE_NO_SIGNAL_PASSTHROUGH_INTERVAL = 5.0;
} // namespace TrafficLight
namespace MotionPlan {
static const float RELATIVE_APPROACH_SPEED = 10.0f / 3.6f;
static const float MIN_FOLLOW_LEAD_DISTANCE = 5.0f;
static const float MAX_FOLLOW_LEAD_DISTANCE = 10.0f;
static const float FOLLOW_DISTANCE_RATE = RATE(MAX_FOLLOW_LEAD_DISTANCE,
MIN_FOLLOW_LEAD_DISTANCE,
SpeedThreshold::ARBITRARY_MAX_SPEED);
static const float CRITICAL_BRAKING_MARGIN = 0.25f;
static const float EPSILON_RELATIVE_SPEED = 0.001f;
static const float MAX_JUNCTION_BLOCK_DISTANCE = 0.5f * WaypointSelection::SAFE_DISTANCE_AFTER_JUNCTION;
} // namespace MotionPlan
namespace PID {
static const float MAX_THROTTLE = 0.7f;
static const float MAX_BRAKE = 1.0f;
static const float VELOCITY_INTEGRAL_MAX = 5.0f;
static const float VELOCITY_INTEGRAL_MIN = -5.0f;
static const float DT = 0.05f;
static const float INV_DT = 1.0f / DT;
} // namespace PID
namespace TrackTraffic {
static const uint64_t BUFFER_STEP_THROUGH = 10;
static const float INV_BUFFER_STEP_THROUGH = 0.1f;
} // namespace TrackTraffic
} // namespace constants
} // namespace traffic_manager
} // namespace carla

71
LibCarla/source/carla/trafficmanager/DataStructures.h Normal file
View File

@ -0,0 +1,71 @@
/// This file contains definitions of common data structures used in traffic manager.
#pragma once
#include <chrono>
#include <deque>
#include <vector>
#include "carla/client/Actor.h"
#include "carla/geom/Location.h"
#include "carla/geom/Rotation.h"
#include "carla/geom/Vector3D.h"
#include "carla/rpc/ActorId.h"
#include "carla/rpc/Command.h"
#include "carla/rpc/TrafficLightState.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {
namespace chr = std::chrono;
namespace cc = carla::client;
namespace cg = carla::geom;
using ActorId = carla::ActorId;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using JunctionID = carla::road::JuncId;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using Buffer = std::deque<SimpleWaypointPtr>;
using BufferMap = std::unordered_map<carla::ActorId, Buffer>;
using TimeInstance = chr::time_point<chr::system_clock, chr::nanoseconds>;
using TLS = carla::rpc::TrafficLightState;
struct LocalizationData {
SimpleWaypointPtr junction_end_point;
SimpleWaypointPtr safe_point;
bool is_at_junction_entrance;
};
using LocalizationFrame = std::vector<LocalizationData>;
struct CollisionHazardData {
float available_distance_margin;
ActorId hazard_actor_id;
bool hazard;
};
using CollisionFrame = std::vector<CollisionHazardData>;
using ControlFrame = std::vector<carla::rpc::Command>;
using TLFrame = std::vector<bool>;
/// Structure to hold the actuation signals.
struct ActuationSignal {
float throttle;
float brake;
float steer;
};
/// Structure to hold the controller state.
struct StateEntry {
TimeInstance time_instance;
float deviation;
float velocity;
float deviation_integral;
float velocity_integral;
};
} // namespace traffic_manager
} // namespace carla

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

@ -4,22 +4,18 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/InMemoryMap.h"
namespace carla {
namespace traffic_manager {
namespace MapConstants {
static const float INFINITE_DISTANCE = std::numeric_limits<float>::max();
static const float GRID_SIZE = 4.0f;
static const float PED_GRID_SIZE = 10.0f;
static const float MAX_GEODESIC_GRID_LENGTH = 20.0f;
static const float TOPOLOGY_DISTANCE = 0.1f;
} // namespace MapConstants
namespace cg = carla::geom;
using namespace MapConstants;
using namespace constants::Map;
using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
using RawNodeList = std::vector<WaypointPtr>;
InMemoryMap::InMemoryMap(WorldMap world_map) : _world_map(world_map) {}
InMemoryMap::~InMemoryMap() {}
@ -71,6 +67,7 @@ namespace MapConstants {
void InMemoryMap::SetUp() {
// 1. Building segment topology (i.e., defining set of segment predecessors and successors)
assert(_world_map != nullptr && "No map reference found.");
auto waypoint_topology = _world_map->GetTopology();
SegmentTopology segment_topology;
@ -84,39 +81,51 @@ namespace MapConstants {
// Setting segment predecessors and successors.
SegmentId waypoint_segment_id = GetSegmentId(connection.first);
SegmentId successor_segment_id = GetSegmentId(connection.second);
segment_topology[waypoint_segment_id].second.push_back(successor_segment_id);
segment_topology[successor_segment_id].first.push_back(waypoint_segment_id);
using SegIdVectorPair = std::pair<std::vector<SegmentId>, std::vector<SegmentId>>;
SegIdVectorPair &connection_first = segment_topology[waypoint_segment_id];
SegIdVectorPair &connection_second = segment_topology[successor_segment_id];
connection_first.second.push_back(successor_segment_id);
connection_second.first.push_back(waypoint_segment_id);
// From path to standard road.
if (waypoint->IsJunction() && !successor->IsJunction()) {
bool waypoint_is_junction = waypoint->IsJunction();
bool successor_is_junction = successor->IsJunction();
if (waypoint_is_junction && !successor_is_junction) {
crd::RoadId path_id = waypoint->GetRoadId();
int64_t std_road_id = static_cast<int64_t>(successor->GetRoadId());
std_road_id = (successor->GetLaneId() < 0) ? -1 * std_road_id : std_road_id;
std_road_connectivity[std_road_id].first.insert(path_id);
auto &in_paths = std_road_connectivity[std_road_id].first;
std::set<crd::RoadId> &in_paths = std_road_connectivity[std_road_id].first;
in_paths.insert(path_id);
if (in_paths.size() >= 2) {
std::for_each(in_paths.begin(), in_paths.end(), [&is_real_junction](crd::RoadId id) {is_real_junction[id] = true;});
for (auto &in_path_id: in_paths) {
is_real_junction[in_path_id] = true;
}
}
}
// From standard road to path.
if (!waypoint->IsJunction() && successor->IsJunction()) {
if (!waypoint_is_junction && successor_is_junction) {
crd::RoadId path_id = successor->GetRoadId();
int64_t std_road_id = static_cast<int64_t>(waypoint->GetRoadId());
std_road_id = (waypoint->GetLaneId() < 0) ? -1 * std_road_id : std_road_id;
std_road_connectivity[std_road_id].second.insert(path_id);
auto &out_paths = std_road_connectivity[std_road_id].second;
std::set<crd::RoadId> &out_paths = std_road_connectivity[std_road_id].second;
out_paths.insert(path_id);
if (out_paths.size() >= 2) {
std::for_each(out_paths.begin(), out_paths.end(), [&is_real_junction](crd::RoadId id) {is_real_junction[id] = true;});
for (auto &out_path_id: out_paths) {
is_real_junction[out_path_id] = true;
}
}
}
}
// 2. Consuming the raw dense topology from cc::Map into SimpleWaypoints.
SegmentMap segment_map;
auto raw_dense_topology = _world_map->GenerateWaypoints(TOPOLOGY_DISTANCE);
assert(_world_map != nullptr && "No map reference found.");
auto raw_dense_topology = _world_map->GenerateWaypoints(MAP_RESOLUTION);
for (auto &waypoint_ptr: raw_dense_topology) {
segment_map[GetSegmentId(waypoint_ptr)].emplace_back(std::make_shared<SimpleWaypoint>(waypoint_ptr));
}
@ -180,13 +189,13 @@ namespace MapConstants {
for (auto &simple_waypoint: dense_topology) {
if (simple_waypoint != nullptr) {
const cg::Location loc = simple_waypoint->GetLocation();
const std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, true));
const int64_t grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, true));
if (waypoint_grid.find(grid_key) == waypoint_grid.end()) {
waypoint_grid.insert({grid_key, {simple_waypoint}});
} else {
waypoint_grid.at(grid_key).insert(simple_waypoint);
}
const std::string ped_grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, false));
const int64_t ped_grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, false));
if (ped_waypoint_grid.find(ped_grid_key) == ped_waypoint_grid.end()) {
ped_waypoint_grid.insert({ped_grid_key, {simple_waypoint}});
} else {
@ -230,8 +239,6 @@ namespace MapConstants {
}
}
}
MakeGeodesiGridCenters();
}
std::pair<int, int> InMemoryMap::MakeGridId(float x, float y, bool vehicle_or_pedestrian) {
@ -243,8 +250,14 @@ 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);
int64_t InMemoryMap::MakeGridKey(std::pair<int, int> grid_id) {
int64_t grid_key = 0;
grid_key |= grid_id.first;
grid_key <<= 32;
grid_key |= grid_id.second;
return grid_key;
}
SimpleWaypointPtr InMemoryMap::GetWaypointInVicinity(cg::Location location) {
@ -257,7 +270,7 @@ namespace MapConstants {
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j});
const int64_t grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j});
if (waypoint_grid.find(grid_key) != waypoint_grid.end()) {
const auto &waypoint_set = waypoint_grid.at(grid_key);
@ -267,7 +280,7 @@ namespace MapConstants {
for (auto &simple_waypoint: waypoint_set) {
if (simple_waypoint->DistanceSquared(location) < std::pow(closest_distance, 2)) {
if (simple_waypoint->DistanceSquared(location) < SQUARE(closest_distance)) {
closest_waypoint = simple_waypoint;
closest_distance = simple_waypoint->DistanceSquared(location);
}
@ -278,7 +291,7 @@ namespace MapConstants {
// Return the closest waypoint in the surrounding grids
// only if it is in the same horizontal plane as the requested location.
if (closest_waypoint != nullptr && std::abs(closest_waypoint->GetLocation().z - location.z) > 1.0) {
if (closest_waypoint != nullptr && std::abs(closest_waypoint->GetLocation().z - location.z) > 1.0f) {
closest_waypoint = nullptr;
}
@ -295,7 +308,7 @@ namespace MapConstants {
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j});
const int64_t grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j});
if (ped_waypoint_grid.find(grid_key) != ped_waypoint_grid.end()) {
const auto &waypoint_set = ped_waypoint_grid.at(grid_key);
@ -417,26 +430,9 @@ namespace MapConstants {
}
}
void InMemoryMap::MakeGeodesiGridCenters() {
for (auto &swp: dense_topology) {
GeoGridId ggid = swp->CheckJunction() ? swp->GetJunctionId() : swp->GetGeodesicGridId();
if (geodesic_grid_center.find(ggid) == geodesic_grid_center.end()) {
geodesic_grid_center.insert({ggid, swp->GetLocation()});
} else {
cg::Location &grid_loc = geodesic_grid_center.at(ggid);
grid_loc = (grid_loc + swp->GetLocation()) / 2;
}
}
}
cg::Location InMemoryMap::GetGeodesicGridCenter(GeoGridId ggid) {
cg::Location grid_center;
if (geodesic_grid_center.find(ggid) != geodesic_grid_center.end()) {
grid_center = geodesic_grid_center.at(ggid);
} else {
grid_center = cg::Location();
}
return grid_center;
std::string InMemoryMap::GetMapName() {
assert(_world_map != nullptr && "No map reference found.");
return _world_map->GetName();
}
} // namespace traffic_manager

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

@ -6,19 +6,15 @@
#pragma once
#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include "carla/client/Map.h"
#include "carla/client/Waypoint.h"
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/Memory.h"
#include "carla/road/Lane.h"
#include "carla/road/RoadTypes.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
@ -31,13 +27,11 @@ namespace cc = carla::client;
namespace crd = carla::road;
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using NodeList = std::vector<SimpleWaypointPtr>;
using RawNodeList = std::vector<WaypointPtr>;
using GeoGridId = crd::JuncId;
using WorldMap = carla::SharedPtr<cc::Map>;
using WaypointGrid = std::unordered_map<std::string, std::unordered_set<SimpleWaypointPtr>>;
using WaypointGrid = std::unordered_map<int64_t, std::unordered_set<SimpleWaypointPtr>>;
using SegmentId = std::tuple<crd::RoadId, crd::LaneId, crd::SectionId>;
using SegmentTopology = std::map<SegmentId, std::pair<std::vector<SegmentId>, std::vector<SegmentId>>>;
@ -71,12 +65,6 @@ namespace crd = carla::road;
/// sampling_resolution.
void SetUp();
/// Computes the segment id of a given waypoint.
///
/// The Id takes into account OpenDrive's road Id, lane Id and Section Id.
SegmentId GetSegmentId(const WaypointPtr &wp) const;
SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const;
/// This method returns the closest waypoint to a given location on the map.
SimpleWaypointPtr GetWaypoint(const cg::Location &location) const;
@ -88,8 +76,7 @@ namespace crd = carla::road;
/// local cache.
NodeList GetDenseTopology() const;
void MakeGeodesiGridCenters();
cg::Location GetGeodesicGridCenter(GeoGridId ggid);
std::string GetMapName();
private:
@ -97,7 +84,7 @@ namespace crd = carla::road;
std::pair<int, int> MakeGridId(float x, float y, bool vehicle_or_pedestrian);
/// Method to generate map key for waypoint_grid.
std::string MakeGridKey(std::pair<int, int> gird_id);
int64_t MakeGridKey(std::pair<int, int> gird_id);
/// This method is used to find and place lane change links.
void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);
@ -106,6 +93,11 @@ namespace crd = carla::road;
const SegmentTopology &segment_topology, const SegmentMap &segment_map);
std::vector<SimpleWaypointPtr> GetPredecessors(const SegmentId segment_id,
const SegmentTopology &segment_topology, const SegmentMap &segment_map);
/// Computes the segment id of a given waypoint.
/// The Id takes into account OpenDrive's road Id, lane Id and Section Id.
SegmentId GetSegmentId(const WaypointPtr &wp) const;
SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const;
};
} // namespace traffic_manager

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

File diff suppressed because it is too large Load Diff

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

@ -1,205 +1,73 @@
// 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 <chrono>
#include <cmath>
#include <ctime>
#include <deque>
#include <memory>
#include <mutex>
#include <tuple>
#include <unordered_map>
#include "carla/StringUtil.h"
#include "carla/client/DebugHelper.h"
#include "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/geom/Transform.h"
#include "carla/geom/Vector3D.h"
#include "carla/Memory.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/AtomicActorSet.h"
#include "carla/trafficmanager/DataStructures.h"
#include "carla/trafficmanager/InMemoryMap.h"
#include "carla/trafficmanager/LocalizationUtils.h"
#include "carla/trafficmanager/MessengerAndDataTypes.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PipelineStage.h"
#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"
#include "carla/trafficmanager/RandomGenerator.h"
#include "carla/trafficmanager/TrackTraffic.h"
#include "carla/trafficmanager/SimulationState.h"
#include "carla/trafficmanager/Stage.h"
namespace carla {
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
namespace chr = std::chrono;
using namespace chr;
using Actor = carla::SharedPtr<cc::Actor>;
using Vehicle = carla::SharedPtr<cc::Vehicle>;
using ActorId = carla::ActorId;
using ActorIdSet = std::unordered_set<ActorId>;
using TLS = carla::rpc::TrafficLightState;
/// Structure to hold kinematic state of actors.
struct KinematicState {
bool physics_enabled;
cg::Location location;
cg::Vector3D velocity;
};
using LocalMapPtr = std::shared_ptr<InMemoryMap>;
using LaneChangeLocationMap = std::unordered_map<ActorId, cg::Location>;
/// This class is responsible for maintaining a horizon of waypoints ahead
/// This class has functionality to maintain a horizon of waypoints ahead
/// of the vehicle for it to follow.
/// The class is also responsible for managing lane change decisions and
/// modify the waypoint trajectory appropriately.
class LocalizationStage : public PipelineStage {
class LocalizationStage : Stage {
private:
/// Section keys to switch between the output data frames.
bool planner_frame_selector;
bool collision_frame_selector;
bool traffic_light_frame_selector;
/// Output data frames to be shared with the motion planner stage.
std::shared_ptr<LocalizationToPlannerFrame> planner_frame_a;
std::shared_ptr<LocalizationToPlannerFrame> planner_frame_b;
/// Output data frames to be shared with the collision stage.
std::shared_ptr<LocalizationToCollisionFrame> collision_frame_a;
std::shared_ptr<LocalizationToCollisionFrame> collision_frame_b;
/// Output data frames to be shared with the traffic light stage
std::shared_ptr<LocalizationToTrafficLightFrame> traffic_light_frame_a;
std::shared_ptr<LocalizationToTrafficLightFrame> traffic_light_frame_b;
/// Pointer to messenger to motion planner stage.
std::shared_ptr<LocalizationToPlannerMessenger> planner_messenger;
/// Pointer to messenger to collision stage.
std::shared_ptr<LocalizationToCollisionMessenger> collision_messenger;
/// Pointer to messenger to traffic light stage.
std::shared_ptr<LocalizationToTrafficLightMessenger> traffic_light_messenger;
/// Reference to set of all actors registered with the traffic manager.
AtomicActorSet &registered_actors;
/// List of actors registered with the traffic manager in
/// current update cycle.
std::vector<Actor> actor_list;
/// State counter to track changes in registered actors.
int registered_actors_state;
/// Reference to local map-cache object.
InMemoryMap &local_map;
/// Runtime parameterization object.
const std::vector<ActorId> &vehicle_id_list;
BufferMap &buffer_map;
const SimulationState &simulation_state;
TrackTraffic &track_traffic;
const LocalMapPtr &local_map;
Parameters &parameters;
/// Reference to Carla's debug helper object.
LocalizationFrame &output_array;
cc::DebugHelper &debug_helper;
/// 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;
/// Map connecting actor ids to indices of data arrays.
std::unordered_map<ActorId, uint64_t> vehicle_id_to_index;
/// Number of vehicles currently registered with the traffic manager.
uint64_t number_of_vehicles;
/// Used to only calculate the extended buffer once at junctions
std::map<carla::ActorId, bool> approached;
/// Point used to know if the junction has free space after its end, mapped to their respective actor id
std::map<carla::ActorId, SimpleWaypointPtr> final_safe_points;
/// Object for tracking paths of the traffic vehicles.
TrackTraffic track_traffic;
/// Map of all vehicles' idle time.
std::unordered_map<ActorId, double> idle_time;
/// Structure to hold the actor with the maximum idle time at each iteration.
std::pair<Actor, double> maximum_idle_time;
/// Variable to hold current timestamp from the world snapshot.
cc::Timestamp current_timestamp;
/// Simulated seconds since the beginning of the current episode when the last actor was destroyed.
double elapsed_last_actor_destruction = 0.0;
/// Counter to track unregistered actors' scan interval.
uint64_t unregistered_scan_duration = 0;
/// A structure used to keep track of actors spawned outside of traffic
/// manager.
std::unordered_map<ActorId, Actor> unregistered_actors;
/// Code snippet execution time profiler.
SnippetProfiler snippet_profiler;
/// Map to keep track of last lane change location.
std::unordered_map<ActorId, cg::Location> last_lane_change_location;
/// Records of all vehicles with hero attribute.
std::unordered_map<ActorId, Actor> hero_actors;
/// Switch indicating hybrid physics mode.
bool hybrid_physics_mode {false};
/// Switch indicating hybrid physics mode.
float hybrid_physics_radius {70.0f};
/// Structure to hold previous state of physics-less vehicle.
std::unordered_map<ActorId, KinematicState> kinematic_state_map;
/// Time instance used to calculate dt in asynchronous mode.
TimePoint previous_update_instance;
/// Step runner flag.
std::atomic<bool> run_step {false};
/// Mutex for progressing synchronous execution.
std::mutex step_execution_mutex;
/// Condition variables for progressing synchronous execution.
std::condition_variable step_execution_trigger;
LaneChangeLocationMap last_lane_change_location;
ActorIdSet vehicles_at_junction;
using SimpleWaypointPair = std::pair<SimpleWaypointPtr, SimpleWaypointPtr>;
std::unordered_map<ActorId, SimpleWaypointPair> vehicles_at_junction_entrance;
RandomGenerator<> pgen;
SimpleWaypointPtr AssignLaneChange(const ActorId actor_id,
const cg::Location vehicle_location,
const float vehicle_speed,
bool force, bool direction);
/// A simple method used to draw waypoint buffer ahead of a vehicle.
void DrawBuffer(Buffer &buffer);
/// Method to determine lane change and obtain target lane waypoint.
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
SimpleWaypointPtr GetSafeLocationAfterJunction(const Vehicle &vehicle, Buffer &waypoint_buffer);
/// Methods to modify waypoint buffer and track traffic.
void PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint);
void PopWaypoint(Buffer& buffer, ActorId actor_id, bool front_or_back = true);
/// Method to scan for unregistered actors and update their grid positioning.
void ScanUnregisteredVehicles();
/// Methods for idle vehicle elimination.
void UpdateIdleTime(const Actor& actor);
bool IsVehicleStuck(const Actor& actor);
void CleanActor(const ActorId actor_id);
bool TryDestroyVehicle(const Actor& actor);
/// Methods for actor state management.
void UpdateSwarmVelocities();
cg::Vector3D GetVelocity(ActorId actor_id);
bool IsPhysicsEnabled(ActorId actor_id);
void ExtendAndFindSafeSpace(const ActorId actor_id,
const bool is_at_junction_entrance,
Buffer &waypoint_buffer);
public:
LocalizationStage(
std::string stage_name,
std::shared_ptr<LocalizationToPlannerMessenger> planner_messenger,
std::shared_ptr<LocalizationToCollisionMessenger> collision_messenger,
std::shared_ptr<LocalizationToTrafficLightMessenger> traffic_light_messenger,
AtomicActorSet &registered_actors,
InMemoryMap &local_map,
LocalizationStage(const std::vector<ActorId> &vehicle_id_list,
BufferMap &buffer_map,
const SimulationState &simulation_state,
TrackTraffic &track_traffic,
const LocalMapPtr &local_map,
Parameters &parameters,
carla::client::DebugHelper &debug_helper,
carla::client::detail::EpisodeProxy &episodeProxy);
LocalizationFrame &output_array,
cc::DebugHelper &debug_helper);
~LocalizationStage();
void Update(const unsigned long index) override;
void DataReceiver() override;
void RemoveActor(const ActorId actor_id) override;
void Action() override;
void DataSender() override;
/// Method to trigger initiation of pipeline in synchronous mode.
bool RunStep();
void Reset() override;
};
} // namespace traffic_manager

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

@ -9,244 +9,73 @@
namespace carla {
namespace traffic_manager {
namespace LocalizationConstants {
const static uint64_t BUFFER_STEP_THROUGH = 10u;
const static float SAMPLING_RESOLUTION = 0.1f;
} // namespace LocalizationConstants
using namespace LocalizationConstants;
float DeviationCrossProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location) {
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
cg::Location next_vector = target_location - vehicle_location;
next_vector = next_vector.MakeSafeUnitVector(2 * std::numeric_limits<float>::epsilon());
float DeviationCrossProduct(const cg::Location &reference_location,
const cg::Vector3D &heading_vector,
const cg::Location &target_location) {
cg::Vector3D next_vector = target_location - reference_location;
float vector_magnitude_epsilon = 2.0f * std::numeric_limits<float>::epsilon();
next_vector = next_vector.MakeSafeUnitVector(vector_magnitude_epsilon);
const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x;
return cross_z;
}
float DeviationDotProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location, bool rear_offset) {
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
cg::Location next_vector;
if (!rear_offset) {
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_location);
}
next_vector = next_vector.MakeSafeUnitVector(2 * std::numeric_limits<float>::epsilon());
const float dot_product = cg::Math::Dot(next_vector, heading_vector);
float DeviationDotProduct(const cg::Location &reference_location,
const cg::Vector3D &heading_vector,
const cg::Location &target_location) {
cg::Vector3D next_vector = target_location - reference_location;
float vector_magnitude_epsilon = 2.0f * std::numeric_limits<float>::epsilon();
next_vector.z = 0.0f;
next_vector = next_vector.MakeSafeUnitVector(vector_magnitude_epsilon);
cg::Vector3D heading_vector_flat(heading_vector.x, heading_vector.y, 0);
heading_vector_flat = heading_vector_flat.MakeSafeUnitVector(vector_magnitude_epsilon);
const float dot_product = cg::Math::Dot(next_vector, heading_vector_flat);
return dot_product;
}
TrackTraffic::TrackTraffic() {}
void PushWaypoint(ActorId actor_id, TrackTraffic &track_traffic,
Buffer &buffer, SimpleWaypointPtr &waypoint) {
void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id,
const std::vector<SimpleWaypointPtr> waypoints) {
// Add actor entry, if not present.
if (actor_to_grids.find(actor_id) == actor_to_grids.end()) {
actor_to_grids.insert({actor_id, {}});
const uint64_t waypoint_id = waypoint->GetId();
buffer.push_back(waypoint);
track_traffic.UpdatePassingVehicle(waypoint_id, actor_id);
}
std::unordered_set<GeoGridId>& current_grids = actor_to_grids.at(actor_id);
void PopWaypoint(ActorId actor_id, TrackTraffic &track_traffic,
Buffer &buffer, bool front_or_back) {
// Clear current actor from all grids containing itself.
for (auto& grid_id: current_grids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_ids = grid_to_actors.at(grid_id);
if (actor_ids.find(actor_id) != actor_ids.end()) {
actor_ids.erase(actor_id);
}
}
}
// Clear all grids current actor is tracking.
current_grids.clear();
// Step through buffer and update grid list for actor and actor list for grids.
for (auto &waypoint: waypoints) {
GeoGridId ggid = waypoint->GetGeodesicGridId();
current_grids.insert(ggid);
// Add grid entry, if not present.
if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
grid_to_actors.insert({ggid, {}});
}
ActorIdSet& actor_ids = grid_to_actors.at(ggid);
if (actor_ids.find(actor_id) == actor_ids.end()) {
actor_ids.insert(actor_id);
}
}
}
void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer& buffer) {
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 itself.
for (auto& grid_id: current_grids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_ids = grid_to_actors.at(grid_id);
if (actor_ids.find(actor_id) != actor_ids.end()) {
actor_ids.erase(actor_id);
}
}
}
// Clear all grids the current actor is tracking.
current_grids.clear();
// Step through buffer and update grid list for actor and actor list for grids.
uint64_t buffer_size = buffer.size();
uint64_t step_size = static_cast<uint64_t>(std::floor(buffer_size/BUFFER_STEP_THROUGH));
for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) {
GeoGridId ggid = buffer.at(std::min(i* step_size, buffer_size-1u))->GetGeodesicGridId();
current_grids.insert(ggid);
// Add grid entry if not present.
if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
grid_to_actors.insert({ggid, {}});
}
ActorIdSet& actor_ids = grid_to_actors.at(ggid);
if (actor_ids.find(actor_id) == actor_ids.end()) {
actor_ids.insert(actor_id);
}
}
}
}
ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) {
ActorIdSet actor_id_set;
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
std::unordered_set<GeoGridId>& grid_ids = actor_to_grids.at(actor_id);
for (auto& grid_id: grid_ids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_ids = grid_to_actors.at(grid_id);
actor_id_set.insert(actor_ids.begin(), actor_ids.end());
}
}
}
return actor_id_set;
}
void TrackTraffic::DeleteActor(ActorId actor_id) {
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
std::unordered_set<GeoGridId>& grid_ids = actor_to_grids.at(actor_id);
for (auto& grid_id: grid_ids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_ids = grid_to_actors.at(grid_id);
if (actor_ids.find(actor_id) != actor_ids.end()) {
actor_ids.erase(actor_id);
}
}
}
actor_to_grids.erase(actor_id);
}
}
std::unordered_set<GeoGridId> TrackTraffic::GetGridIds(ActorId actor_id) {
std::unordered_set<GeoGridId> grid_ids;
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
grid_ids = actor_to_grids.at(actor_id);
}
return grid_ids;
}
std::unordered_map<GeoGridId, ActorIdSet> TrackTraffic::GetGridActors() {
return grid_to_actors;
}
void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
ActorIdSet& actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
if (actor_id_set.find(actor_id) == actor_id_set.end()) {
actor_id_set.insert(actor_id);
}
SimpleWaypointPtr removed_waypoint = front_or_back ? buffer.front() : buffer.back();
const uint64_t removed_waypoint_id = removed_waypoint->GetId();
if (front_or_back) {
buffer.pop_front();
} else {
waypoint_overlap_tracker.insert({waypoint_id, {actor_id}});
buffer.pop_back();
}
track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id);
}
void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
auto& actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
if (actor_id_set.find(actor_id) != actor_id_set.end()) {
actor_id_set.erase(actor_id);
}
if (actor_id_set.size() == 0) {
waypoint_overlap_tracker.erase(waypoint_id);
}
}
}
ActorIdSet TrackTraffic::GetPassingVehicles(uint64_t waypoint_id) {
if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
return waypoint_overlap_tracker.at(waypoint_id);
} else {
return ActorIdSet();
}
}
std::pair<SimpleWaypointPtr,uint64_t> TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) {
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance) {
SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
const SimpleWaypointPtr &buffer_front = waypoint_buffer.front();
uint64_t startPosn = static_cast<uint64_t>(std::fabs(target_point_distance/SAMPLING_RESOLUTION));
uint64_t startPosn = static_cast<uint64_t>(std::fabs(target_point_distance * INV_MAP_RESOLUTION));
uint64_t index = 0;
/// Condition to determine forward or backward scanning of WayPoint Buffer.
if (startPosn < waypoint_buffer.size()) {
bool mScanForward = false;
double target_point_dist_power = std::pow(target_point_distance, 2);
const float target_point_dist_power = target_point_distance * target_point_distance;
if (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power) {
mScanForward = true;
}
if (mScanForward) {
for (uint64_t i = startPosn;
(i < waypoint_buffer.size())
&& (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power);
(i < waypoint_buffer.size()) && (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power);
++i) {
target_waypoint = waypoint_buffer.at(i);
index = i;
}
}
else {
} else {
for (uint64_t i = startPosn;
(buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power);
--i) {
@ -254,8 +83,7 @@ std::pair<SimpleWaypointPtr,uint64_t> TrackTraffic::GetTargetWaypoint(const Buff
index = i;
}
}
}
else {
} else {
target_waypoint = waypoint_buffer.back();
index = waypoint_buffer.size() - 1;
}

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

@ -14,7 +14,9 @@
#include "carla/road/RoadTypes.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
#include "carla/trafficmanager/TrackTraffic.h"
namespace carla {
namespace traffic_manager {
@ -27,50 +29,32 @@ namespace traffic_manager {
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using Buffer = std::deque<SimpleWaypointPtr>;
using GeoGridId = carla::road::JuncId;
class TrackTraffic{
private:
/// Structure to keep track of overlapping waypoints between vehicles.
using WaypointOverlap = std::unordered_map<uint64_t, ActorIdSet>;
WaypointOverlap waypoint_overlap_tracker;
/// Stored vehicle id set record.
ActorIdSet actor_id_set_record;
/// Geodesic grids occupied by actors's paths.
std::unordered_map<ActorId, std::unordered_set<GeoGridId>> actor_to_grids;
/// Actors currently passing through grids.
std::unordered_map<GeoGridId, ActorIdSet> grid_to_actors;
public:
TrackTraffic();
/// Methods to update, remove and retrieve vehicles passing through a waypoint.
void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
ActorIdSet GetPassingVehicles(uint64_t waypoint_id);
void UpdateGridPosition(const ActorId actor_id, const Buffer& buffer);
void UpdateUnregisteredGridPosition(const ActorId actor_id,
const std::vector<SimpleWaypointPtr> waypoints);
/// Method to return the wayPoints from the waypoint Buffer by using target point distance
std::pair<SimpleWaypointPtr,uint64_t> GetTargetWaypoint(const Buffer& waypoint_buffer, const float& target_point_distance);
ActorIdSet GetOverlappingVehicles(ActorId actor_id);
/// Method to delete actor data from tracking.
void DeleteActor(ActorId actor_id);
std::unordered_set<GeoGridId> GetGridIds(ActorId actor_id);
std::unordered_map<GeoGridId, ActorIdSet> GetGridActors();
};
using constants::Map::MAP_RESOLUTION;
using constants::Map::INV_MAP_RESOLUTION;
/// 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 &vehicle_location, const cg::Location &target_location);
float DeviationCrossProduct(const cg::Location &reference_location,
const cg::Vector3D &heading_vector,
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 &vehicle_location, const cg::Location &target_location, bool rear_offset=false);
float DeviationDotProduct(const cg::Location &reference_location,
const cg::Vector3D &heading_vector,
const cg::Location &target_location);
// Function to add a waypoint to a path buffer and update waypoint tracking.
void PushWaypoint(ActorId actor_id, TrackTraffic& track_traffic,
Buffer& buffer, SimpleWaypointPtr& waypoint);
// Function to remove a waypoint from a path buffer and update waypoint tracking.
void PopWaypoint(ActorId actor_id, TrackTraffic& track_traffic,
Buffer& buffer, bool front_or_back=true);
/// Method to return the wayPoints from the waypoint Buffer by using target point distance
using TargetWPInfo = std::pair<SimpleWaypointPtr,uint64_t>;
TargetWPInfo GetTargetWaypoint(const Buffer& waypoint_buffer, const float& target_point_distance);
} // namespace traffic_manager
} // namespace carla

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

@ -1,96 +0,0 @@
// 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 <atomic>
#include <chrono>
#include <deque>
#include <condition_variable>
#include <mutex>
namespace carla {
namespace traffic_manager {
using namespace std::chrono_literals;
template <typename Data>
class Messenger {
private:
/// Flag used to wake up and join any waiting function calls on this object.
std::atomic<bool> stop_messenger;
/// Member used to hold data sent by the sender.
std::deque<Data> d_queue;
/// Mutex used to manage contention between the sender and receiver.
std::mutex data_modification_mutex;
/// Variable to conditionally block sender in case waiting for the receiver.
std::condition_variable send_condition;
/// Variable to conditionally block receiver in case waiting for the sender.
std::condition_variable receive_condition;
public:
Messenger() {
stop_messenger.store(false);
}
~Messenger() {}
void Push(Data data) {
std::unique_lock<std::mutex> lock(data_modification_mutex);
while (!d_queue.empty() && !stop_messenger.load()) {
send_condition.wait_for(lock, 1ms, [=] {
return (d_queue.empty() && stop_messenger.load());
});
}
if(!stop_messenger.load()){
d_queue.push_front(data);
receive_condition.notify_one();
}
}
Data Peek() {
std::unique_lock<std::mutex> lock(data_modification_mutex);
while (d_queue.empty() && !stop_messenger.load()) {
receive_condition.wait_for(lock, 1ms, [=] {
return (!d_queue.empty() && stop_messenger.load());
});
}
if(!stop_messenger.load()) {
Data data = d_queue.back();
return data;
}
return Data();
}
void Pop() {
std::unique_lock<std::mutex> lock(data_modification_mutex);
if (!(d_queue.empty() && stop_messenger.load())) {
d_queue.pop_back();
send_condition.notify_one();
}
}
void Start() {
stop_messenger.store(false);
}
void Stop() {
stop_messenger.store(true);
d_queue.clear();
send_condition.notify_one();
receive_condition.notify_one();
}
};
} // namespace traffic_manager
} // namespace carla

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

@ -1,113 +0,0 @@
// 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 <tuple>
#include <unordered_map>
#include <vector>
#include "carla/client/Actor.h"
#include "carla/geom/Vector3D.h"
#include "carla/geom/Transform.h"
#include "carla/Memory.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/Messenger.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
/// Convenience typing.
/// Alias for waypoint buffer used in the localization stage.
using Buffer = std::deque<std::shared_ptr<SimpleWaypoint>>;
/// Alias used for the list of buffers in the localization stage.
using BufferList = std::unordered_map<carla::ActorId, Buffer>;
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
/// Data types.
/// Type of data sent by the localization stage to the motion planner stage.
struct LocalizationToPlannerData {
Actor actor;
float deviation;
float distance;
bool approaching_true_junction;
cg::Vector3D velocity;
bool physics_enabled;
std::vector<std::shared_ptr<SimpleWaypoint>> position_window;
};
/// Type of data sent by the motion planner stage to the batch control stage.
struct PlannerToControlData {
Actor actor;
float throttle;
float brake;
float steer;
bool physics_enabled;
cg::Transform transform;
};
/// Type of data sent by the localization stage to the collision stage.
struct LocalizationToCollisionData {
Actor actor;
Buffer buffer;
std::vector<std::tuple<ActorId, Actor, cg::Vector3D>> overlapping_actors;
std::shared_ptr<SimpleWaypoint> safe_point_after_junction;
std::shared_ptr<SimpleWaypoint> closest_waypoint;
std::shared_ptr<SimpleWaypoint> junction_look_ahead_waypoint;
cg::Vector3D velocity;
};
/// Type of data sent by the collision stage to the motion planner stage.
struct CollisionToPlannerData {
bool hazard;
float distance_to_other_vehicle;
cg::Vector3D other_vehicle_velocity;
};
/// Type of data sent by the localization stage to the traffic light stage.
struct LocalizationToTrafficLightData {
Actor actor;
std::shared_ptr<SimpleWaypoint> closest_waypoint;
std::shared_ptr<SimpleWaypoint> junction_look_ahead_waypoint;
};
/// Type of data sent by the traffic light stage to the motion planner stage.
struct TrafficLightToPlannerData {
bool traffic_light_hazard;
};
/// Data frame types.
/// Array types of data flowing between stages.
using LocalizationToPlannerFrame = std::vector<LocalizationToPlannerData>;
using PlannerToControlFrame = std::vector<PlannerToControlData>;
using LocalizationToCollisionFrame = std::vector<LocalizationToCollisionData>;
using LocalizationToTrafficLightFrame = std::vector<LocalizationToTrafficLightData>;
using CollisionToPlannerFrame = std::vector<CollisionToPlannerData>;
using TrafficLightToPlannerFrame = std::vector<TrafficLightToPlannerData>;
/// Messenger types
using LocalizationToPlannerMessenger = Messenger<std::shared_ptr<LocalizationToPlannerFrame>>;
using PlannerToControlMessenger = Messenger<std::shared_ptr<PlannerToControlFrame>>;
using LocalizationToCollisionMessenger = Messenger<std::shared_ptr<LocalizationToCollisionFrame>>;
using LocalizationToTrafficLightMessenger = Messenger<std::shared_ptr<LocalizationToTrafficLightFrame>>;
using CollisionToPlannerMessenger = Messenger<std::shared_ptr<CollisionToPlannerFrame>>;
using TrafficLightToPlannerMessenger = Messenger<std::shared_ptr<TrafficLightToPlannerFrame>>;
} // namespace traffic_manager
} // namespace carla

290
LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp Normal file
View File

@ -0,0 +1,290 @@
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/PIDController.h"
#include "carla/trafficmanager/MotionPlanStage.h"
namespace carla {
namespace traffic_manager {
using namespace constants::MotionPlan;
using namespace constants::WaypointSelection;
using namespace constants::SpeedThreshold;
using constants::HybridMode::HYBRID_MODE_DT;
MotionPlanStage::MotionPlanStage(
const std::vector<ActorId> &vehicle_id_list,
const SimulationState &simulation_state,
const Parameters &parameters,
const BufferMap &buffer_map,
const TrackTraffic &track_traffic,
const std::vector<float> &urban_longitudinal_parameters,
const std::vector<float> &highway_longitudinal_parameters,
const std::vector<float> &urban_lateral_parameters,
const std::vector<float> &highway_lateral_parameters,
const LocalizationFrame &localization_frame,
const CollisionFrame&collision_frame,
const TLFrame &tl_frame,
ControlFrame &output_array)
: vehicle_id_list(vehicle_id_list),
simulation_state(simulation_state),
parameters(parameters),
buffer_map(buffer_map),
track_traffic(track_traffic),
urban_longitudinal_parameters(urban_longitudinal_parameters),
highway_longitudinal_parameters(highway_longitudinal_parameters),
urban_lateral_parameters(urban_lateral_parameters),
highway_lateral_parameters(highway_lateral_parameters),
localization_frame(localization_frame),
collision_frame(collision_frame),
tl_frame(tl_frame),
output_array(output_array) {}
void MotionPlanStage::Update(const unsigned long index) {
const ActorId actor_id = vehicle_id_list.at(index);
const cg::Location ego_location = simulation_state.GetLocation(actor_id);
const cg::Vector3D ego_velocity = simulation_state.GetVelocity(actor_id);
const float ego_speed = ego_velocity.Length();
const cg::Vector3D ego_heading = simulation_state.GetHeading(actor_id);
const bool ego_physics_enabled = simulation_state.IsPhysicsEnabled(actor_id);
const Buffer &waypoint_buffer = buffer_map.at(actor_id);
const LocalizationData &localization = localization_frame.at(index);
const CollisionHazardData &collision_hazard = collision_frame.at(index);
const bool &tl_hazard = tl_frame.at(index);
const float target_point_distance = std::max(ego_speed * TARGET_WAYPOINT_TIME_HORIZON,
TARGET_WAYPOINT_HORIZON_LENGTH);
const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
const cg::Location target_location = target_waypoint->GetLocation();
float dot_product = DeviationDotProduct(ego_location, ego_heading, target_location);
float cross_product = DeviationCrossProduct(ego_location, ego_heading, target_location);
dot_product = 1.0f - dot_product;
if (cross_product < 0.0f) {
dot_product *= -1.0f;
}
const float current_deviation = dot_product;
// If previous state for vehicle not found, initialize state entry.
if (pid_state_map.find(actor_id) == pid_state_map.end()) {
const auto initial_state = StateEntry{chr::system_clock::now(), 0.0f, 0.0f, 0.0f, 0.0f};
pid_state_map.insert({actor_id, initial_state});
}
// Retrieving the previous state.
traffic_manager::StateEntry previous_state;
previous_state = pid_state_map.at(actor_id);
// Select PID parameters.
std::vector<float> longitudinal_parameters;
std::vector<float> lateral_parameters;
if (ego_speed > HIGHWAY_SPEED) {
longitudinal_parameters = highway_longitudinal_parameters;
lateral_parameters = highway_lateral_parameters;
} else {
longitudinal_parameters = urban_longitudinal_parameters;
lateral_parameters = urban_lateral_parameters;
}
// Target velocity for vehicle.
const float ego_speed_limit = simulation_state.GetSpeedLimit(actor_id);
float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, ego_speed_limit) / 3.6f;
// Collision handling and target velocity correction.
std::pair<bool, float> collision_response = CollisionHandling(collision_hazard, tl_hazard, ego_velocity,
ego_heading, max_target_velocity);
bool collision_emergency_stop = collision_response.first;
float dynamic_target_velocity = collision_response.second;
// Don't enter junction if there isn't enough free space after the junction.
bool safe_after_junction = SafeAfterJunction(localization, tl_hazard, collision_emergency_stop);
// In case of collision or traffic light hazard.
bool emergency_stop = tl_hazard || collision_emergency_stop || !safe_after_junction;
ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
cg::Transform teleportation_transform;
// If physics is enabled for the vehicle, use PID controller.
const auto current_time = chr::system_clock::now();
StateEntry current_state;
if (ego_physics_enabled) {
// State update for vehicle.
current_state = PID::StateUpdate(previous_state, ego_speed, dynamic_target_velocity,
current_deviation, current_time);
// Controller actuation.
actuation_signal = PID::RunStep(current_state, previous_state,
longitudinal_parameters, lateral_parameters);
if (emergency_stop) {
current_state.deviation_integral = 0.0f;
current_state.velocity_integral = 0.0f;
actuation_signal.throttle = 0.0f;
actuation_signal.brake = 1.0f;
}
}
// For physics-less vehicles, determine position and orientation for teleportation.
else {
// Flushing controller state for vehicle.
current_state = {chr::system_clock::now(),
0.0f, 0.0f,
0.0f, 0.0f};
// Add entry to teleportation duration clock table if not present.
if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
teleportation_instance.insert({actor_id, chr::system_clock::now()});
}
// Measuring time elapsed since last teleportation for the vehicle.
chr::duration<float> elapsed_time = current_time - teleportation_instance.at(actor_id);
// Find a location ahead of the vehicle for teleportation to achieve intended velocity.
if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time.count() > HYBRID_MODE_DT)) {
// Target displacement magnitude to achieve target velocity.
const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT;
const SimpleWaypointPtr teleport_target_waypoint = GetTargetWaypoint(waypoint_buffer, target_displacement).first;
// Construct target transform to accurately achieve desired velocity.
float missing_displacement = 0.0f;
const float base_displacement = teleport_target_waypoint->Distance(ego_location);
if (base_displacement < target_displacement) {
missing_displacement = target_displacement - base_displacement;
}
cg::Transform target_base_transform = teleport_target_waypoint->GetTransform();
cg::Location target_base_location = target_base_transform.location;
cg::Vector3D target_heading = target_base_transform.GetForwardVector();
cg::Location teleportation_location = target_base_location + cg::Location(target_heading * missing_displacement);
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
}
// In case of an emergency stop, stay in the same location.
// Also, teleport only once every dt in asynchronous mode.
else {
teleportation_transform = cg::Transform(ego_location, simulation_state.GetRotation(actor_id));
}
}
// Updating PID state.
StateEntry &state = pid_state_map.at(actor_id);
state = current_state;
// Constructing the actuation signal.
if (ego_physics_enabled) {
carla::rpc::VehicleControl vehicle_control;
vehicle_control.throttle = actuation_signal.throttle;
vehicle_control.brake = actuation_signal.brake;
vehicle_control.steer = actuation_signal.steer;
output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
} else {
output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
}
}
bool MotionPlanStage::SafeAfterJunction(const LocalizationData &localization,
const bool tl_hazard,
const bool collision_emergency_stop) {
SimpleWaypointPtr junction_end_point = localization.junction_end_point;
SimpleWaypointPtr safe_point = localization.safe_point;
bool safe_after_junction = true;
if (!tl_hazard && !collision_emergency_stop
&& localization.is_at_junction_entrance
&& junction_end_point != nullptr && safe_point != nullptr
&& junction_end_point->DistanceSquared(safe_point) > SQUARE(MIN_SAFE_INTERVAL_LENGTH)) {
ActorIdSet initial_set = track_traffic.GetPassingVehicles(junction_end_point->GetId());
float safe_interval_length_squared = junction_end_point->DistanceSquared(safe_point);
cg::Location mid_point = (junction_end_point->GetLocation() + safe_point->GetLocation())/2.0f;
// Scan through the safe interval and find if any vehicles are present in it
// by finding their occupied waypoints.
for (SimpleWaypointPtr current_waypoint = junction_end_point;
current_waypoint->DistanceSquared(junction_end_point) < safe_interval_length_squared && safe_after_junction;
current_waypoint = current_waypoint->GetNextWaypoint().front()) {
ActorIdSet current_set = track_traffic.GetPassingVehicles(current_waypoint->GetId());
ActorIdSet difference;
std::set_difference(current_set.begin(), current_set.end(),
initial_set.begin(), initial_set.end(),
std::inserter(difference, difference.begin()));
if (difference.size() > 0) {
for (const ActorId &blocking_id: difference) {
cg::Location blocking_actor_location = simulation_state.GetLocation(blocking_id);
if (cg::Math::DistanceSquared(blocking_actor_location, mid_point) < SQUARE(MAX_JUNCTION_BLOCK_DISTANCE)
&& simulation_state.GetVelocity(blocking_id).SquaredLength() < SQUARE(AFTER_JUNCTION_MIN_SPEED)) {
safe_after_junction = false;
}
}
}
}
}
return safe_after_junction;
}
std::pair<bool, float> MotionPlanStage::CollisionHandling(const CollisionHazardData &collision_hazard,
const bool tl_hazard,
const cg::Vector3D ego_velocity,
const cg::Vector3D ego_heading,
const float max_target_velocity) {
bool collision_emergency_stop = false;
float dynamic_target_velocity = max_target_velocity;
if (collision_hazard.hazard && !tl_hazard) {
const ActorId other_actor_id = collision_hazard.hazard_actor_id;
const cg::Vector3D other_velocity = simulation_state.GetVelocity(other_actor_id);
const float ego_relative_speed = (ego_velocity - other_velocity).Length();
const float available_distance_margin = collision_hazard.available_distance_margin;
const float other_speed_along_heading = cg::Math::Dot(other_velocity, ego_heading);
// Consider collision avoidance decisions only if there is positive relative velocity
// of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle).
if (ego_relative_speed > EPSILON_RELATIVE_SPEED) {
// If other vehicle is approaching lead vehicle and lead vehicle is further
// than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
float follow_lead_distance = ego_relative_speed * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE;
if (available_distance_margin > follow_lead_distance) {
// Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE
// by maintaining a relative speed of RELATIVE_APPROACH_SPEED
dynamic_target_velocity = other_speed_along_heading + RELATIVE_APPROACH_SPEED;
}
// If vehicle is approaching a lead vehicle and the lead vehicle is further
// than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE.
else if (available_distance_margin > CRITICAL_BRAKING_MARGIN) {
// Then follow the lead vehicle by acquiring it's speed along current heading.
dynamic_target_velocity = std::max(other_speed_along_heading, RELATIVE_APPROACH_SPEED);
} else {
// If lead vehicle closer than CRITICAL_BRAKING_MARGIN, initiate emergency stop.
collision_emergency_stop = true;
}
}
if (available_distance_margin < CRITICAL_BRAKING_MARGIN) {
collision_emergency_stop = true;
}
}
dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity);
return {collision_emergency_stop, dynamic_target_velocity};
}
void MotionPlanStage::RemoveActor(const ActorId actor_id) {
pid_state_map.erase(actor_id);
teleportation_instance.erase(actor_id);
}
void MotionPlanStage::Reset() {
pid_state_map.clear();
teleportation_instance.clear();
}
} // namespace traffic_manager
} // namespace carla

72
LibCarla/source/carla/trafficmanager/MotionPlanStage.h Normal file
View File

@ -0,0 +1,72 @@
/// This file has functionality for motion planning based on information
/// from localization, collision avoidance and traffic light response.
#pragma once
#include "carla/trafficmanager/DataStructures.h"
#include "carla/trafficmanager/LocalizationUtils.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/SimulationState.h"
#include "carla/trafficmanager/Stage.h"
#include "carla/trafficmanager/TrackTraffic.h"
namespace carla {
namespace traffic_manager {
class MotionPlanStage: Stage {
private:
const std::vector<ActorId> &vehicle_id_list;
const SimulationState &simulation_state;
const Parameters &parameters;
const BufferMap &buffer_map;
const TrackTraffic &track_traffic;
// PID paramenters for various road conditions.
const std::vector<float> urban_longitudinal_parameters;
const std::vector<float> highway_longitudinal_parameters;
const std::vector<float> urban_lateral_parameters;
const std::vector<float> highway_lateral_parameters;
const LocalizationFrame &localization_frame;
const CollisionFrame &collision_frame;
const TLFrame &tl_frame;
// Structure holding the controller state for registered vehicles.
std::unordered_map<ActorId, StateEntry> pid_state_map;
// Structure to keep track of duration between teleportation
// in hybrid physics mode.
std::unordered_map<ActorId, TimeInstance> teleportation_instance;
ControlFrame &output_array;
std::pair<bool, float> CollisionHandling(const CollisionHazardData &collision_hazard,
const bool tl_hazard,
const cg::Vector3D ego_velocity,
const cg::Vector3D ego_heading,
const float max_target_velocity);
bool SafeAfterJunction(const LocalizationData &localization,
const bool tl_hazard,
const bool collision_emergency_stop);
public:
MotionPlanStage(const std::vector<ActorId> &vehicle_id_list,
const SimulationState &simulation_state,
const Parameters &parameters,
const BufferMap &buffer_map,
const TrackTraffic &track_traffic,
const std::vector<float> &urban_longitudinal_parameters,
const std::vector<float> &highway_longitudinal_parameters,
const std::vector<float> &urban_lateral_parameters,
const std::vector<float> &highway_lateral_parameters,
const LocalizationFrame &localization_frame,
const CollisionFrame &collision_frame,
const TLFrame &tl_frame,
ControlFrame &output_array);
void Update(const unsigned long index);
void RemoveActor(const ActorId actor_id);
void Reset();
};
} // namespace traffic_manager
} // namespace carla

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

@ -1,318 +0,0 @@
// 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/MotionPlannerStage.h"
namespace carla {
namespace traffic_manager {
namespace PlannerConstants {
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
static const float RELATIVE_APPROACH_SPEED = 10.0f / 3.6f;
static const float MIN_FOLLOW_LEAD_DISTANCE = 5.0f;
static const float MAX_FOLLOW_LEAD_DISTANCE = 10.0f;
static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f;
static const float FOLLOW_DISTANCE_RATE = (MAX_FOLLOW_LEAD_DISTANCE-MIN_FOLLOW_LEAD_DISTANCE)/ARBITRARY_MAX_SPEED;
static const float CRITICAL_BRAKING_MARGIN = 0.25f;
static const float HYBRID_MODE_DT = 0.05f;
static const float EPSILON_VELOCITY = 0.001f;
} // namespace PlannerConstants
using namespace PlannerConstants;
MotionPlannerStage::MotionPlannerStage(
std::string stage_name,
std::shared_ptr<LocalizationToPlannerMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> collision_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> traffic_light_messenger,
std::shared_ptr<PlannerToControlMessenger> control_messenger,
Parameters &parameters,
std::vector<float> urban_longitudinal_parameters,
std::vector<float> highway_longitudinal_parameters,
std::vector<float> urban_lateral_parameters,
std::vector<float> highway_lateral_parameters,
cc::DebugHelper &debug_helper)
: PipelineStage(stage_name),
localization_messenger(localization_messenger),
collision_messenger(collision_messenger),
traffic_light_messenger(traffic_light_messenger),
control_messenger(control_messenger),
parameters(parameters),
urban_longitudinal_parameters(urban_longitudinal_parameters),
highway_longitudinal_parameters(highway_longitudinal_parameters),
urban_lateral_parameters(urban_lateral_parameters),
highway_lateral_parameters(highway_lateral_parameters),
debug_helper(debug_helper){
// Initializing the output frame selector.
frame_selector = true;
// Initializing number of vehicles to zero in the beginning.
number_of_vehicles = 0u;
}
MotionPlannerStage::~MotionPlannerStage() {}
void MotionPlannerStage::Action() {
// Selecting an output frame.
const auto current_control_frame = frame_selector ? control_frame_a : control_frame_b;
// Looping over all vehicles.
for (uint64_t i = 0u;
i < number_of_vehicles &&
localization_frame != nullptr &&
collision_frame != nullptr &&
traffic_light_frame != nullptr;
++i) {
const LocalizationToPlannerData &localization_data = localization_frame->at(i);
const CollisionToPlannerData& collision_data = collision_frame->at(i);
if (!localization_data.actor->IsAlive()) {
continue;
}
const Actor actor = localization_data.actor;
const float current_deviation = localization_data.deviation;
const float current_distance = localization_data.distance;
const cg::Vector3D current_velocity_vector = localization_data.velocity;
const float current_velocity = current_velocity_vector.Length();
const ActorId actor_id = actor->GetId();
const cg::Vector3D ego_heading = actor->GetTransform().GetForwardVector();
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const auto current_time = chr::system_clock::now();
// If previous state for vehicle not found, initialize state entry.
if (pid_state_map.find(actor_id) == pid_state_map.end()) {
const auto initial_state = StateEntry{0.0f, 0.0f, 0.0f, chr::system_clock::now(), 0.0f, 0.0f, 0.0f};
pid_state_map.insert({actor_id, initial_state});
}
// Retrieving the previous state.
traffic_manager::StateEntry previous_state;
previous_state = pid_state_map.at(actor_id);
// Change PID parameters if on highway.
if (current_velocity > HIGHWAY_SPEED) {
longitudinal_parameters = highway_longitudinal_parameters;
lateral_parameters = highway_lateral_parameters;
} else {
longitudinal_parameters = urban_longitudinal_parameters;
lateral_parameters = urban_lateral_parameters;
}
// Target velocity for vehicle.
float max_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f;
float dynamic_target_velocity = max_target_velocity;
//////////////////////// Collision related data handling ///////////////////////////
bool collision_emergency_stop = false;
if (collision_data.hazard) {
cg::Vector3D other_vehicle_velocity = collision_data.other_vehicle_velocity;
float ego_relative_velocity = (current_velocity_vector - other_vehicle_velocity).Length();
float other_velocity_along_heading = cg::Math::Dot(other_vehicle_velocity, ego_heading);
// Consider collision avoidance decisions only if there is positive relative velocity
// of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle).
if (ego_relative_velocity > EPSILON_VELOCITY) {
// If other vehicle is approaching lead vehicle and lead vehicle is further
// than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
float follow_lead_distance = ego_relative_velocity * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE;
if (collision_data.distance_to_other_vehicle > follow_lead_distance) {
// Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE
// by maintaining a relative speed of RELATIVE_APPROACH_SPEED
dynamic_target_velocity = other_velocity_along_heading + RELATIVE_APPROACH_SPEED;
}
// If vehicle is approaching a lead vehicle and the lead vehicle is further
// than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE.
else if (collision_data.distance_to_other_vehicle > CRITICAL_BRAKING_MARGIN) {
// Then follow the lead vehicle by acquiring it's speed along current heading.
dynamic_target_velocity = std::max(other_velocity_along_heading, RELATIVE_APPROACH_SPEED);
} else {
// If lead vehicle closer than CRITICAL_BRAKING_MARGIN, initiate emergency stop.
collision_emergency_stop = true;
}
}
if (collision_data.distance_to_other_vehicle < CRITICAL_BRAKING_MARGIN) {
collision_emergency_stop = true;
}
}
///////////////////////////////////////////////////////////////////////////////////
// Clip dynamic target velocity to maximum allowed speed for the vehicle.
dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity);
bool emergency_stop = false;
// In case of collision or traffic light hazard.
if (traffic_light_frame->at(i).traffic_light_hazard
|| collision_emergency_stop) {
emergency_stop = true;
}
// Message items to be sent to batch control stage.
ActuationSignal actuation_signal {0.0f, 0.0f, 0.0f};
bool physics_enabled = true;
cg::Transform teleportation_transform;
// If physics is enabled for the vehicle, use PID controller.
StateEntry current_state;
if (localization_data.physics_enabled) {
// State update for vehicle.
current_state = controller.StateUpdate(previous_state, current_velocity,
dynamic_target_velocity, current_deviation,
current_distance, current_time);
// Controller actuation.
actuation_signal = controller.RunStep(current_state, previous_state,
longitudinal_parameters, lateral_parameters);
if (emergency_stop) {
current_state.deviation_integral = 0.0f;
current_state.velocity_integral = 0.0f;
actuation_signal.throttle = 0.0f;
actuation_signal.brake = 1.0f;
}
}
// For physics-less vehicles, determine position and orientation for teleportation.
else if (hybrid_physics_mode) {
physics_enabled = false;
// Flushing controller state for vehicle.
current_state = {0.0f, 0.0f, 0.0f,
chr::system_clock::now(),
0.0f, 0.0f, 0.0f};
// Add entry to teleportation duration clock table if not present.
if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
teleportation_instance.insert({actor_id, chr::system_clock::now()});
}
// Measuring time elapsed since last teleportation for the vehicle.
chr::duration<float> elapsed_time = current_time - teleportation_instance.at(actor_id);
// Find a location ahead of the vehicle for teleportation to achieve intended velocity.
if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time.count() > HYBRID_MODE_DT)) {
// Target displacement magnitude to achieve target velocity.
const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT;
const float target_displacement_square = std::pow(target_displacement, 2.0f);
SimpleWaypointPtr target_interval_begin = nullptr;
SimpleWaypointPtr target_interval_end = nullptr;
bool teleportation_interval_found = false;
cg::Location vehicle_location = actor->GetLocation();
// Find the interval containing position to achieve target displacement.
for (uint32_t j = 0u;
j+1 < localization_data.position_window.size() && !teleportation_interval_found;
++j) {
target_interval_begin = localization_data.position_window.at(j);
target_interval_end = localization_data.position_window.at(j+1);
cg::Vector3D relative_position = target_interval_begin->GetLocation() - vehicle_location;
if (cg::Math::Dot(relative_position, ego_heading) > 0.0f
&& ((target_interval_begin->DistanceSquared(vehicle_location) > target_displacement_square)
|| (target_interval_begin->DistanceSquared(vehicle_location) < target_displacement_square
&& target_interval_end->DistanceSquared(vehicle_location) > target_displacement_square))) {
teleportation_interval_found = true;
}
}
if (target_interval_begin != nullptr && target_interval_end != nullptr) {
// Construct target transform to accurately achieve desired velocity.
float missing_displacement = 0.0f;
const float base_displacement = target_interval_begin->Distance(vehicle_location);
if (base_displacement < target_displacement) {
missing_displacement = target_displacement - base_displacement;
}
cg::Transform target_base_transform = target_interval_begin->GetTransform();
cg::Location target_base_location = target_base_transform.location;
cg::Vector3D target_heading = target_base_transform.GetForwardVector();
cg::Location teleportation_location = target_base_location
+ cg::Location(target_heading * missing_displacement);
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
} else {
teleportation_transform = actor->GetTransform();
}
}
// In case of an emergency stop, stay in the same location.
// Also, teleport only once every dt in asynchronous mode.
else {
teleportation_transform = actor->GetTransform();
}
}
// Updating PID state.
StateEntry &state = pid_state_map.at(actor_id);
state = current_state;
// Constructing the actuation signal.
PlannerToControlData &message = current_control_frame->at(i);
message.actor = actor;
message.throttle = actuation_signal.throttle;
message.brake = actuation_signal.brake;
message.steer = actuation_signal.steer;
message.physics_enabled = physics_enabled;
message.transform = teleportation_transform;
}
}
void MotionPlannerStage::DataReceiver() {
localization_frame = localization_messenger->Peek();
collision_frame = collision_messenger->Peek();
traffic_light_frame = traffic_light_messenger->Peek();
// Allocating new containers for the changed number of registered vehicles.
if (localization_frame != nullptr &&
number_of_vehicles != (*localization_frame.get()).size()) {
number_of_vehicles = static_cast<uint64_t>((*localization_frame.get()).size());
// Allocate output frames.
control_frame_a = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
control_frame_b = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
}
hybrid_physics_mode = parameters.GetHybridPhysicsMode();
}
void MotionPlannerStage::DataSender() {
localization_messenger->Pop();
collision_messenger->Pop();
traffic_light_messenger->Pop();
control_messenger->Push(frame_selector ? control_frame_a : control_frame_b);
frame_selector = !frame_selector;
}
void MotionPlannerStage::DrawPIDValues(const boost::shared_ptr<cc::Vehicle> vehicle,
const float throttle, const float brake) {
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_location + cg::Location(0.0f,0.0f,4.0f),
std::to_string(brake), false, {255u, 0u, 0u}, 0.005f);
}
} // namespace traffic_manager
} // namespace carla

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

@ -1,107 +0,0 @@
// 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 <chrono>
#include <memory>
#include <unordered_map>
#include <vector>
#include "carla/client/Vehicle.h"
#include "carla/geom/Math.h"
#include "carla/rpc/Actor.h"
#include "carla/trafficmanager/MessengerAndDataTypes.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PIDController.h"
#include "carla/trafficmanager/PipelineStage.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {
namespace chr = std::chrono;
namespace cc = carla::client;
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::rpc::ActorId;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
/// 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.
class MotionPlannerStage : public PipelineStage {
private:
/// Selection key to switch between the output frames.
bool frame_selector;
/// Pointers to data frames to be shared with the batch control stage
std::shared_ptr<PlannerToControlFrame> control_frame_a;
std::shared_ptr<PlannerToControlFrame> control_frame_b;
/// Pointers to data frames received from various stages.
std::shared_ptr<LocalizationToPlannerFrame> localization_frame;
std::shared_ptr<CollisionToPlannerFrame> collision_frame;
std::shared_ptr<TrafficLightToPlannerFrame> traffic_light_frame;
/// Pointers to messenger objects connecting to various stages.
std::shared_ptr<LocalizationToPlannerMessenger> localization_messenger;
std::shared_ptr<CollisionToPlannerMessenger> collision_messenger;
std::shared_ptr<TrafficLightToPlannerMessenger> traffic_light_messenger;
std::shared_ptr<PlannerToControlMessenger> control_messenger;
/// Map to store states for integral and differential components
/// of the PID controller for every vehicle
std::unordered_map<ActorId, StateEntry> pid_state_map;
/// Run time parameterization object.
Parameters &parameters;
/// Configuration parameters for the PID controller.
std::vector<float> urban_longitudinal_parameters;
std::vector<float> highway_longitudinal_parameters;
std::vector<float> urban_lateral_parameters;
std::vector<float> highway_lateral_parameters;
std::vector<float> longitudinal_parameters;
std::vector<float> lateral_parameters;
/// Controller object.
PIDController controller;
/// Number of vehicles registered with the traffic manager.
uint64_t number_of_vehicles;
/// Reference to Carla's debug helper object.
cc::DebugHelper &debug_helper;
/// Switch indicating hybrid physics mode.
bool hybrid_physics_mode {false};
/// Teleportation duration clock;
std::unordered_map<ActorId, TimePoint> teleportation_instance;
public:
MotionPlannerStage(
std::string stage_name,
std::shared_ptr<LocalizationToPlannerMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> collision_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> traffic_light_messenger,
std::shared_ptr<PlannerToControlMessenger> control_messenger,
Parameters &parameters,
std::vector<float> longitudinal_parameters,
std::vector<float> highway_longitudinal_parameters,
std::vector<float> lateral_parameters,
std::vector<float> highway_lateral_parameters,
cc::DebugHelper &debug_helper);
~MotionPlannerStage();
void DataReceiver() override;
void Action() override;
void DataSender() override;
void DrawPIDValues(const boost::shared_ptr<cc::Vehicle> vehicle, const float throttle, const float brake);
};
} // namespace traffic_manager
} // namespace carla

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

@ -1,99 +0,0 @@
// 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/PIDController.h"
#include <algorithm>
#define CLAMP(__v, __hi, __lo) (__v > __hi? __hi : (__v < __lo? __lo: __v))
namespace carla {
namespace traffic_manager {
namespace PIDControllerConstants {
const float MAX_THROTTLE = 0.7f;
const float MAX_BRAKE = 1.0f;
const float VELOCITY_INTEGRAL_MAX = 5.0f;
const float VELOCITY_INTEGRAL_MIN = -5.0f;
// PID will be stable only over 20 FPS.
const float dt = 1/20.0f;
} // namespace PIDControllerConstants
using namespace PIDControllerConstants;
PIDController::PIDController() {}
// Initializing present state.
StateEntry PIDController::StateUpdate(
StateEntry previous_state,
float current_velocity,
float target_velocity,
float angular_deviation,
float distance,
TimeInstance current_time) {
traffic_manager::StateEntry current_state = {
angular_deviation, distance,
(current_velocity - target_velocity) / target_velocity,
current_time,
0.0f,
0.0f,
0.0f
};
// Calculating integrals.
current_state.deviation_integral = angular_deviation * dt + previous_state.deviation_integral;
current_state.distance_integral = distance * dt + previous_state.distance_integral;
current_state.velocity_integral = dt * current_state.velocity + previous_state.velocity_integral;
// Clamp velocity integral to avoid accumulating over-compensation
// with time for vehicles that take a long time to reach target velocity.
current_state.velocity_integral = CLAMP(current_state.velocity_integral,
VELOCITY_INTEGRAL_MAX, VELOCITY_INTEGRAL_MIN);
return current_state;
}
ActuationSignal PIDController::RunStep(
StateEntry present_state,
StateEntry previous_state,
const std::vector<float> &longitudinal_parameters,
const std::vector<float> &lateral_parameters) const {
// Longitudinal PID calculation.
const float expr_v =
longitudinal_parameters[0] * present_state.velocity +
longitudinal_parameters[1] * present_state.velocity_integral +
longitudinal_parameters[2] * (present_state.velocity -
previous_state.velocity) / dt;
float throttle;
float brake;
if (expr_v < 0.0f) {
throttle = std::min(std::abs(expr_v), MAX_THROTTLE);
brake = 0.0f;
} else {
throttle = 0.0f;
brake = std::min(expr_v, MAX_BRAKE);
}
// Lateral PID calculation.
float steer =
lateral_parameters[0] * present_state.deviation +
lateral_parameters[1] * present_state.deviation_integral +
lateral_parameters[2] * (present_state.deviation -
previous_state.deviation) / dt;
steer = std::max(-0.8f, std::min(steer, 0.8f));
return ActuationSignal{throttle, brake, steer};
}
} // namespace traffic_manager
} // namespace carla

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

@ -6,63 +6,82 @@
#pragma once
#include <chrono>
#include <cmath>
#include <vector>
#include <algorithm>
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/DataStructures.h"
namespace carla {
namespace traffic_manager {
namespace chr = std::chrono;
using namespace constants::PID;
using TimeInstance = chr::time_point<chr::system_clock, chr::nanoseconds>;
/// Structure to hold the actuation signals.
struct ActuationSignal {
float throttle;
float brake;
float steer;
};
namespace PID {
/// Structure to hold the controller state.
struct StateEntry {
float deviation;
float distance;
float velocity;
TimeInstance time_instance;
float deviation_integral;
float distance_integral;
float velocity_integral;
};
/// This class calculates PID actuation signals to control the vehicle
/// such that the vehicle maintains a target velocity and aligns its
/// orientation along its trajectory.
class PIDController {
public:
PIDController();
/// This method calculates the present state of the vehicle including
/// This function calculates the present state of the vehicle including
/// the accumulated integral component of the PID system.
StateEntry StateUpdate(
StateEntry previous_state,
StateEntry StateUpdate(StateEntry previous_state,
float current_velocity,
float target_velocity,
float angular_deviation,
float distance,
TimeInstance current_time);
TimeInstance current_time) {
StateEntry current_state = {
current_time,
angular_deviation,
(current_velocity - target_velocity) / target_velocity,
0.0f,
0.0f};
/// This method calculates the actuation signals based on the resent state
// Calculating integrals.
current_state.deviation_integral = angular_deviation * DT + previous_state.deviation_integral;
current_state.velocity_integral = DT * current_state.velocity + previous_state.velocity_integral;
// Clamp velocity integral to avoid accumulating over-compensation
// with time for vehicles that take a long time to reach target velocity.
current_state.velocity_integral = std::min(VELOCITY_INTEGRAL_MAX, std::max(current_state.velocity_integral, VELOCITY_INTEGRAL_MIN));
return current_state;
}
/// This function calculates the actuation signals based on the resent state
/// change of the vehicle to minimize PID error.
ActuationSignal RunStep(
StateEntry present_state,
ActuationSignal RunStep(StateEntry present_state,
StateEntry previous_state,
const std::vector<float> &longitudinal_parameters,
const std::vector<float> &lateral_parameters) const;
const std::vector<float> &lateral_parameters) {
};
// Longitudinal PID calculation.
const float expr_v =
longitudinal_parameters[0] * present_state.velocity +
longitudinal_parameters[1] * present_state.velocity_integral +
longitudinal_parameters[2] * (present_state.velocity - previous_state.velocity) * INV_DT;
float throttle;
float brake;
if (expr_v < 0.0f) {
throttle = std::min(std::abs(expr_v), MAX_THROTTLE);
brake = 0.0f;
} else {
throttle = 0.0f;
brake = std::min(expr_v, MAX_BRAKE);
}
// Lateral PID calculation.
float steer =
lateral_parameters[0] * present_state.deviation +
lateral_parameters[1] * present_state.deviation_integral +
lateral_parameters[2] * (present_state.deviation - previous_state.deviation) * INV_DT;
steer = std::max(-0.8f, std::min(steer, 0.8f));
return ActuationSignal{throttle, brake, steer};
}
} // namespace PID
} // namespace traffic_manager
} // namespace carla

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

@ -4,8 +4,6 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include <random>
#include "carla/trafficmanager/Parameters.h"
namespace carla {
@ -19,6 +17,8 @@ Parameters::Parameters() {
Parameters::~Parameters() {}
//////////////////////////////////// SETTERS //////////////////////////////////
void Parameters::SetHybridPhysicsMode(const bool mode_switch) {
hybrid_physics_mode.store(mode_switch);
@ -43,7 +43,7 @@ void Parameters::SetCollisionDetection(const ActorPtr &reference_actor, const Ac
if (ignore_collision.Contains(reference_id)) {
std::shared_ptr<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
if (actor_set->Contains(other_id)) {
actor_set->Remove({other_actor});
actor_set->Remove({other_id});
}
}
} else {
@ -87,118 +87,19 @@ void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float
distance_to_leading_vehicle.AddEntry(entry);
}
bool Parameters::GetSynchronousMode() {
return synchronous_mode.load();
void Parameters::SetSynchronousMode(const bool mode_switch) {
synchronous_mode.store(mode_switch);
}
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();
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const float speed_limit = vehicle->GetSpeedLimit();
float percentage_difference = global_percentage_difference_from_limit;
if (percentage_difference_from_speed_limit.Contains(actor_id)) {
percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id);
}
return speed_limit * (1.0f - percentage_difference / 100.0f);
}
bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor) {
const ActorId reference_actor_id = reference_actor->GetId();
const ActorId other_actor_id = other_actor->GetId();
bool avoid_collision = true;
if (ignore_collision.Contains(reference_actor_id) &&
ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) {
avoid_collision = false;
}
return avoid_collision;
}
ChangeLaneInfo Parameters::GetForceLaneChange(const ActorPtr &actor) {
const ActorId actor_id = actor->GetId();
ChangeLaneInfo change_lane_info;
if (force_lane_change.Contains(actor_id)) {
change_lane_info = force_lane_change.GetValue(actor_id);
}
force_lane_change.RemoveEntry(actor_id);
return change_lane_info;
}
float Parameters::GetKeepRightPercentage(const ActorPtr &actor) {
const ActorId actor_id = actor->GetId();
float percentage = -1.0f;
if (perc_keep_right.Contains(actor_id)) {
percentage = perc_keep_right.GetValue(actor_id);
}
perc_keep_right.RemoveEntry(actor_id);
return percentage;
}
bool Parameters::GetAutoLaneChange(const ActorPtr &actor) {
const ActorId actor_id = actor->GetId();
bool auto_lane_change_policy = true;
if (auto_lane_change.Contains(actor_id)) {
auto_lane_change_policy = auto_lane_change.GetValue(actor_id);
}
return auto_lane_change_policy;
}
float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) {
const ActorId actor_id = actor->GetId();
float specific_distance_margin = 0.0f;
if (distance_to_leading_vehicle.Contains(actor_id)) {
specific_distance_margin = distance_to_leading_vehicle.GetValue(actor_id);
} else {
specific_distance_margin = distance_margin;
}
return specific_distance_margin;
}
void Parameters::SetGlobalDistanceToLeadingVehicle(const float dist) {
distance_margin.store(dist);
}
float Parameters::GetHybridPhysicsRadius() {
return hybrid_physics_radius.load();
}
void Parameters::SetHybridPhysicsRadius(const float radius) {
float new_radius = std::max(radius, 0.0f);
hybrid_physics_radius.store(new_radius);
}
void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) {
float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f);
@ -227,9 +128,100 @@ void Parameters::SetPercentageIgnoreWalkers(const ActorPtr &actor, const float p
perc_ignore_walkers.AddEntry(entry);
}
float Parameters::GetPercentageRunningLight(const ActorPtr &actor) {
void Parameters::SetHybridPhysicsRadius(const float radius) {
float new_radius = std::max(radius, 0.0f);
hybrid_physics_radius.store(new_radius);
}
//////////////////////////////////// GETTERS //////////////////////////////////
float Parameters::GetHybridPhysicsRadius() const {
return hybrid_physics_radius.load();
}
bool Parameters::GetSynchronousMode() const {
return synchronous_mode.load();
}
double Parameters::GetSynchronousModeTimeOutInMiliSecond() const {
return synchronous_time_out.count();
}
float Parameters::GetVehicleTargetVelocity(const ActorId &actor_id, const float speed_limit) const {
float percentage_difference = global_percentage_difference_from_limit;
if (percentage_difference_from_speed_limit.Contains(actor_id)) {
percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id);
}
return speed_limit * (1.0f - percentage_difference / 100.0f);
}
bool Parameters::GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const {
bool avoid_collision = true;
if (ignore_collision.Contains(reference_actor_id) &&
ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) {
avoid_collision = false;
}
return avoid_collision;
}
ChangeLaneInfo Parameters::GetForceLaneChange(const ActorId &actor_id) {
ChangeLaneInfo change_lane_info {false, false};
if (force_lane_change.Contains(actor_id)) {
change_lane_info = force_lane_change.GetValue(actor_id);
}
force_lane_change.RemoveEntry(actor_id);
return change_lane_info;
}
float Parameters::GetKeepRightPercentage(const ActorId &actor_id) {
float percentage = -1.0f;
if (perc_keep_right.Contains(actor_id)) {
percentage = perc_keep_right.GetValue(actor_id);
}
perc_keep_right.RemoveEntry(actor_id);
return percentage;
}
bool Parameters::GetAutoLaneChange(const ActorId &actor_id) const {
bool auto_lane_change_policy = true;
if (auto_lane_change.Contains(actor_id)) {
auto_lane_change_policy = auto_lane_change.GetValue(actor_id);
}
return auto_lane_change_policy;
}
float Parameters::GetDistanceToLeadingVehicle(const ActorId &actor_id) const {
float specific_distance_margin = 0.0f;
if (distance_to_leading_vehicle.Contains(actor_id)) {
specific_distance_margin = distance_to_leading_vehicle.GetValue(actor_id);
} else {
specific_distance_margin = distance_margin;
}
return specific_distance_margin;
}
float Parameters::GetPercentageRunningLight(const ActorId &actor_id) const {
const ActorId actor_id = actor->GetId();
float percentage = 0.0f;
if (perc_run_traffic_light.Contains(actor_id)) {
@ -239,9 +231,8 @@ float Parameters::GetPercentageRunningLight(const ActorPtr &actor) {
return percentage;
}
float Parameters::GetPercentageRunningSign(const ActorPtr &actor) {
float Parameters::GetPercentageRunningSign(const ActorId &actor_id) const {
const ActorId actor_id = actor->GetId();
float percentage = 0.0f;
if (perc_run_traffic_sign.Contains(actor_id)) {
@ -251,9 +242,8 @@ float Parameters::GetPercentageRunningSign(const ActorPtr &actor) {
return percentage;
}
float Parameters::GetPercentageIgnoreWalkers(const ActorPtr &actor) {
float Parameters::GetPercentageIgnoreWalkers(const ActorId &actor_id) const {
const ActorId actor_id = actor->GetId();
float percentage = 0.0f;
if (perc_ignore_walkers.Contains(actor_id)) {
@ -263,9 +253,8 @@ float Parameters::GetPercentageIgnoreWalkers(const ActorPtr &actor) {
return percentage;
}
float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) {
float Parameters::GetPercentageIgnoreVehicles(const ActorId &actor_id) const {
const ActorId actor_id = actor->GetId();
float percentage = 0.0f;
if (perc_ignore_vehicles.Contains(actor_id)) {
@ -275,7 +264,7 @@ float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) {
return percentage;
}
bool Parameters::GetHybridPhysicsMode() {
bool Parameters::GetHybridPhysicsMode() const {
return hybrid_physics_mode.load();
}

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

@ -8,6 +8,7 @@
#include <atomic>
#include <chrono>
#include <random>
#include "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
@ -33,7 +34,6 @@ namespace traffic_manager {
class Parameters {
private:
/// Target velocity map for individual vehicles.
AtomicMap<ActorId, float> percentage_difference_from_speed_limit;
/// Global target velocity limit % difference.
@ -69,6 +69,8 @@ namespace traffic_manager {
Parameters();
~Parameters();
////////////////////////////////// SETTERS /////////////////////////////////////
/// 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);
@ -94,39 +96,6 @@ namespace traffic_manager {
/// the leading vehicle.
void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance);
/// Method to query target velocity for a vehicle.
float GetVehicleTargetVelocity(const ActorPtr &actor);
/// Method to query collision avoidance rule between a pair of vehicles.
bool GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor);
/// Method to query lane change command for a vehicle.
ChangeLaneInfo GetForceLaneChange(const ActorPtr &actor);
/// Method to query percentage probability of keep right rule for a vehicle.
float GetKeepRightPercentage(const ActorPtr &actor);
/// Method to query auto lane change rule for a vehicle.
bool GetAutoLaneChange(const ActorPtr &actor);
/// Method to query distance to leading vehicle for a given vehicle.
float GetDistanceToLeadingVehicle(const ActorPtr &actor);
/// 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 vehicle.
float GetPercentageIgnoreVehicles(const ActorPtr &actor);
/// Method to get % to ignore any walker.
float GetPercentageIgnoreWalkers(const ActorPtr &actor);
/// Method to set distance to leading vehicle for a given vehicle.
void SetGlobalDistanceToLeadingVehicle(const float dist);
/// Method to set % to run any traffic sign.
void SetPercentageRunningSign(const ActorPtr &actor, const float perc);
@ -142,14 +111,8 @@ namespace traffic_manager {
/// Method to set probabilistic preference to keep on the right lane.
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
/// 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();
/// Method to set the distance to leading vehicle for all registered vehicles.
void SetGlobalDistanceToLeadingVehicle(const float dist);
/// Set Synchronous mode time out.
void SetSynchronousModeTimeOutInMiliSecond(const double time);
@ -157,18 +120,58 @@ namespace traffic_manager {
/// Method to set hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch);
/// Method to retrieve hybrid physics mode.
bool GetHybridPhysicsMode();
/// Method to set synchronous mode.
void SetSynchronousMode(const bool mode_switch = true);
/// Method to set hybrid physics radius.
void SetHybridPhysicsRadius(const float radius);
///////////////////////////////// GETTERS /////////////////////////////////////
/// Method to retrieve hybrid physics radius.
float GetHybridPhysicsRadius();
float GetHybridPhysicsRadius() const;
/// Method to query target velocity for a vehicle.
float GetVehicleTargetVelocity(const ActorId &actor_id, const float speed_limit) const;
/// Method to query collision avoidance rule between a pair of vehicles.
bool GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const;
/// Method to query lane change command for a vehicle.
ChangeLaneInfo GetForceLaneChange(const ActorId &actor_id);
/// Method to query percentage probability of keep right rule for a vehicle.
float GetKeepRightPercentage(const ActorId &actor_id);
/// Method to query auto lane change rule for a vehicle.
bool GetAutoLaneChange(const ActorId &actor_id) const;
/// Method to query distance to leading vehicle for a given vehicle.
float GetDistanceToLeadingVehicle(const ActorId &actor_id) const;
/// Method to get % to run any traffic light.
float GetPercentageRunningSign(const ActorId &actor_id) const;
/// Method to get % to run any traffic light.
float GetPercentageRunningLight(const ActorId &actor_id) const;
/// Method to get % to ignore any vehicle.
float GetPercentageIgnoreVehicles(const ActorId &actor_id) const;
/// Method to get % to ignore any walker.
float GetPercentageIgnoreWalkers(const ActorId &actor_id) const;
/// Method to get synchronous mode.
bool GetSynchronousMode() const;
/// Get synchronous mode time out
double GetSynchronousModeTimeOutInMiliSecond() const;
/// Method to retrieve hybrid physics mode.
bool GetHybridPhysicsMode() const;
/// Synchronous mode time out variable.
std::chrono::duration<double, std::milli> synchronous_time_out;
};
} // namespace traffic_manager

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

@ -1,48 +0,0 @@
// 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/PerformanceDiagnostics.h"
namespace carla {
namespace traffic_manager {
PerformanceDiagnostics::PerformanceDiagnostics(std::string stage_name)
: stage_name(stage_name) {
throughput_clock = chr::system_clock::now();
throughput_counter = 0u;
inter_update_clock = chr::system_clock::now();
inter_update_duration = chr::duration<float>(0);
}
void PerformanceDiagnostics::RegisterUpdate(bool begin_or_end) {
const auto current_time = chr::system_clock::now();
if (begin_or_end) {
const chr::duration<float> throughput_count_duration = current_time - throughput_clock;
if (throughput_count_duration.count() > 1.0f) {
// std::cout << "Stage: " + stage_name + ", throughput: " << throughput_counter
// << ", avg. cycle duration: " << 1000* inter_update_duration.count()
// << " ms" << std::endl;
throughput_clock = current_time;
throughput_counter = 0u;
} else {
++throughput_counter;
}
inter_update_clock = current_time;
} else {
const chr::duration<float> last_update_duration = current_time - inter_update_clock;
inter_update_duration = (inter_update_duration + last_update_duration) / 2.0f;
}
}
} // namespace traffic_manager
} // namespace carla

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

@ -1,102 +0,0 @@
// 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 <chrono>
#include <string>
#include <unordered_map>
#include "carla/Logging.h"
namespace carla {
namespace traffic_manager {
namespace chr = std::chrono;
using namespace chr;
using TimePoint = chr::time_point<chr::system_clock, chr::nanoseconds>;
class PerformanceDiagnostics {
private:
/// Stage name.
std::string stage_name;
/// Throughput clock.
TimePoint throughput_clock;
/// Throughput counter.
uint64_t throughput_counter;
/// Inter-update clock.
TimePoint inter_update_clock;
/// Inter-update duration.
chr::duration<float> inter_update_duration;
public:
PerformanceDiagnostics(std::string name);
void RegisterUpdate(bool begin_or_end);
};
class SnippetProfiler {
private:
std::unordered_map<std::string, TimePoint> print_clocks;
std::unordered_map<std::string, TimePoint> snippet_clocks;
std::unordered_map<std::string, chr::duration<float>> snippet_durations;
std::unordered_map<std::string, unsigned long> number_of_calls;
public:
SnippetProfiler(){};
void MeasureExecutionTime(std::string snippet_name, bool begin_or_end) {
if (print_clocks.find(snippet_name) == print_clocks.end()) {
print_clocks.insert({snippet_name, chr::system_clock::now()});
}
if (snippet_clocks.find(snippet_name) == snippet_clocks.end()) {
snippet_clocks.insert({snippet_name, TimePoint()});
}
if (snippet_durations.find(snippet_name) == snippet_durations.end()) {
snippet_durations.insert({snippet_name, chr::duration<float>()});
}
if (number_of_calls.find(snippet_name) == number_of_calls.end()) {
number_of_calls.insert({snippet_name, 0u});
}
TimePoint current_time = chr::system_clock::now();
TimePoint& print_clock = print_clocks.at(snippet_name);
TimePoint& snippet_clock = snippet_clocks.at(snippet_name);
chr::duration<float>& snippet_duration = snippet_durations.at(snippet_name);
unsigned long& call_count = number_of_calls.at(snippet_name);
if (begin_or_end) {
snippet_clock = current_time;
} else {
chr::duration<float> measured_duration = current_time - snippet_clock;
snippet_duration += measured_duration;
++call_count;
}
chr::duration<float> print_duration = current_time - print_clock;
if (print_duration.count() > 1.0f) {
call_count = call_count == 0u ? 1 : call_count;
std::cout << "Snippet name : " << snippet_name << ", "
<< "avg. duration : " << 1000 * snippet_duration.count() / call_count << " ms, "
<< "total duration : " << snippet_duration.count() << " s, "
<< "total calls : " << call_count << ", "
<< std::endl;
snippet_duration = 0s;
call_count = 0u;
print_clock = current_time;
}
}
};
} // namespace traffic_manager
} // namespace carla

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

@ -1,60 +0,0 @@
// 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/PipelineStage.h"
namespace carla {
namespace traffic_manager {
PipelineStage::PipelineStage(
const std::string &stage_name)
: stage_name(stage_name),
performance_diagnostics(PerformanceDiagnostics(stage_name)) {
run_stage.store(false);
}
PipelineStage::~PipelineStage() {
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() {
while (run_stage.load()){
// Receive data.
DataReceiver();
if(run_stage.load()){
performance_diagnostics.RegisterUpdate(true);
Action();
performance_diagnostics.RegisterUpdate(false);
}
if(run_stage.load()) {
DataSender();
}
}
}
} // namespace traffic_manager
} // namespace carla

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

@ -1,77 +0,0 @@
// 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 <atomic>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <iostream>
#include <memory>
#include <shared_mutex>
#include <string>
#include <thread>
#include <vector>
#include "carla/Logging.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/Messenger.h"
#include "carla/trafficmanager/PerformanceDiagnostics.h"
namespace carla {
namespace traffic_manager {
namespace chr = std::chrono;
using namespace std::chrono_literals;
/// This class provides base functionality and template for
/// various stages of the pipeline.
class PipelineStage {
private:
std::unique_ptr<std::thread> worker_thread;
protected:
/// Flag to start/stop stage.
std::atomic<bool> run_stage;
/// Stage name string.
std::string stage_name;
private:
/// Object to track stage performance.
PerformanceDiagnostics performance_diagnostics;
void Update();
protected:
/// Implement this method with the logic to receive data from
/// the previous stage(s) and distribute to Action() threads.
virtual void DataReceiver() = 0;
/// Implement this method with logic to gather results from the
/// action threads and send to next stage(s).
virtual void DataSender() = 0;
/// Implement this method with logic to process data inside the stage
virtual void Action() = 0;
public:
PipelineStage(const std::string &stage_name);
virtual ~PipelineStage();
void Start();
void Stop();
};
} // namespace traffic_manager
} // namespace carla

20
LibCarla/source/carla/trafficmanager/RandomGenerator.h Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include <random>
namespace carla {
namespace traffic_manager {
template<class T = double,
class = std::enable_if_t<std::is_floating_point<T>::value>
> class RandomGenerator {
public:
RandomGenerator(): mt{std::random_device{}()}, dist(0.0, 100.0) {}
T next() { return dist(mt); }
private:
std::mt19937 mt;
std::uniform_real_distribution<T> dist;
};
} // namespace traffic_manager
} // namespace carla

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

@ -4,6 +4,8 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/geom/Math.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {

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

@ -6,12 +6,10 @@
#pragma once
#include <stdexcept>
#include <memory.h>
#include "carla/client/Waypoint.h"
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/geom/Transform.h"
#include "carla/geom/Vector3D.h"
#include "carla/Memory.h"
@ -93,7 +91,7 @@ namespace traffic_manager {
void SetGeodesicGridId(GeoGridId _geodesic_grid_id);
GeoGridId GetGeodesicGridId();
/// Metod to retreive junction id of the waypoint.
/// Method to retreive junction id of the waypoint.
GeoGridId GetJunctionId() const;
/// Calculates the distance from the object's waypoint to the passed

83
LibCarla/source/carla/trafficmanager/SimulationState.cpp Normal file
View File

@ -0,0 +1,83 @@
#include "carla/trafficmanager/SimulationState.h"
namespace carla {
namespace traffic_manager {
SimulationState::SimulationState() {}
void SimulationState::AddActor(ActorId actor_id,
KinematicState kinematic_state,
StaticAttributes attributes,
TrafficLightState tl_state) {
actor_set.insert(actor_id);
kinematic_state_map.insert({actor_id, kinematic_state});
static_attribute_map.insert({actor_id, attributes});
tl_state_map.insert({actor_id, tl_state});
}
bool SimulationState::ContainsActor(ActorId actor_id) const {
return actor_set.find(actor_id) != actor_set.end();
}
void SimulationState::RemoveActor(ActorId actor_id) {
actor_set.erase(actor_id);
kinematic_state_map.erase(actor_id);
static_attribute_map.erase(actor_id);
tl_state_map.erase(actor_id);
}
void SimulationState::Reset() {
actor_set.clear();
kinematic_state_map.clear();
static_attribute_map.clear();
tl_state_map.clear();
}
void SimulationState::UpdateKinematicState(ActorId actor_id, KinematicState state) {
kinematic_state_map.at(actor_id) = state;
}
void SimulationState::UpdateTrafficLightState(ActorId actor_id, TrafficLightState state) {
tl_state_map.at(actor_id) = state;
}
cg::Location SimulationState::GetLocation(ActorId actor_id) const {
return kinematic_state_map.at(actor_id).location;
}
cg::Rotation SimulationState::GetRotation(ActorId actor_id) const {
return kinematic_state_map.at(actor_id).rotation;
}
cg::Vector3D SimulationState::GetHeading(ActorId actor_id) const {
return kinematic_state_map.at(actor_id).rotation.GetForwardVector();
}
cg::Vector3D SimulationState::GetVelocity(ActorId actor_id) const {
return kinematic_state_map.at(actor_id).velocity;
}
float SimulationState::GetSpeedLimit(ActorId actor_id) const {
return kinematic_state_map.at(actor_id).speed_limit;
}
bool SimulationState::IsPhysicsEnabled(ActorId actor_id) const {
return kinematic_state_map.at(actor_id).physics_enabled;
}
TrafficLightState SimulationState::GetTLS(ActorId actor_id) const {
return tl_state_map.at(actor_id);
}
ActorType SimulationState::GetType(ActorId actor_id) const {
return static_attribute_map.at(actor_id).actor_type;
}
cg::Vector3D SimulationState::GetDimensions(ActorId actor_id) const {
const StaticAttributes &attributes = static_attribute_map.at(actor_id);
return cg::Vector3D(attributes.half_length, attributes.half_width, attributes.half_height);
}
} // namespace traffic_manager
} // namespace carla

96
LibCarla/source/carla/trafficmanager/SimulationState.h Normal file
View File

@ -0,0 +1,96 @@
#pragma once
#include <unordered_set>
#include "carla/trafficmanager/DataStructures.h"
namespace carla {
namespace traffic_manager {
enum ActorType {
Vehicle,
Pedestrian,
Any
};
struct KinematicState {
cg::Location location;
cg::Rotation rotation;
cg::Vector3D velocity;
float speed_limit;
bool physics_enabled;
};
using KinematicStateMap = std::unordered_map<ActorId, KinematicState>;
struct TrafficLightState {
TLS tl_state;
bool at_traffic_light;
};
using TrafficLightStateMap = std::unordered_map<ActorId, TrafficLightState>;
struct StaticAttributes {
ActorType actor_type;
float half_length;
float half_width;
float half_height;
};
using StaticAttributeMap = std::unordered_map<ActorId, StaticAttributes>;
/// This class holds the state of all the vehicles in the simlation.
class SimulationState {
private:
// Structure to hold ids of all actors in the simulation.
std::unordered_set<ActorId> actor_set;
// Structure containing dynamic motion related state of actors.
KinematicStateMap kinematic_state_map;
// Structure containing static attributes of actors.
StaticAttributeMap static_attribute_map;
// Structure containing dynamic traffic light related state of actors.
TrafficLightStateMap tl_state_map;
public :
SimulationState();
// Method to add an actor to the simulation state.
void AddActor(ActorId actor_id,
KinematicState kinematic_state,
StaticAttributes attributes,
TrafficLightState tl_state);
// Method to verify if an actor is present currently present in the simulation state.
bool ContainsActor(ActorId actor_id) const;
// Method to remove an actor from simulation state.
void RemoveActor(ActorId actor_id);
// Method to flush all states and actors.
void Reset();
void UpdateKinematicState(ActorId actor_id, KinematicState state);
void UpdateTrafficLightState(ActorId actor_id, TrafficLightState state);
cg::Location GetLocation(const ActorId actor_id) const;
cg::Rotation GetRotation(const ActorId actor_id) const;
cg::Vector3D GetHeading(const ActorId actor_id) const;
cg::Vector3D GetVelocity(const ActorId actor_id) const;
float GetSpeedLimit(const ActorId actor_id) const;
bool IsPhysicsEnabled(const ActorId actor_id) const;
TrafficLightState GetTLS(const ActorId actor_id) const;
ActorType GetType(const ActorId actor_id) const;
cg::Vector3D GetDimensions(const ActorId actor_id) const;
};
} // namespace traffic_manager
} // namespace carla

80
LibCarla/source/carla/trafficmanager/SnippetProfiler.h Normal file
View File

@ -0,0 +1,80 @@
// 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 <chrono>
#include <string>
#include <unordered_map>
namespace carla {
namespace traffic_manager {
namespace chr = std::chrono;
using namespace chr;
using TimePoint = chr::time_point<chr::system_clock, chr::nanoseconds>;
// This class can be used to measure execution time, total call duration and number of calls
// of any code snippet by assigning it a name.
class SnippetProfiler {
private:
std::unordered_map<std::string, TimePoint> print_clocks;
std::unordered_map<std::string, TimePoint> snippet_clocks;
std::unordered_map<std::string, chr::duration<float>> snippet_durations;
std::unordered_map<std::string, unsigned long> number_of_calls;
public:
SnippetProfiler(){};
void MeasureExecutionTime(std::string snippet_name, bool begin_or_end) {
TimePoint current_time = chr::system_clock::now();
if (print_clocks.find(snippet_name) == print_clocks.end()) {
print_clocks.insert({snippet_name, current_time});
}
if (snippet_clocks.find(snippet_name) == snippet_clocks.end()) {
snippet_clocks.insert({snippet_name, current_time});
}
if (snippet_durations.find(snippet_name) == snippet_durations.end()) {
snippet_durations.insert({snippet_name, chr::duration<float>()});
}
if (number_of_calls.find(snippet_name) == number_of_calls.end()) {
number_of_calls.insert({snippet_name, 0u});
}
TimePoint &print_clock = print_clocks.at(snippet_name);
TimePoint &snippet_clock = snippet_clocks.at(snippet_name);
chr::duration<float> &snippet_duration = snippet_durations.at(snippet_name);
unsigned long &call_count = number_of_calls.at(snippet_name);
if (begin_or_end) {
snippet_clock = current_time;
} else {
chr::duration<float> measured_duration = current_time - snippet_clock;
snippet_duration += measured_duration;
++call_count;
}
chr::duration<float> print_duration = current_time - print_clock;
if (print_duration.count() > 1.0f) {
call_count = call_count == 0u ? 1 : call_count;
std::cout << "Snippet name : " << snippet_name << ", "
<< "avg. duration : " << 1000 * snippet_duration.count() / call_count << " ms, "
<< "total duration : " << snippet_duration.count() << " s, "
<< "total calls : " << call_count << ", "
<< std::endl;
snippet_duration = 0s;
call_count = 0u;
print_clock = current_time;
}
}
};
} // namespace traffic_manager
} // namespace carla

29
LibCarla/source/carla/trafficmanager/Stage.h Normal file
View File

@ -0,0 +1,29 @@
#pragma once
#include "carla/rpc/ActorId.h"
namespace carla {
namespace traffic_manager {
using ActorId = carla::rpc::ActorId;
/// Stage type interface.
class Stage {
public:
Stage() {};
Stage(const Stage&) {};
virtual ~Stage() {};
// This method should be called every update cycle
// and represents the core operation of the stage.
virtual void Update(const unsigned long index) = 0;
// This method should remove an actor from the internal state of the stage type.
virtual void RemoveActor(const ActorId actor_id) = 0;
// This method should flush all internal state of the state type.
virtual void Reset() = 0;
};
} // namespace traffic_manager
} // namespace carla

172
LibCarla/source/carla/trafficmanager/TrackTraffic.cpp Normal file
View File

@ -0,0 +1,172 @@
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/TrackTraffic.h"
namespace carla {
namespace traffic_manager {
using constants::TrackTraffic::BUFFER_STEP_THROUGH;
using constants::TrackTraffic::INV_BUFFER_STEP_THROUGH;
TrackTraffic::TrackTraffic() {}
void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id,
const std::vector<SimpleWaypointPtr> waypoints) {
DeleteActor(actor_id);
std::unordered_set<GeoGridId> current_grids;
// Step through waypoints and update grid list for actor and actor list for grids.
for (auto &waypoint : waypoints) {
UpdatePassingVehicle(waypoint->GetId(), actor_id);
GeoGridId ggid = waypoint->GetGeodesicGridId();
current_grids.insert(ggid);
if (grid_to_actors.find(ggid) != grid_to_actors.end()) {
ActorIdSet &actor_ids = grid_to_actors.at(ggid);
if (actor_ids.find(actor_id) == actor_ids.end()) {
actor_ids.insert(actor_id);
}
} else {
grid_to_actors.insert({ggid, {actor_id}});
}
}
actor_to_grids.insert({actor_id, current_grids});
}
void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buffer) {
if (!buffer.empty()) {
// Clear current actor from all grids containing itself.
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
std::unordered_set<GeoGridId> &current_grids = actor_to_grids.at(actor_id);
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);
actor_ids.erase(actor_id);
}
}
actor_to_grids.erase(actor_id);
}
// Step through buffer and update grid list for actor and actor list for grids.
std::unordered_set<GeoGridId> current_grids;
uint64_t buffer_size = buffer.size();
uint64_t step_size = static_cast<uint64_t>(static_cast<float>(buffer_size) * INV_BUFFER_STEP_THROUGH);
for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) {
GeoGridId ggid = buffer.at(std::min(i * step_size, buffer_size - 1u))->GetGeodesicGridId();
current_grids.insert(ggid);
// Add grid entry if not present.
if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
grid_to_actors.insert({ggid, {}});
}
ActorIdSet &actor_ids = grid_to_actors.at(ggid);
if (actor_ids.find(actor_id) == actor_ids.end()) {
actor_ids.insert(actor_id);
}
}
actor_to_grids.insert({actor_id, current_grids});
}
}
ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) const {
ActorIdSet actor_id_set;
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
const std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
for (auto &grid_id : grid_ids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
const ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
actor_id_set.insert(actor_ids.begin(), actor_ids.end());
}
}
}
return actor_id_set;
}
void TrackTraffic::DeleteActor(ActorId actor_id) {
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
std::unordered_set<GeoGridId> &grid_ids = actor_to_grids.at(actor_id);
for (auto &grid_id : grid_ids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
ActorIdSet &actor_ids = grid_to_actors.at(grid_id);
actor_ids.erase(actor_id);
}
}
actor_to_grids.erase(actor_id);
}
if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
WaypointIdSet waypoint_id_set = waypoint_occupied.at(actor_id);
for (const uint64_t &waypoint_id : waypoint_id_set) {
RemovePassingVehicle(waypoint_id, actor_id);
}
}
}
void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
if (actor_id_set.find(actor_id) == actor_id_set.end()) {
actor_id_set.insert(actor_id);
}
} else {
waypoint_overlap_tracker.insert({waypoint_id, {actor_id}});
}
if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
if (waypoint_id_set.find(waypoint_id) == waypoint_id_set.end()) {
waypoint_id_set.insert(waypoint_id);
}
} else {
waypoint_occupied.insert({actor_id, {waypoint_id}});
}
}
void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id);
actor_id_set.erase(actor_id);
if (actor_id_set.size() == 0) {
waypoint_overlap_tracker.erase(waypoint_id);
}
}
if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) {
WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id);
waypoint_id_set.erase(waypoint_id);
if (waypoint_id_set.size() == 0) {
waypoint_occupied.erase(actor_id);
}
}
}
ActorIdSet TrackTraffic::GetPassingVehicles(uint64_t waypoint_id) const {
if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) {
return waypoint_overlap_tracker.at(waypoint_id);
} else {
return ActorIdSet();
}
}
void TrackTraffic::Clear() {
waypoint_overlap_tracker.clear();
waypoint_occupied.clear();
actor_to_grids.clear();
grid_to_actors.clear();
}
} // namespace traffic_manager
} // namespace carla

56
LibCarla/source/carla/trafficmanager/TrackTraffic.h Normal file
View File

@ -0,0 +1,56 @@
#pragma once
#include "carla/road/RoadTypes.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {
using ActorId = carla::ActorId;
using ActorIdSet = std::unordered_set<ActorId>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using Buffer = std::deque<SimpleWaypointPtr>;
using GeoGridId = carla::road::JuncId;
// This class is used to track the waypoint occupancy of all the actors.
class TrackTraffic {
private:
/// Structure to keep track of overlapping waypoints between vehicles.
using WaypointOverlap = std::unordered_map<uint64_t, ActorIdSet>;
WaypointOverlap waypoint_overlap_tracker;
/// Structure to keep track of waypoints occupied by vehicles;
using WaypointIdSet = std::unordered_set<uint64_t>;
using WaypointOccupancyMap = std::unordered_map<ActorId, WaypointIdSet>;
WaypointOccupancyMap waypoint_occupied;
/// Geodesic grids occupied by actors's paths.
std::unordered_map<ActorId, std::unordered_set<GeoGridId>> actor_to_grids;
/// Actors currently passing through grids.
std::unordered_map<GeoGridId, ActorIdSet> grid_to_actors;
public:
TrackTraffic();
/// Methods to update, remove and retrieve vehicles passing through a waypoint.
void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
ActorIdSet GetPassingVehicles(uint64_t waypoint_id) const;
void UpdateGridPosition(const ActorId actor_id, const Buffer &buffer);
void UpdateUnregisteredGridPosition(const ActorId actor_id,
const std::vector<SimpleWaypointPtr> waypoints);
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const;
/// Method to delete actor data from tracking.
void DeleteActor(ActorId actor_id);
void Clear();
};
} // namespace traffic_manager
} // namespace carla

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

@ -1,83 +1,66 @@
// 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 <iostream>
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/LocalizationUtils.h"
#include "carla/trafficmanager/TrafficLightStage.h"
namespace carla {
namespace traffic_manager {
static const uint64_t NO_SIGNAL_PASSTHROUGH_INTERVAL = 5u;
using constants::TrafficLight::NO_SIGNAL_PASSTHROUGH_INTERVAL;
using constants::TrafficLight::DOUBLE_NO_SIGNAL_PASSTHROUGH_INTERVAL;
using constants::WaypointSelection::JUNCTION_LOOK_AHEAD;
TrafficLightStage::TrafficLightStage(
std::string stage_name,
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger,
Parameters &parameters,
cc::DebugHelper &debug_helper)
: PipelineStage(stage_name),
localization_messenger(localization_messenger),
planner_messenger(planner_messenger),
const std::vector<ActorId> &vehicle_id_list,
const SimulationState &simulation_state,
const BufferMap &buffer_map,
const Parameters &parameters,
TLFrame &output_array)
: vehicle_id_list(vehicle_id_list),
simulation_state(simulation_state),
buffer_map(buffer_map),
parameters(parameters),
debug_helper(debug_helper){
// 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() {}
void TrafficLightStage::Action() {
// 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) {
output_array(output_array) {}
void TrafficLightStage::Update(const unsigned long index) {
bool traffic_light_hazard = false;
const LocalizationToTrafficLightData &data = localization_frame->at(i);
if (!data.actor->IsAlive()) {
continue;
}
const Actor ego_actor = data.actor;
const ActorId ego_actor_id = ego_actor->GetId();
const SimpleWaypointPtr closest_waypoint = data.closest_waypoint;
const SimpleWaypointPtr look_ahead_point = data.junction_look_ahead_waypoint;
const ActorId ego_actor_id = vehicle_id_list.at(index);
const Buffer &waypoint_buffer = buffer_map.at(ego_actor_id);
const SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId();
const TimeInstance current_time = chr::system_clock::now();
const auto ego_vehicle = boost::static_pointer_cast<cc::Vehicle>(ego_actor);
TLS traffic_light_state = ego_vehicle->GetTrafficLightState();
const TrafficLightState tl_state = simulation_state.GetTLS(ego_actor_id);
const TLS traffic_light_state = tl_state.tl_state;
const bool is_at_traffic_light = tl_state.at_traffic_light;
// 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() &&
if (is_at_traffic_light &&
traffic_light_state != TLS::Green &&
parameters.GetPercentageRunningLight(boost::shared_ptr<cc::Actor>(ego_actor)) <= (rand() % 101)) {
parameters.GetPercentageRunningLight(ego_actor_id) <= pgen.next()) {
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 &&
parameters.GetPercentageRunningSign(boost::shared_ptr<cc::Actor>(ego_actor)) <= (rand() % 101)) {
else if (look_ahead_point->CheckJunction()
&& !is_at_traffic_light
&& traffic_light_state != TLS::Green
&& parameters.GetPercentageRunningSign(ego_actor_id) <= pgen.next()) {
std::lock_guard<std::mutex> lock(no_signal_negotiation_mutex);
traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_time);
}
output_array.at(index) = traffic_light_hazard;
}
bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id,
const TimeInstance current_time) {
bool traffic_light_hazard = false;
if (vehicle_last_junction.find(ego_actor_id) == vehicle_last_junction.end()) {
// Initializing new map entry for the actor with
@ -97,7 +80,7 @@ namespace traffic_manager {
const TimeInstance &previous_ticket = vehicle_last_ticket.at(ego_actor_id);
const chr::duration<double> diff = current_time - previous_ticket;
if (diff.count() > NO_SIGNAL_PASSTHROUGH_INTERVAL) {
if (diff.count() > DOUBLE_NO_SIGNAL_PASSTHROUGH_INTERVAL) {
need_to_issue_new_ticket = true;
}
}
@ -133,58 +116,19 @@ namespace traffic_manager {
if (diff.count() > 0.0) {
traffic_light_hazard = true;
}
return traffic_light_hazard;
}
TrafficLightToPlannerData &message = current_planner_frame->at(i);
message.traffic_light_hazard = traffic_light_hazard;
}
void TrafficLightStage::RemoveActor(const ActorId actor_id) {
vehicle_last_ticket.erase(actor_id);
vehicle_last_junction.erase(actor_id);
}
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()) {
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);
}
}
void TrafficLightStage::DataSender() {
localization_messenger->Pop();
planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b);
frame_selector = !frame_selector;
}
void TrafficLightStage::DrawLight(TLS traffic_light_state, const Actor &ego_actor) const {
const cg::Location ego_location = ego_actor->GetLocation();
if (traffic_light_state == TLS::Green) {
debug_helper.DrawString(
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) {
debug_helper.DrawString(
cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f),
"Yellow",
false,
{255u, 255u, 0u}, 0.1f, true);
} else {
debug_helper.DrawString(
cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f),
"Red",
false,
{255u, 0u, 0u}, 0.1f, true);
}
void TrafficLightStage::Reset() {
vehicle_last_ticket.clear();
vehicle_last_junction.clear();
junction_last_ticket.clear();
}
} // namespace traffic_manager

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

@ -1,93 +1,47 @@
// 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 <chrono>
#include <memory>
#include <mutex>
#include <unordered_map>
#include "carla/client/ActorList.h"
#include "carla/client/TrafficLight.h"
#include "carla/client/Vehicle.h"
#include "carla/client/World.h"
#include "carla/Memory.h"
#include "carla/rpc/TrafficLightState.h"
#include "carla/trafficmanager/MessengerAndDataTypes.h"
#include "carla/trafficmanager/DataStructures.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PipelineStage.h"
#include "carla/trafficmanager/RandomGenerator.h"
#include "carla/trafficmanager/SimulationState.h"
#include "carla/trafficmanager/Stage.h"
namespace carla {
namespace traffic_manager {
namespace chr = std::chrono;
namespace cc = carla::client;
namespace cg = carla::geom;
using ActorId = carla::ActorId;
using Actor = carla::SharedPtr<cc::Actor>;
using JunctionID = carla::road::JuncId;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using TrafficLight = carla::SharedPtr<cc::TrafficLight>;
using TLS = carla::rpc::TrafficLightState;
using TimeInstance = chr::time_point<chr::system_clock, chr::nanoseconds>;
/// This class provides the information about the Traffic Lights at the
/// junctions.
class TrafficLightStage : public PipelineStage {
/// This class has functionality for responding to traffic lights
/// and managing entry into non-signalized junctions.
class TrafficLightStage: Stage {
private:
/// Selection key to switch between output frames.
bool frame_selector;
/// Pointer data frame received from localization stage.
std::shared_ptr<LocalizationToTrafficLightFrame> localization_frame;
/// Pointers to data frames to be shared with motion planner stage.
std::shared_ptr<TrafficLightToPlannerFrame> planner_frame_a;
std::shared_ptr<TrafficLightToPlannerFrame> planner_frame_b;
/// Pointers to messenger objects.
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger;
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger;
/// Runtime parameterization object.
Parameters &parameters;
/// Reference to Carla's debug helper object.
cc::DebugHelper &debug_helper;
const std::vector<ActorId> &vehicle_id_list;
const SimulationState &simulation_state;
const BufferMap &buffer_map;
const Parameters &parameters;
/// Map containing the time ticket issued for vehicles.
std::unordered_map<ActorId, TimeInstance> vehicle_last_ticket;
/// Map containing the previous time ticket issued for junctions.
std::unordered_map<JunctionID, TimeInstance> junction_last_ticket;
/// Map containing the previous junction visited by a vehicle.
std::unordered_map<ActorId, JunctionID> vehicle_last_junction;
/// No signal negotiation mutex.
std::mutex no_signal_negotiation_mutex;
/// Number of vehicles registered with the traffic manager.
uint64_t number_of_vehicles;
TLFrame &output_array;
RandomGenerator<> pgen;
void DrawLight(TLS traffic_light_state, const Actor &ego_actor) const;
bool HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id,
const TimeInstance current_time);
public:
TrafficLightStage(const std::vector<ActorId> &vehicle_id_list,
const SimulationState &Simulation_state,
const BufferMap &buffer_map,
const Parameters &parameters,
TLFrame &output_array);
TrafficLightStage(
std::string stage_name,
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger,
Parameters &parameters,
cc::DebugHelper &debug_helper);
~TrafficLightStage();
void Update(const unsigned long index) override;
void DataReceiver() override;
void Action() override;
void DataSender() override;
void RemoveActor(const ActorId actor_id) override;
void Reset() override;
};
} // namespace traffic_manager

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

@ -8,6 +8,8 @@
#include "carla/Sockets.h"
#include "carla/client/detail/Simulator.h"
#include "carla/trafficmanager/TrafficManager.h"
#include "carla/trafficmanager/TrafficManagerBase.h"
#include "carla/trafficmanager/TrafficManagerLocal.h"

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

@ -11,11 +11,14 @@
#include <vector>
#include "carla/client/Actor.h"
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/TrafficManagerBase.h"
namespace carla {
namespace traffic_manager {
using constants::Networking::TM_DEFAULT_PORT;
using ActorPtr = carla::SharedPtr<carla::client::Actor>;
/// This class integrates all the various stages of

3
LibCarla/source/carla/trafficmanager/TrafficManagerBase.h
View File

@ -9,9 +9,6 @@
#include <memory>
#include "carla/client/Actor.h"
#define MIN_TRY_COUNT 20
#define TM_DEFAULT_PORT 8000
namespace carla {
namespace traffic_manager {

10
LibCarla/source/carla/trafficmanager/TrafficManagerClient.h
View File

@ -6,15 +6,17 @@
#pragma once
#include "carla/client/Actor.h"
#include <rpc/client.h>
#include "carla/trafficmanager/Constants.h"
#include "carla/rpc/Actor.h"
#define TM_TIMEOUT 2000 // In ms
#define TM_DEFAULT_PORT 8000 // TM_SERVER_PORT
#include <rpc/client.h>
namespace carla {
namespace traffic_manager {
using constants::Networking::TM_TIMEOUT;
using constants::Networking::TM_DEFAULT_PORT;
/// Provides communication with the rpc of TrafficManagerServer.
class TrafficManagerClient {

353
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp
View File

@ -4,149 +4,274 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/client/detail/Simulator.h"
#include "carla/trafficmanager/TrafficManagerLocal.h"
namespace carla {
namespace traffic_manager {
using namespace constants::FrameMemory;
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,
cc::detail::EpisodeProxy &episode_proxy,
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);
episode_proxy(episode_proxy),
world(cc::World(episode_proxy)),
debug_helper(world.MakeDebugHelper()),
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,
localization_stage(LocalizationStage(vehicle_id_list,
buffer_map,
simulation_state,
track_traffic,
local_map,
parameters,
localization_frame,
debug_helper)),
collision_stage(CollisionStage(vehicle_id_list,
simulation_state,
buffer_map,
track_traffic,
parameters,
collision_frame,
debug_helper)),
traffic_light_stage(TrafficLightStage(vehicle_id_list,
simulation_state,
buffer_map,
parameters,
tl_frame)),
motion_plan_stage(MotionPlanStage(vehicle_id_list,
simulation_state,
parameters,
buffer_map,
track_traffic,
longitudinal_PID_parameters,
longitudinal_highway_PID_parameters,
lateral_PID_parameters,
lateral_highway_PID_parameters,
debug_helper);
localization_frame,
collision_frame,
tl_frame,
control_frame)),
control_stage = std::make_unique<BatchControlStage>(
"Batch control stage",
planner_control_messenger,
episodeProxyTM,
parameters);
alsm(ALSM(registered_vehicles,
buffer_map,
track_traffic,
parameters,
world,
local_map,
simulation_state,
localization_stage,
collision_stage,
traffic_light_stage,
motion_plan_stage)),
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();
server(TrafficManagerServer(RPCportTM, static_cast<carla::traffic_manager::TrafficManagerBase *>(this))) {
localization_stage->Start();
collision_stage->Start();
traffic_light_stage->Start();
planner_stage->Start();
control_stage->Start();
parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit);
registered_vehicles_state = -1;
SetupLocalMap();
Start();
}
TrafficManagerLocal::~TrafficManagerLocal() {
episode_proxy.Lock()->DestroyTrafficManager(server.port());
Release();
}
void TrafficManagerLocal::SetupLocalMap() {
const carla::SharedPtr<cc::Map> world_map = world.GetMap();
local_map = std::make_shared<InMemoryMap>(world_map);
local_map->SetUp();
}
void TrafficManagerLocal::Start() {
run_traffic_manger.store(true);
worker_thread = std::make_unique<std::thread>(&TrafficManagerLocal::Run, this);
}
void TrafficManagerLocal::Run() {
localization_frame.reserve(INITIAL_SIZE);
collision_frame.reserve(INITIAL_SIZE);
tl_frame.reserve(INITIAL_SIZE);
control_frame.reserve(INITIAL_SIZE);
current_reserved_capacity = INITIAL_SIZE;
while (run_traffic_manger.load()) {
bool synchronous_mode = parameters.GetSynchronousMode();
bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
// Wait for external trigger to initiate cycle in synchronous mode.
if (synchronous_mode) {
std::unique_lock<std::mutex> lock(step_execution_mutex);
step_begin_trigger.wait(lock, [this]() {return step_begin.load();});
step_begin.store(false);
}
// Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode.
if (!synchronous_mode && hybrid_physics_mode) {
TimePoint current_instance = chr::system_clock::now();
chr::duration<float> elapsed_time = current_instance - previous_update_instance;
float time_to_wait = HYBRID_MODE_DT - elapsed_time.count();
if (time_to_wait > 0.0f) {
std::this_thread::sleep_for(chr::duration<float>(time_to_wait));
}
previous_update_instance = current_instance;
}
// Updating simulation state, actor life cycle and performing necessary cleanup.
alsm.Update();
// Re-allocating inter-stage communication frames based on changed number of registered vehicles.
int current_registered_vehicles_state = registered_vehicles.GetState();
unsigned long number_of_vehicles = vehicle_id_list.size();
if (registered_vehicles_state != current_registered_vehicles_state || number_of_vehicles != registered_vehicles.Size()) {
vehicle_id_list = registered_vehicles.GetIDList();
number_of_vehicles = vehicle_id_list.size();
// Reserve more space if needed.
uint64_t growth_factor = static_cast<uint64_t>(static_cast<float>(number_of_vehicles) * INV_GROWTH_STEP_SIZE);
uint64_t new_frame_capacity = INITIAL_SIZE + GROWTH_STEP_SIZE * growth_factor;
if (new_frame_capacity > current_reserved_capacity) {
localization_frame.reserve(new_frame_capacity);
collision_frame.reserve(new_frame_capacity);
tl_frame.reserve(new_frame_capacity);
control_frame.reserve(new_frame_capacity);
}
registered_vehicles_state = registered_vehicles.GetState();
}
// Reset frames for current cycle.
localization_frame.clear();
localization_frame.resize(number_of_vehicles);
collision_frame.clear();
collision_frame.resize(number_of_vehicles);
tl_frame.clear();
tl_frame.resize(number_of_vehicles);
control_frame.clear();
control_frame.resize(number_of_vehicles);
// Run core operation stages.
for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
localization_stage.Update(index);
}
for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
collision_stage.Update(index);
traffic_light_stage.Update(index);
motion_plan_stage.Update(index);
}
collision_stage.ClearCycleCache();
// Building the command array for current cycle.
std::vector<carla::rpc::Command> batch_command(number_of_vehicles);
for (unsigned long i = 0u; i < number_of_vehicles; ++i) {
batch_command.at(i) = control_frame.at(i);
}
// Sending the current cycle's batch command to the simulator.
episode_proxy.Lock()->ApplyBatchSync(std::move(batch_command), false);
if (synchronous_mode) {
step_end.store(true);
step_end_trigger.notify_one();
}
}
}
bool TrafficManagerLocal::SynchronousTick() {
if (parameters.GetSynchronousMode()) {
step_begin.store(true);
step_begin_trigger.notify_one();
std::unique_lock<std::mutex> lock(step_execution_mutex);
step_end_trigger.wait(lock, [this]() { return step_end.load(); });
step_end.store(false);
}
return true;
}
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();
run_traffic_manger.store(false);
if (worker_thread) {
if (worker_thread->joinable()) {
worker_thread->join();
}
worker_thread.release();
}
vehicle_id_list.clear();
registered_vehicles.Clear();
registered_vehicles_state = -1;
track_traffic.Clear();
previous_update_instance = chr::system_clock::now();
current_reserved_capacity = 0u;
simulation_state.Reset();
localization_stage.Reset();
collision_stage.Reset();
traffic_light_stage.Reset();
motion_plan_stage.Reset();
buffer_map.clear();
localization_frame.clear();
collision_frame.clear();
tl_frame.clear();
control_frame.clear();
run_traffic_manger.store(true);
step_begin.store(false);
step_end.store(false);
}
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();
local_map.reset();
}
void TrafficManagerLocal::Reset() {
Release();
carla::client::detail::EpisodeProxy episode_proxy = episodeProxyTM.Lock()->GetCurrentEpisode();
episodeProxyTM = episode_proxy;
episode_proxy = episode_proxy.Lock()->GetCurrentEpisode();
world = cc::World(episode_proxy);
SetupLocalMap();
Start();
}
void TrafficManagerLocal::RegisterVehicles(const std::vector<ActorPtr>& actor_list) {
registered_actors.Insert(actor_list);
void TrafficManagerLocal::RegisterVehicles(const std::vector<ActorPtr> &vehicle_list) {
registered_vehicles.Insert(vehicle_list);
}
void TrafficManagerLocal::UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
registered_actors.Remove(actor_list);
std::vector<ActorId> actor_id_list;
for (auto &actor : actor_list) {
actor_id_list.push_back(actor->GetId());
}
registered_vehicles.Remove(actor_id_list);
}
void TrafficManagerLocal::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
@ -216,28 +341,16 @@ bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) {
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*");
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) {
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();
for (auto iter = world_traffic_lights->begin(); iter != world_traffic_lights->end(); iter++) {
auto tl = *iter;
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());
@ -261,7 +374,6 @@ void TrafficManagerLocal::ResetAllTrafficLights() {
tln->Freeze(true);
}
}
}
void TrafficManagerLocal::SetSynchronousMode(bool mode) {
@ -272,19 +384,12 @@ void TrafficManagerLocal::SetSynchronousModeTimeOutInMiliSecond(double time) {
parameters.SetSynchronousModeTimeOutInMiliSecond(time);
}
bool TrafficManagerLocal::SynchronousTick() {
localization_stage->RunStep();
control_stage->RunStep();
return true;
}
carla::client::detail::EpisodeProxy &TrafficManagerLocal::GetEpisodeProxy() {
return episodeProxyTM;
return episode_proxy;
}
std::vector<ActorId> TrafficManagerLocal::GetRegisteredVehiclesIDs() {
return registered_actors.GetIDList();
return registered_vehicles.GetIDList();
}
} // namespace traffic_manager

121
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h
View File

@ -6,95 +6,135 @@
#pragma once
#include <atomic>
#include <chrono>
#include <thread>
#include <vector>
#include "carla/client/detail/Simulator.h"
#include "carla/client/DebugHelper.h"
#include "carla/client/detail/EpisodeProxy.h"
#include "carla/client/TrafficLight.h"
#include "carla/client/World.h"
#include "carla/Memory.h"
#include "carla/rpc/Command.h"
#include "carla/trafficmanager/AtomicActorSet.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/SimulationState.h"
#include "carla/trafficmanager/TrackTraffic.h"
#include "carla/trafficmanager/TrafficManagerBase.h"
#include "carla/trafficmanager/TrafficManagerServer.h"
#include "carla/trafficmanager/ALSM.h"
#include "carla/trafficmanager/LocalizationStage.h"
#include "carla/trafficmanager/CollisionStage.h"
#include "carla/trafficmanager/TrafficLightStage.h"
#include "carla/trafficmanager/MotionPlanStage.h"
namespace carla {
namespace traffic_manager {
using ActorPtr = carla::SharedPtr<carla::client::Actor>;
namespace chr = std::chrono;
using namespace std::chrono_literals;
using TimePoint = chr::time_point<chr::system_clock, chr::nanoseconds>;
using TLGroup = std::vector<carla::SharedPtr<carla::client::TrafficLight>>;
using LocalMapPtr = std::shared_ptr<InMemoryMap>;
using constants::HybridMode::HYBRID_MODE_DT;
/// 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::client::detail::EpisodeProxy episode_proxy;
/// Carla client and object.
cc::World world;
/// Set of all actors registered with traffic manager.
AtomicActorSet registered_vehicles;
/// State counter to track changes in registered actors.
int registered_vehicles_state;
/// List of vehicles registered with the traffic manager in
/// current update cycle.
std::vector<ActorId> vehicle_id_list;
/// Pointer to local map cache.
LocalMapPtr local_map;
/// Structures to hold waypoint buffers for all vehicles.
BufferMap buffer_map;
/// 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;
cc::DebugHelper debug_helper;
/// Object for tracking paths of the traffic vehicles.
TrackTraffic track_traffic;
/// Type containing the current state of all actors involved in the simulation.
SimulationState simulation_state;
/// Time instance used to calculate dt in asynchronous mode.
TimePoint previous_update_instance;
/// Parameterization object.
Parameters parameters;
/// Array to hold output data of localization stage.
LocalizationFrame localization_frame;
/// Array to hold output data of collision avoidance.
CollisionFrame collision_frame;
/// Array to hold output data of traffic light response.
TLFrame tl_frame;
/// Array to hold output data of motion planning.
ControlFrame control_frame;
/// Variable to keep track of currently reserved array space for frames.
uint64_t current_reserved_capacity {0u};
/// Various stages representing core operations of traffic manager.
LocalizationStage localization_stage;
CollisionStage collision_stage;
TrafficLightStage traffic_light_stage;
MotionPlanStage motion_plan_stage;
ALSM alsm;
/// Traffic manager server instance.
TrafficManagerServer server;
/// Switch to turn on / turn off traffic manager.
std::atomic<bool> run_traffic_manger{true};
/// Flags to signal step begin and end.
std::atomic<bool> step_begin{false};
std::atomic<bool> step_end{false};
/// Mutex for progressing synchronous execution.
std::mutex step_execution_mutex;
/// Condition variables for progressing synchronous execution.
std::condition_variable step_begin_trigger;
std::condition_variable step_end_trigger;
/// Single worker thread for sequential execution of sub-components.
std::unique_ptr<std::thread> worker_thread;
/// 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,
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,
cc::detail::EpisodeProxy &episode_proxy,
uint16_t &RPCportTM);
/// Destructor.
virtual ~TrafficManagerLocal();
/// Method to setup InMemoryMap.
void SetupLocalMap();
/// To start the TrafficManager.
void Start();
/// Initiates thread to run the TrafficManager sequentially.
void Run();
/// To stop the TrafficManager.
void Stop();
@ -174,7 +214,6 @@ namespace traffic_manager {
/// Method to set hybrid physics radius.
void SetHybridPhysicsRadius(const float radius);
};
} // namespace traffic_manager

5
LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp
View File

@ -4,10 +4,7 @@
// 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/client/detail/Simulator.h"
#include "carla/trafficmanager/TrafficManagerRemote.h"

4
LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h
View File

@ -7,12 +7,10 @@
#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"
@ -21,8 +19,6 @@ 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.

3
LibCarla/source/carla/trafficmanager/TrafficManagerServer.h
View File

@ -12,6 +12,7 @@
#include "carla/client/Actor.h"
#include "carla/client/detail/ActorVariant.h"
#include "carla/rpc/Server.h"
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/TrafficManagerBase.h"
namespace carla {
@ -19,6 +20,8 @@ namespace traffic_manager {
using ActorPtr = carla::SharedPtr<carla::client::Actor>;
using namespace constants::Networking;
class TrafficManagerServer {
public:

4
PythonAPI/carla/source/libcarla/Actor.cpp
View File

@ -17,6 +17,8 @@
#include <ostream>
#include <iostream>
namespace ctm = carla::traffic_manager;
namespace carla {
namespace client {
@ -117,7 +119,7 @@ void export_actor() {
.def("get_light_state", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetLightState))
.def("apply_physics_control", &cc::Vehicle::ApplyPhysicsControl, (arg("physics_control")))
.def("get_physics_control", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetPhysicsControl))
.def("set_autopilot", CALL_WITHOUT_GIL_2(cc::Vehicle, SetAutopilot, bool, uint16_t), (arg("enabled") = true, arg("tm_port") = TM_DEFAULT_PORT))
.def("set_autopilot", CALL_WITHOUT_GIL_2(cc::Vehicle, SetAutopilot, bool, uint16_t), (arg("enabled") = true, arg("tm_port") = ctm::TM_DEFAULT_PORT))
.def("get_speed_limit", &cc::Vehicle::GetSpeedLimit)
.def("get_traffic_light_state", &cc::Vehicle::GetTrafficLightState)
.def("is_at_traffic_light", &cc::Vehicle::IsAtTrafficLight)

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

@ -15,6 +15,8 @@
#include <boost/python/stl_iterator.hpp>
namespace ctm = carla::traffic_manager;
static void SetTimeout(carla::client::Client &client, double seconds) {
client.SetTimeout(TimeDurationFromSeconds(seconds));
}
@ -192,6 +194,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))
.def("get_trafficmanager", CONST_CALL_WITHOUT_GIL_1(cc::Client, GetInstanceTM, uint16_t), (arg("port")=ctm::TM_DEFAULT_PORT))
;
}