Traffic Manager for Large Maps (#4324)
* Added LargeMapManager * Added DVector Vector of doubles * Declaration of interface * Added AUncenteredPivotPointMesh * Added map generation from assets folder * Updates on FDVector * Added tile load on demand * Fixed load tile issues after rebasing origin * Updated FDVector * Added some auxiliary functions * Added conversion and operator * Added methods to check and configure the state * Removed const and updated printed info * Added macro to avoid logs * Added info dump functions * The asset spawn check was added to the tick * LargeMapManager added to CarlaGM * Moving to level load autocontrol * Updated with new function name, AddNewClientToConsider * Fixing forced tile load * Added operator*(float) * Disabled Localplayer * Now the streaming is handled by us * Fixed remove of considered actors * Fixed typo * Added MapOrigin to the Episode * Added Vector3DInt to LibCarla * Removed floats in Vector3DInt * Added offset to client actor's location * Fixed old cast warning * Removed float * Added explicit cast to avoid warning * Fixed client actor transform * Removed unnecessary code * Added origin offset to spawn_actor * Adding spawn points generation * Added spawn points folder in editor * Changed Spawnpoint for only Transforms * World rebase is triggered directly * Added offset when attaching actors * Detecting hero vehicle * Added LargeMapManager * Added DVector Vector of doubles * Declaration of interface * Added AUncenteredPivotPointMesh * Added map generation from assets folder * Updates on FDVector * Added tile load on demand * Fixed load tile issues after rebasing origin * Updated FDVector * Added some auxiliary functions * Added conversion and operator * Added methods to check and configure the state * Removed const and updated printed info * Added macro to avoid logs * Added info dump functions * The asset spawn check was added to the tick * LargeMapManager added to CarlaGM * Moving to level load autocontrol * Updated with new function name, AddNewClientToConsider * Fixing forced tile load * Added operator*(float) * Disabled Localplayer * Now the streaming is handled by us * Fixed remove of considered actors * Fixed typo * Added MapOrigin to the Episode * Added Vector3DInt to LibCarla * Removed floats in Vector3DInt * Added offset to client actor's location * Fixed old cast warning * Removed float * Added explicit cast to avoid warning * Fixed client actor transform * Removed unnecessary code * Added origin offset to spawn_actor * Adding spawn points generation * Added spawn points folder in editor * Changed Spawnpoint for only Transforms * World rebase is triggered directly * Added offset when attaching actors * Detecting hero vehicle * Only hero vehicles are considered to stream tiles * Little update on log macro * Adding support for ghost actors * Refactored LargeMap classes * Refactored LargeMap classes * Revert refactor * Remove files * Added dormant state to actors * Fixed spawn+rebase on new OnActorSpawn * Removed circular dependency warning * Converted FActorView into an USTRUCT * Added actor state conversion to dormant * Removed ConvertActorDormant * Added spawn and transition of dormants * Ghost to Dormant working * Replicated dormant actors * Add substraction operators * Added dormant to ghost conversion * Fixed some issues in dormant to ghost conversion * WIP! Attached actors not working properly * WIP! Attached actors not working properly * Changing several constant variables Removing sorting in LocalizationStage.cpp * Fixing variable name * Fixing variable name * WIP: Fixing hybrid mode * Added tiles loaded from umaps. * Fixed Missing includes * Debug settings are still on. Added extra points in tight curves for PID improvement. Fixed Hybrid Mode, now moves in the direction of the next waypoint. Reduced calculations in MotionPlanningStage.cpp * Removed all debug helper functions * Added FActorData classes to save and restore actor state for dormant actors. Edited all client - server calls to recognize the dormant state. FActorView now can hold null AActor* when is dormant. Large Maps now load tiled umaps instead of assets. * Fixed dormant check for attached actors * Expanded FActorData classes. Added world observer aware of dormant actors. * Extended FActorData classes with walker data. * Fixed missing includes * Removed some with editor macros. * Added virtual destructor to FActorData. * Adding includes * Fixed warnings. Added UNoWorldOffsetSceneComponent to have unreal actors unnafected by rebase. * Fixed tile alingment. * Added sensor data to restore the comunication. * Added parent hierarchy consideration to sleep/wakeup of ghost/dormant actors. * Fixed crash for sensor destruction. * Changed InMemoryMap to generate more points if two waypoints are even further than a multiple of the limit. Removed SetSimulatePhysics call when not needed. Changed PID functions to inline. * Fixed child-parent when waking up dormant actors. Added profiling traces. Fixed transformation of debug shapes in large maps. * WIP: Relocating TM-controlled vehicles near the hero vehicle * WIP: Vehicle teleportation near hero vehicle. Missing a way to place them in a better order around the map. Fixes non-deterministic seed setup. * Fixes bug where the hero vehicle was also set to not have physics. * Introduces parameter SetRespawnDormantVehicles WIP Respawning dormant vehicles * Fixed child-parent when waking up dormant actors. Added profiling traces. Fixed transformation of debug shapes in large maps. * WIP: renaming FActorView to FCarlaActor. Changing calls related to FCarlaActor to acomodate new typing. * Renaming FActorView to FCarlaActor. Changing calls related to FCarlaActor to acomodate new typing. * Now traffic lights can be spawned without controllers inside junctions. * Added functions to interface the FCarlaActor object with recorder. Started adapting recorder to new FCarlaActor object. * Fixed missing includes. * Updated CarlaRecorder and CarlaReplayer to use new FCarlaActor interface for vehicles, walkers and sensors. * Fixed missing includes. * Streaming level list now is persistant and can be saved for unreal to detect the dependencies. * Enabled packages for large maps. * Added Tiles to the MapPath.txt file. Removed debug warnings. * WIP: Fix seed when setting deterministic mode. WIP: Respawn actors when dormant Added --hero and --respawn to spawn_npc.py * Fixed traffic light trigger box placement in large maps. * Fixed traffic light warning. Set speed limit for dormant actors to 30. Fixed semantic segmentation on streaming levels. * added server bind for set_respawn_dormant_vehicles parameter * TM doesn't destroy hero vehicle if idle. ALSM stage will now update hero vehicles first. MP stage will treat dormant vehicles like physics-less in hybrid mode. Added fixed seed increment in deterministic mode. * Fixed Traffic Light issue with recorder and dormant state. Removed old ActorView files. * handles scenario where the hero vehicle does not exist * Fixing line markings semantics * Fixed recorder issues with traffic lights. * Filter out the base large map as available map * Added tile_stream_distance and actor_stream_distance to world settings. * removed unused comments. * Added IsActive functions. Dormant actors that cannot be respawned now remain dormant. * Fixed missing includes. * Fixed missing includes * Review fixes. Missing includes. * Fixed link errors. * Fixes issue with Hybrid mode in async mode. Introduces new set_boundaries_respawn_dormant_vehicles parameter. Fixes issue when reloading map. * WIP: New algorithm to move vehicles from dormant to active * Finished new policy for teleporting vehicles. Created new generate_traffic.py script, which works in sync mode. Removed spawn_npc.py script. General cleanup in Traffic Manager. * Cleanup. * Set FXAA antialiasing method * Added automatic generation of speed limit signals. * Adding some includes * Fixed speed limit component crash when waking up. * Fixed speed limit component wrong check. * Fixed speed limit not reset when waking up actors. * Fixed generating waypoints when angle is tight * Added warning when using asynchronous mode in generate traffic. Lane changes now require more space. Vehicle switching from no physics to physics in hybrid mode will have an initial speed. Changed lower bound to 20 meters for respawn in Large Maps. * Calculating TLStage even when dormant Co-authored-by: doterop <daniel.oterop@gmail.com> Co-authored-by: Axel <axellopez92@outlook.com> Co-authored-by: bernat <bernatx@gmail.com> Co-authored-by: Joel Moriana <joel.moriana@gmail.com> Co-authored-by: Axel1092 <lopez1092@hotmail.com> Co-authored-by: Axel1092 <35765780+Axel1092@users.noreply.github.com>
This commit is contained in:
Jacopo Bartiromo
GitHub
parent
458fddb9b8
commit
6efe45f04d
|
|
@ -67,10 +67,13 @@ void ALSM::Update() {
|
|||
}
|
||||
|
||||
// Invalidate hero actor if it is not alive anymore.
|
||||
if (hybrid_physics_mode && hero_actors.size() != 0u) {
|
||||
if (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()) {
|
||||
if (destroyed_unregistered.find(hero_actor_info.first) != destroyed_unregistered.end()) {
|
||||
hero_actors_to_delete.insert(hero_actor_info.first);
|
||||
}
|
||||
if (destroyed_registered.find(hero_actor_info.first) != destroyed_registered.end()) {
|
||||
hero_actors_to_delete.insert(hero_actor_info.first);
|
||||
}
|
||||
}
|
||||
|
|
@ -80,23 +83,11 @@ void ALSM::Update() {
|
|||
}
|
||||
}
|
||||
|
||||
// Scan for new unregistered actors.
|
||||
ALSM::ActorVector new_actors = IdentifyNewActors(world_actors);
|
||||
for (const ActorPtr &actor: new_actors) {
|
||||
unregistered_actors.insert({actor->GetId(), actor});
|
||||
// Remove taken waypoints from last frame.
|
||||
track_traffic.ClearTakenWaypoints();
|
||||
|
||||
// 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});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// Scan for new unregistered actors.
|
||||
IdentifyNewActors(world_actors);
|
||||
|
||||
// Update dynamic state and static attributes for all registered vehicles.
|
||||
ALSM::IdleInfo max_idle_time = std::make_pair(0u, current_timestamp.elapsed_seconds);
|
||||
|
|
@ -104,7 +95,8 @@ void ALSM::Update() {
|
|||
|
||||
// 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) {
|
||||
&& (current_timestamp.elapsed_seconds - elapsed_last_actor_destruction) > DELTA_TIME_BETWEEN_DESTRUCTIONS
|
||||
&& hero_actors.find(max_idle_time.first) == hero_actors.end()) {
|
||||
registered_vehicles.Destroy(max_idle_time.first);
|
||||
RemoveActor(max_idle_time.first, true);
|
||||
elapsed_last_actor_destruction = current_timestamp.elapsed_seconds;
|
||||
|
|
@ -123,17 +115,26 @@ void ALSM::Update() {
|
|||
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) {
|
||||
void ALSM::IdentifyNewActors(const ActorList &actor_list) {
|
||||
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);
|
||||
// Identify any new hero vehicle
|
||||
if (actor->GetTypeId().front() == 'v') {
|
||||
if (hero_actors.size() == 0u || hero_actors.find(actor_id) == hero_actors.end()) {
|
||||
for (auto&& attribute: actor->GetAttributes()) {
|
||||
if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") {
|
||||
hero_actors.insert({actor_id, actor});
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!registered_vehicles.Contains(actor_id)
|
||||
&& unregistered_actors.find(actor_id) == unregistered_actors.end()) {
|
||||
|
||||
unregistered_actors.insert({actor_id, actor});
|
||||
}
|
||||
}
|
||||
}
|
||||
return new_actors;
|
||||
}
|
||||
|
||||
ALSM::DestroyeddActors ALSM::IdentifyDestroyedActors(const ActorList &actor_list) {
|
||||
|
|
@ -171,80 +172,109 @@ ALSM::DestroyeddActors ALSM::IdentifyDestroyedActors(const ActorList &actor_list
|
|||
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;
|
||||
bool hero_actor_present = hero_actors.size() != 0u;
|
||||
float physics_radius = parameters.GetHybridPhysicsRadius();
|
||||
float physics_radius_square = SQUARE(physics_radius);
|
||||
bool is_respawn_vehicles = parameters.GetRespawnDormantVehicles();
|
||||
if (is_respawn_vehicles && !hero_actor_present) {
|
||||
track_traffic.SetHeroLocation(cg::Location(0,0,0));
|
||||
}
|
||||
// Update first the information regarding any hero vehicle.
|
||||
for (auto &hero_actor_info: hero_actors){
|
||||
if (is_respawn_vehicles) {
|
||||
track_traffic.SetHeroLocation(hero_actor_info.second->GetTransform().location);
|
||||
}
|
||||
UpdateData(hybrid_physics_mode, max_idle_time, hero_actor_info.second, hero_actor_present, physics_radius_square);
|
||||
}
|
||||
// Update information for all other registered vehicles.
|
||||
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});
|
||||
if (hero_actors.find(actor_id) == hero_actors.end()) {
|
||||
UpdateData(hybrid_physics_mode, max_idle_time, vehicle, hero_actor_present, physics_radius_square);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
void ALSM::UpdateData(const bool hybrid_physics_mode,
|
||||
ALSM::IdleInfo &max_idle_time, const Actor &vehicle,
|
||||
const bool hero_actor_present, const float physics_radius_square) {
|
||||
|
||||
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 && hybrid_physics_mode) {
|
||||
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;
|
||||
if (!has_physics_enabled.count(actor_id) || has_physics_enabled[actor_id] != enable_physics) {
|
||||
bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true;
|
||||
if (!has_physics_enabled.count(actor_id) || has_physics_enabled[actor_id] != enable_physics) {
|
||||
if (hero_actors.find(actor_id) == hero_actors.end()) {
|
||||
vehicle->SetSimulatePhysics(enable_physics);
|
||||
has_physics_enabled[actor_id] = 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;
|
||||
if (enable_physics == true && simulation_state.ContainsActor(actor_id)) {
|
||||
vehicle->SetTargetVelocity(simulation_state.GetVelocity(actor_id));
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
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, vehicle->IsDormant()};
|
||||
|
||||
// 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) {
|
||||
|
||||
|
|
@ -256,7 +286,8 @@ void ALSM::UpdateUnregisteredActorsData() {
|
|||
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};
|
||||
const bool actor_is_dormant = actor_ptr->IsDormant();
|
||||
KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true, actor_is_dormant};
|
||||
|
||||
TrafficLightState tl_state;
|
||||
ActorType actor_type = ActorType::Any;
|
||||
|
|
|
|||
|
|
@ -72,9 +72,8 @@ private:
|
|||
// Method to determine if a vehicle is stuck at a place for too long.
|
||||
bool IsVehicleStuck(const ActorId& actor_id);
|
||||
|
||||
using ActorVector = std::vector<ActorPtr>;
|
||||
// Method to identify actors newly spawned in the simulation since last tick.
|
||||
ActorVector IdentifyNewActors(const ActorList &actor_list);
|
||||
void IdentifyNewActors(const ActorList &actor_list);
|
||||
|
||||
using DestroyeddActors = std::pair<ActorIdSet, ActorIdSet>;
|
||||
// Method to identify actors deleted in the last frame.
|
||||
|
|
@ -84,6 +83,10 @@ private:
|
|||
using IdleInfo = std::pair<ActorId, double>;
|
||||
void UpdateRegisteredActorsData(const bool hybrid_physics_mode, IdleInfo &max_idle_time);
|
||||
|
||||
void UpdateData(const bool hybrid_physics_mode,
|
||||
ALSM::IdleInfo &max_idle_time, const Actor &vehicle,
|
||||
const bool hero_actor_present, const float physics_radius_square);
|
||||
|
||||
void UpdateUnregisteredActorsData();
|
||||
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -31,8 +31,8 @@ static const double DELTA_TIME_BETWEEN_DESTRUCTIONS = 10.0;
|
|||
} // namespace VehicleRemoval
|
||||
|
||||
namespace HybridMode {
|
||||
static const float HYBRID_MODE_DT_FL = 0.05f;
|
||||
static const double HYBRID_MODE_DT = 0.05;
|
||||
static const float HYBRID_MODE_DT_FL = 0.1f;
|
||||
static const double HYBRID_MODE_DT = 0.1;
|
||||
static const double INV_HYBRID_DT = 1.0 / HYBRID_MODE_DT;
|
||||
static const float PHYSICS_RADIUS = 50.0f;
|
||||
} // namespace HybridMode
|
||||
|
|
@ -63,7 +63,7 @@ static const float MIN_SAFE_INTERVAL_LENGTH = 1.5f * SAFE_DISTANCE_AFTER_JUNCTIO
|
|||
} // namespace WaypointSelection
|
||||
|
||||
namespace LaneChange {
|
||||
static const float MINIMUM_LANE_CHANGE_DISTANCE = 15.0f;
|
||||
static const float MINIMUM_LANE_CHANGE_DISTANCE = 18.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;
|
||||
|
|
@ -71,7 +71,7 @@ static const float INTER_LANE_CHANGE_DISTANCE = 10.0f;
|
|||
|
||||
namespace Collision {
|
||||
static const float BOUNDARY_EXTENSION_MAXIMUM = 40.0f;
|
||||
static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f;
|
||||
static const float BOUNDARY_EXTENSION_MINIMUM = 2.5f;
|
||||
static const float BOUNDARY_EXTENSION_RATE = RATE(BOUNDARY_EXTENSION_MAXIMUM,
|
||||
BOUNDARY_EXTENSION_MINIMUM,
|
||||
SpeedThreshold::ARBITRARY_MAX_SPEED);
|
||||
|
|
@ -99,7 +99,9 @@ static const float MAX_GEODESIC_GRID_LENGTH = 20.0f;
|
|||
static const double MAP_RESOLUTION = 5.0;
|
||||
static const double MAX_WPT_DISTANCE = MAP_RESOLUTION/2.0 + SQUARE(MAP_RESOLUTION);
|
||||
static const float INV_MAP_RESOLUTION = 0.2f;
|
||||
static const float SEVEN_DEG_TO_RAD = 0.122f;
|
||||
static const float TWENTY_DEG_TO_RAD = 0.35f;
|
||||
static float const DELTA = 25.0f;
|
||||
static float const Z_DELTA = 500.0f;
|
||||
} // namespace Map
|
||||
|
||||
namespace TrafficLight {
|
||||
|
|
@ -117,6 +119,8 @@ static const float FOLLOW_DISTANCE_RATE = RATE(MAX_FOLLOW_LEAD_DISTANCE,
|
|||
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;
|
||||
static const float TWO_KM = 2000.0f;
|
||||
static const uint16_t ATTEMPTS_TO_TELEPORT = 5u;
|
||||
} // namespace MotionPlan
|
||||
|
||||
namespace PID {
|
||||
|
|
|
|||
|
|
@ -6,11 +6,11 @@
|
|||
|
||||
#include "carla/trafficmanager/Constants.h"
|
||||
#include "carla/trafficmanager/InMemoryMap.h"
|
||||
#include <boost/geometry/geometries/box.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace traffic_manager {
|
||||
|
||||
|
||||
namespace cg = carla::geom;
|
||||
using namespace constants::Map;
|
||||
|
||||
|
|
@ -28,9 +28,10 @@ namespace traffic_manager {
|
|||
return GetSegmentId(swp->GetWaypoint());
|
||||
}
|
||||
|
||||
std::vector<SimpleWaypointPtr> InMemoryMap::GetSuccessors(const SegmentId segment_id,
|
||||
const SegmentTopology &segment_topology, const SegmentMap &segment_map) {
|
||||
std::vector<SimpleWaypointPtr> result;
|
||||
NodeList InMemoryMap::GetSuccessors(const SegmentId segment_id,
|
||||
const SegmentTopology &segment_topology,
|
||||
const SegmentMap &segment_map) {
|
||||
NodeList result;
|
||||
if (segment_topology.find(segment_id) == segment_topology.end()) {
|
||||
return result;
|
||||
}
|
||||
|
|
@ -46,9 +47,10 @@ namespace traffic_manager {
|
|||
return result;
|
||||
}
|
||||
|
||||
std::vector<SimpleWaypointPtr> InMemoryMap::GetPredecessors(const SegmentId segment_id,
|
||||
const SegmentTopology &segment_topology, const SegmentMap &segment_map) {
|
||||
std::vector<SimpleWaypointPtr> result;
|
||||
NodeList InMemoryMap::GetPredecessors(const SegmentId segment_id,
|
||||
const SegmentTopology &segment_topology,
|
||||
const SegmentMap &segment_map) {
|
||||
NodeList result;
|
||||
if (segment_topology.find(segment_id) == segment_topology.end()) {
|
||||
return result;
|
||||
}
|
||||
|
|
@ -139,7 +141,7 @@ namespace traffic_manager {
|
|||
auto compare_s = [](const SimpleWaypointPtr &swp1, const SimpleWaypointPtr &swp2) {
|
||||
return (swp1->GetWaypoint()->GetDistance() < swp2->GetWaypoint()->GetDistance());
|
||||
};
|
||||
auto wpt_angle = [](cg::Location l1, cg::Location l2) {
|
||||
auto wpt_angle = [](cg::Vector3D l1, cg::Vector3D l2) {
|
||||
return cg::Math::GetVectorAngle(l1, l2);
|
||||
};
|
||||
auto max = [](int16_t x, int16_t y) {
|
||||
|
|
@ -163,13 +165,13 @@ namespace traffic_manager {
|
|||
// Adding more waypoints if the angle is too tight or if they are too distant.
|
||||
for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
|
||||
float distance = distance_squared(segment_waypoints.at(i)->GetLocation(), segment_waypoints.at(i+1)->GetLocation());
|
||||
double angle = wpt_angle(segment_waypoints.at(i)->GetLocation(), segment_waypoints.at(i+1)->GetLocation());
|
||||
int16_t angle_splits = static_cast<int16_t>(angle/SEVEN_DEG_TO_RAD);
|
||||
double angle = wpt_angle(segment_waypoints.at(i)->GetTransform().rotation.GetForwardVector(), segment_waypoints.at(i+1)->GetTransform().rotation.GetForwardVector());
|
||||
int16_t angle_splits = static_cast<int16_t>(angle/TWENTY_DEG_TO_RAD);
|
||||
int16_t distance_splits = static_cast<int16_t>(distance/MAX_WPT_DISTANCE);
|
||||
auto max_splits = max(angle_splits, distance_splits);
|
||||
if (max_splits >= 1) {
|
||||
// Compute how many waypoints do we need to generate.
|
||||
for (auto j = 0; j < max_splits; ++j) {
|
||||
for (uint16_t j = 0; j < max_splits; ++j) {
|
||||
auto next_waypoints = segment_waypoints.at(i)->GetWaypoint()->GetNext(std::sqrt(distance)/(max_splits+1));
|
||||
if (next_waypoints.size() != 0) {
|
||||
auto new_waypoint = next_waypoints.front();
|
||||
|
|
@ -266,13 +268,42 @@ namespace traffic_manager {
|
|||
std::vector<SpatialTreeEntry> result_1;
|
||||
|
||||
rtree.query(bgi::nearest(query_point, 1), std::back_inserter(result_1));
|
||||
|
||||
SpatialTreeEntry &closest_entry = result_1.front();
|
||||
SimpleWaypointPtr &closest_point = closest_entry.second;
|
||||
|
||||
return closest_point;
|
||||
}
|
||||
|
||||
std::vector<SimpleWaypointPtr> InMemoryMap::GetDenseTopology() const {
|
||||
NodeList InMemoryMap::GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const {
|
||||
Point3D query_point(loc.x, loc.y, loc.z);
|
||||
|
||||
Point3D lower_p1(loc.x + random_sample, loc.y + random_sample, loc.z + Z_DELTA);
|
||||
Point3D lower_p2(loc.x - random_sample, loc.y - random_sample, loc.z - Z_DELTA);
|
||||
Point3D upper_p1(loc.x + random_sample + DELTA, loc.y + random_sample + DELTA, loc.z + Z_DELTA);
|
||||
Point3D upper_p2(loc.x - random_sample - DELTA, loc.y - random_sample - DELTA, loc.z - Z_DELTA);
|
||||
|
||||
Box lower_query_box(lower_p2, lower_p1);
|
||||
Box upper_query_box(upper_p2, upper_p1);
|
||||
|
||||
NodeList result;
|
||||
uint8_t x = 0;
|
||||
for (Rtree::const_query_iterator
|
||||
it = rtree.qbegin(bgi::within(upper_query_box)
|
||||
&& !bgi::within(lower_query_box)
|
||||
&& bgi::satisfies([&](SpatialTreeEntry const& v) { return !v.second->CheckJunction();}));
|
||||
it != rtree.qend();
|
||||
++it) {
|
||||
x++;
|
||||
result.push_back(it->second);
|
||||
if (x >= n_points)
|
||||
break;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
NodeList InMemoryMap::GetDenseTopology() const {
|
||||
return dense_topology;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
|
|
@ -21,6 +22,7 @@
|
|||
#include "carla/Memory.h"
|
||||
#include "carla/road/RoadTypes.h"
|
||||
|
||||
#include "carla/trafficmanager/RandomGenerator.h"
|
||||
#include "carla/trafficmanager/SimpleWaypoint.h"
|
||||
|
||||
namespace carla {
|
||||
|
|
@ -39,11 +41,13 @@ namespace bgi = boost::geometry::index;
|
|||
using WorldMap = carla::SharedPtr<cc::Map>;
|
||||
|
||||
using Point3D = bg::model::point<float, 3, bg::cs::cartesian>;
|
||||
using Box = bg::model::box<Point3D>;
|
||||
using SpatialTreeEntry = std::pair<Point3D, SimpleWaypointPtr>;
|
||||
|
||||
using SegmentId = std::tuple<crd::RoadId, crd::LaneId, crd::SectionId>;
|
||||
using SegmentTopology = std::map<SegmentId, std::pair<std::vector<SegmentId>, std::vector<SegmentId>>>;
|
||||
using SegmentMap = std::map<SegmentId, std::vector<SimpleWaypointPtr>>;
|
||||
using Rtree = bgi::rtree<SpatialTreeEntry, bgi::rstar<16>>;
|
||||
|
||||
/// This class builds a discretized local map-cache.
|
||||
/// Instantiate the class with the world and run SetUp() to construct the
|
||||
|
|
@ -58,7 +62,7 @@ namespace bgi = boost::geometry::index;
|
|||
/// sparse topology.
|
||||
NodeList dense_topology;
|
||||
/// Spatial quadratic R-tree for indexing and querying waypoints.
|
||||
bgi::rtree<SpatialTreeEntry, bgi::rstar<16>> rtree;
|
||||
Rtree rtree;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -72,6 +76,9 @@ namespace bgi = boost::geometry::index;
|
|||
/// This method returns the closest waypoint to a given location on the map.
|
||||
SimpleWaypointPtr GetWaypoint(const cg::Location loc) const;
|
||||
|
||||
/// This method returns n waypoints in an delta area with a certain distance from the ego vehicle.
|
||||
NodeList GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const;
|
||||
|
||||
/// This method returns the full list of discrete samples of the map in the
|
||||
/// local cache.
|
||||
NodeList GetDenseTopology() const;
|
||||
|
|
@ -83,10 +90,12 @@ namespace bgi = boost::geometry::index;
|
|||
/// This method is used to find and place lane change links.
|
||||
void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);
|
||||
|
||||
std::vector<SimpleWaypointPtr> GetSuccessors(const SegmentId segment_id,
|
||||
const SegmentTopology &segment_topology, const SegmentMap &segment_map);
|
||||
std::vector<SimpleWaypointPtr> GetPredecessors(const SegmentId segment_id,
|
||||
const SegmentTopology &segment_topology, const SegmentMap &segment_map);
|
||||
NodeList GetSuccessors(const SegmentId segment_id,
|
||||
const SegmentTopology &segment_topology,
|
||||
const SegmentMap &segment_map);
|
||||
NodeList 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.
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ MotionPlanStage::MotionPlanStage(
|
|||
const SimulationState &simulation_state,
|
||||
const Parameters ¶meters,
|
||||
const BufferMap &buffer_map,
|
||||
const TrackTraffic &track_traffic,
|
||||
TrackTraffic &track_traffic,
|
||||
const std::vector<float> &urban_longitudinal_parameters,
|
||||
const std::vector<float> &highway_longitudinal_parameters,
|
||||
const std::vector<float> &urban_lateral_parameters,
|
||||
|
|
@ -29,8 +29,10 @@ MotionPlanStage::MotionPlanStage(
|
|||
const CollisionFrame&collision_frame,
|
||||
const TLFrame &tl_frame,
|
||||
const cc::World &world,
|
||||
ControlFrame &output_array)
|
||||
: vehicle_id_list(vehicle_id_list),
|
||||
ControlFrame &output_array,
|
||||
RandomGeneratorMap &random_devices,
|
||||
const LocalMapPtr &local_map)
|
||||
: vehicle_id_list(vehicle_id_list),
|
||||
simulation_state(simulation_state),
|
||||
parameters(parameters),
|
||||
buffer_map(buffer_map),
|
||||
|
|
@ -43,149 +45,201 @@ MotionPlanStage::MotionPlanStage(
|
|||
collision_frame(collision_frame),
|
||||
tl_frame(tl_frame),
|
||||
world(world),
|
||||
output_array(output_array) {}
|
||||
output_array(output_array),
|
||||
random_devices(random_devices),
|
||||
local_map(local_map) {}
|
||||
|
||||
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 cg::Location vehicle_location = simulation_state.GetLocation(actor_id);
|
||||
const cg::Vector3D vehicle_velocity = simulation_state.GetVelocity(actor_id);
|
||||
const float vehicle_speed = vehicle_velocity.Length();
|
||||
const cg::Vector3D vehicle_heading = simulation_state.GetHeading(actor_id);
|
||||
const bool vehicle_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);
|
||||
current_timestamp = world.GetSnapshot().GetTimestamp();
|
||||
StateEntry current_state;
|
||||
|
||||
|
||||
// Instanciating teleportation transform.
|
||||
cg::Transform teleportation_transform;
|
||||
|
||||
// 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;
|
||||
// Get information about the hero location from the actor_id state.
|
||||
cg::Location hero_location = track_traffic.GetHeroLocation();
|
||||
bool is_hero_alive = hero_location != cg::Location(0, 0, 0);
|
||||
|
||||
// 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;
|
||||
|
||||
if (ego_physics_enabled) {
|
||||
ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
|
||||
|
||||
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{current_timestamp, 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;
|
||||
}
|
||||
|
||||
// If physics is enabled for the vehicle, use PID controller.
|
||||
// State update for vehicle.
|
||||
current_state = PID::StateUpdate(previous_state, ego_speed, dynamic_target_velocity,
|
||||
current_deviation, current_timestamp);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// Constructing the actuation signal.
|
||||
|
||||
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);
|
||||
|
||||
// Updating PID state.
|
||||
StateEntry &state = pid_state_map.at(actor_id);
|
||||
state = current_state;
|
||||
|
||||
}
|
||||
// For physics-less vehicles, determine position and orientation for teleportation.
|
||||
else {
|
||||
if (simulation_state.IsDormant(actor_id) && parameters.GetRespawnDormantVehicles() && is_hero_alive) {
|
||||
// Flushing controller state for vehicle.
|
||||
current_state = {current_timestamp,
|
||||
0.0f, 0.0f,
|
||||
0.0f, 0.0f};
|
||||
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, current_timestamp});
|
||||
}
|
||||
|
||||
// Get lower and upper bound for teleporting vehicle.
|
||||
float lower_bound = parameters.GetLowerBoundaryRespawnDormantVehicles();
|
||||
float upper_bound = parameters.GetUpperBoundaryRespawnDormantVehicles();
|
||||
float dilate_factor = (upper_bound-lower_bound)/100.0f;
|
||||
|
||||
// Measuring time elapsed since last teleportation for the vehicle.
|
||||
double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
|
||||
|
||||
// Find a location ahead of the vehicle for teleportation to achieve intended velocity.
|
||||
if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT)) {
|
||||
|
||||
// Target displacement magnitude to achieve target velocity.
|
||||
const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT_FL;
|
||||
SimpleWaypointPtr teleport_target = waypoint_buffer.front();
|
||||
cg::Transform target_base_transform = teleport_target->GetTransform();
|
||||
cg::Location target_base_location = target_base_transform.location;
|
||||
cg::Vector3D target_heading = target_base_transform.GetForwardVector();
|
||||
cg::Vector3D correct_heading = (target_base_location - ego_location).MakeSafeUnitVector(EPSILON);
|
||||
|
||||
if (ego_location.Distance(target_base_location) < target_displacement) {
|
||||
cg::Location teleportation_location = ego_location + cg::Location(target_heading.MakeSafeUnitVector(EPSILON) * target_displacement);
|
||||
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
|
||||
}
|
||||
else {
|
||||
cg::Location teleportation_location = ego_location + cg::Location(correct_heading * target_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.
|
||||
if (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT) {
|
||||
float random_sample = (static_cast<float>(random_devices.at(actor_id).next())*dilate_factor) + lower_bound;
|
||||
NodeList teleport_waypoint_list = local_map->GetWaypointsInDelta(hero_location, ATTEMPTS_TO_TELEPORT, random_sample);
|
||||
if (!teleport_waypoint_list.empty()) {
|
||||
teleportation_transform = teleport_waypoint_list.at(0)->GetTransform();
|
||||
for (auto &teleport_waypoint : teleport_waypoint_list) {
|
||||
uint64_t waypoint_id = teleport_waypoint->GetId();
|
||||
if (track_traffic.IsWaypointFree(waypoint_id)) {
|
||||
teleportation_transform = teleport_waypoint->GetTransform();
|
||||
track_traffic.AddTakenWaypoint(waypoint_id);
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// No possible points were found, stay dormant.
|
||||
teleportation_transform = cg::Transform(vehicle_location, simulation_state.GetRotation(actor_id));
|
||||
}
|
||||
} else {
|
||||
teleportation_transform = cg::Transform(ego_location, simulation_state.GetRotation(actor_id));
|
||||
// Teleport only once every dt in asynchronous mode.
|
||||
teleportation_transform = cg::Transform(vehicle_location, simulation_state.GetRotation(actor_id));
|
||||
}
|
||||
// Constructing the actuation signal.
|
||||
output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
|
||||
}
|
||||
|
||||
else {
|
||||
|
||||
// Target velocity for vehicle.
|
||||
const float vehicle_speed_limit = simulation_state.GetSpeedLimit(actor_id);
|
||||
float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, vehicle_speed_limit) / 3.6f;
|
||||
|
||||
// Collision handling and target velocity correction.
|
||||
std::pair<bool, float> collision_response = CollisionHandling(collision_hazard, tl_hazard, vehicle_velocity,
|
||||
vehicle_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;
|
||||
|
||||
if (vehicle_physics_enabled && !simulation_state.IsDormant(actor_id)) {
|
||||
ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
|
||||
|
||||
const float target_point_distance = std::max(vehicle_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(vehicle_location, vehicle_heading, target_location);
|
||||
float cross_product = DeviationCrossProduct(vehicle_location, vehicle_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{current_timestamp, 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 (vehicle_speed > HIGHWAY_SPEED) {
|
||||
longitudinal_parameters = highway_longitudinal_parameters;
|
||||
lateral_parameters = highway_lateral_parameters;
|
||||
} else {
|
||||
longitudinal_parameters = urban_longitudinal_parameters;
|
||||
lateral_parameters = urban_lateral_parameters;
|
||||
}
|
||||
|
||||
// If physics is enabled for the vehicle, use PID controller.
|
||||
// State update for vehicle.
|
||||
current_state = PID::StateUpdate(previous_state, vehicle_speed, dynamic_target_velocity,
|
||||
current_deviation, current_timestamp);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// Constructing the actuation signal.
|
||||
|
||||
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);
|
||||
|
||||
// Updating PID state.
|
||||
StateEntry &state = pid_state_map.at(actor_id);
|
||||
state = current_state;
|
||||
|
||||
}
|
||||
// For physics-less vehicles, determine position and orientation for teleportation.
|
||||
else {
|
||||
// Flushing controller state for vehicle.
|
||||
current_state = {current_timestamp,
|
||||
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, current_timestamp});
|
||||
}
|
||||
|
||||
// Measuring time elapsed since last teleportation for the vehicle.
|
||||
double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
|
||||
|
||||
// Find a location ahead of the vehicle for teleportation to achieve intended velocity.
|
||||
if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT)) {
|
||||
|
||||
// Target displacement magnitude to achieve target velocity.
|
||||
const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT_FL;
|
||||
SimpleWaypointPtr teleport_target = waypoint_buffer.front();
|
||||
cg::Transform target_base_transform = teleport_target->GetTransform();
|
||||
cg::Location target_base_location = target_base_transform.location;
|
||||
cg::Vector3D target_heading = target_base_transform.GetForwardVector();
|
||||
cg::Vector3D correct_heading = (target_base_location - vehicle_location).MakeSafeUnitVector(EPSILON);
|
||||
|
||||
if (vehicle_location.Distance(target_base_location) < target_displacement) {
|
||||
cg::Location teleportation_location = vehicle_location + cg::Location(target_heading.MakeSafeUnitVector(EPSILON) * target_displacement);
|
||||
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
|
||||
}
|
||||
else {
|
||||
cg::Location teleportation_location = vehicle_location + cg::Location(correct_heading * target_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(vehicle_location, simulation_state.GetRotation(actor_id));
|
||||
}
|
||||
// Constructing the actuation signal.
|
||||
output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool MotionPlanStage::SafeAfterJunction(const LocalizationData &localization,
|
||||
|
|
@ -234,8 +288,8 @@ bool MotionPlanStage::SafeAfterJunction(const LocalizationData &localization,
|
|||
|
||||
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 cg::Vector3D vehicle_velocity,
|
||||
const cg::Vector3D vehicle_heading,
|
||||
const float max_target_velocity) {
|
||||
bool collision_emergency_stop = false;
|
||||
float dynamic_target_velocity = max_target_velocity;
|
||||
|
|
@ -243,17 +297,17 @@ std::pair<bool, float> MotionPlanStage::CollisionHandling(const CollisionHazardD
|
|||
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 vehicle_relative_speed = (vehicle_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);
|
||||
const float other_speed_along_heading = cg::Math::Dot(other_velocity, vehicle_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 (vehicle_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;
|
||||
float follow_lead_distance = vehicle_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
|
||||
|
|
|
|||
|
|
@ -5,8 +5,10 @@
|
|||
#pragma once
|
||||
|
||||
#include "carla/trafficmanager/DataStructures.h"
|
||||
#include "carla/trafficmanager/InMemoryMap.h"
|
||||
#include "carla/trafficmanager/LocalizationUtils.h"
|
||||
#include "carla/trafficmanager/Parameters.h"
|
||||
#include "carla/trafficmanager/RandomGenerator.h"
|
||||
#include "carla/trafficmanager/SimulationState.h"
|
||||
#include "carla/trafficmanager/Stage.h"
|
||||
#include "carla/trafficmanager/TrackTraffic.h"
|
||||
|
|
@ -14,13 +16,15 @@
|
|||
namespace carla {
|
||||
namespace traffic_manager {
|
||||
|
||||
using LocalMapPtr = std::shared_ptr<InMemoryMap>;
|
||||
|
||||
class MotionPlanStage: Stage {
|
||||
private:
|
||||
const std::vector<ActorId> &vehicle_id_list;
|
||||
const SimulationState &simulation_state;
|
||||
const Parameters ¶meters;
|
||||
const BufferMap &buffer_map;
|
||||
const TrackTraffic &track_traffic;
|
||||
TrackTraffic &track_traffic;
|
||||
// PID paramenters for various road conditions.
|
||||
const std::vector<float> urban_longitudinal_parameters;
|
||||
const std::vector<float> highway_longitudinal_parameters;
|
||||
|
|
@ -37,6 +41,8 @@ private:
|
|||
std::unordered_map<ActorId, cc::Timestamp> teleportation_instance;
|
||||
ControlFrame &output_array;
|
||||
cc::Timestamp current_timestamp;
|
||||
RandomGeneratorMap &random_devices;
|
||||
const LocalMapPtr &local_map;
|
||||
|
||||
std::pair<bool, float> CollisionHandling(const CollisionHazardData &collision_hazard,
|
||||
const bool tl_hazard,
|
||||
|
|
@ -53,7 +59,7 @@ public:
|
|||
const SimulationState &simulation_state,
|
||||
const Parameters ¶meters,
|
||||
const BufferMap &buffer_map,
|
||||
const TrackTraffic &track_traffic,
|
||||
TrackTraffic &track_traffic,
|
||||
const std::vector<float> &urban_longitudinal_parameters,
|
||||
const std::vector<float> &highway_longitudinal_parameters,
|
||||
const std::vector<float> &urban_lateral_parameters,
|
||||
|
|
@ -62,7 +68,9 @@ public:
|
|||
const CollisionFrame &collision_frame,
|
||||
const TLFrame &tl_frame,
|
||||
const cc::World &world,
|
||||
ControlFrame &output_array);
|
||||
ControlFrame &output_array,
|
||||
RandomGeneratorMap &random_devices,
|
||||
const LocalMapPtr &local_map);
|
||||
|
||||
void Update(const unsigned long index);
|
||||
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#include "carla/trafficmanager/Parameters.h"
|
||||
#include "carla/trafficmanager/Constants.h"
|
||||
|
||||
namespace carla {
|
||||
namespace traffic_manager {
|
||||
|
|
@ -24,6 +25,21 @@ void Parameters::SetHybridPhysicsMode(const bool mode_switch) {
|
|||
hybrid_physics_mode.store(mode_switch);
|
||||
}
|
||||
|
||||
void Parameters::SetRespawnDormantVehicles(const bool mode_switch) {
|
||||
|
||||
respawn_dormant_vehicles.store(mode_switch);
|
||||
}
|
||||
|
||||
void Parameters::SetMaxBoundaries(const float lower, const float upper) {
|
||||
min_lower_bound = lower;
|
||||
max_upper_bound = upper;
|
||||
}
|
||||
|
||||
void Parameters::SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound) {
|
||||
respawn_lower_bound = min_lower_bound > lower_bound ? min_lower_bound : lower_bound;
|
||||
respawn_upper_bound = max_upper_bound < upper_bound ? max_upper_bound : upper_bound;
|
||||
}
|
||||
|
||||
void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
|
||||
|
||||
float new_percentage = std::min(100.0f, percentage);
|
||||
|
|
@ -273,6 +289,22 @@ bool Parameters::GetHybridPhysicsMode() const {
|
|||
return hybrid_physics_mode.load();
|
||||
}
|
||||
|
||||
bool Parameters::GetRespawnDormantVehicles() const {
|
||||
|
||||
return respawn_dormant_vehicles.load();
|
||||
}
|
||||
|
||||
float Parameters::GetLowerBoundaryRespawnDormantVehicles() const {
|
||||
|
||||
return respawn_lower_bound.load();
|
||||
}
|
||||
|
||||
float Parameters::GetUpperBoundaryRespawnDormantVehicles() const {
|
||||
|
||||
return respawn_upper_bound.load();
|
||||
}
|
||||
|
||||
|
||||
bool Parameters::GetOSMMode() const {
|
||||
|
||||
return osm_mode.load();
|
||||
|
|
|
|||
|
|
@ -62,6 +62,16 @@ private:
|
|||
std::atomic<float> distance_margin{2.0};
|
||||
/// Hybrid physics mode switch.
|
||||
std::atomic<bool> hybrid_physics_mode{false};
|
||||
/// Automatic respawn mode switch.
|
||||
std::atomic<bool> respawn_dormant_vehicles{false};
|
||||
/// Minimum distance to respawn vehicles with respect to the hero vehicle.
|
||||
std::atomic<float> respawn_lower_bound{100.0};
|
||||
/// Maximum distance to respawn vehicles with respect to the hero vehicle.
|
||||
std::atomic<float> respawn_upper_bound{1000.0};
|
||||
/// Minimum possible distance to respawn vehicles with respect to the hero vehicle.
|
||||
float min_lower_bound;
|
||||
/// Maximum possible distance to respawn vehicles with respect to the hero vehicle.
|
||||
float max_upper_bound;
|
||||
/// Hybrid physics radius.
|
||||
std::atomic<float> hybrid_physics_radius {70.0};
|
||||
/// Parameter specifying Open Street Map mode.
|
||||
|
|
@ -131,6 +141,15 @@ public:
|
|||
/// Method to set Open Street Map mode.
|
||||
void SetOSMMode(const bool mode_switch);
|
||||
|
||||
/// Method to set if we are automatically respawning vehicles.
|
||||
void SetRespawnDormantVehicles(const bool mode_switch);
|
||||
|
||||
/// Method to set boundaries for respawning vehicles.
|
||||
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound);
|
||||
|
||||
/// Method to set limits for boundaries when respawning vehicles.
|
||||
void SetMaxBoundaries(const float lower, const float upper);
|
||||
|
||||
///////////////////////////////// GETTERS /////////////////////////////////////
|
||||
|
||||
/// Method to retrieve hybrid physics radius.
|
||||
|
|
@ -175,6 +194,15 @@ public:
|
|||
/// Method to retrieve hybrid physics mode.
|
||||
bool GetHybridPhysicsMode() const;
|
||||
|
||||
/// Method to retrieve if we are automatically respawning vehicles.
|
||||
bool GetRespawnDormantVehicles() const;
|
||||
|
||||
/// Method to retrieve minimum distance from hero vehicle when respawning vehicles.
|
||||
float GetLowerBoundaryRespawnDormantVehicles() const;
|
||||
|
||||
/// Method to retrieve maximum distance from hero vehicle when respawning vehicles.
|
||||
float GetUpperBoundaryRespawnDormantVehicles() const;
|
||||
|
||||
/// Method to get Open Street Map mode.
|
||||
bool GetOSMMode() const;
|
||||
|
||||
|
|
|
|||
|
|
@ -66,6 +66,10 @@ bool SimulationState::IsPhysicsEnabled(ActorId actor_id) const {
|
|||
return kinematic_state_map.at(actor_id).physics_enabled;
|
||||
}
|
||||
|
||||
bool SimulationState::IsDormant(ActorId actor_id) const {
|
||||
return kinematic_state_map.at(actor_id).is_dormant;
|
||||
}
|
||||
|
||||
TrafficLightState SimulationState::GetTLS(ActorId actor_id) const {
|
||||
return tl_state_map.at(actor_id);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ struct KinematicState {
|
|||
cg::Vector3D velocity;
|
||||
float speed_limit;
|
||||
bool physics_enabled;
|
||||
bool is_dormant;
|
||||
};
|
||||
using KinematicStateMap = std::unordered_map<ActorId, KinematicState>;
|
||||
|
||||
|
|
@ -84,6 +85,10 @@ public :
|
|||
|
||||
bool IsPhysicsEnabled(const ActorId actor_id) const;
|
||||
|
||||
bool IsDormant(const ActorId actor_id) const;
|
||||
|
||||
cg::Location GetHeroLocation(const ActorId actor_id) const;
|
||||
|
||||
TrafficLightState GetTLS(const ActorId actor_id) const;
|
||||
|
||||
ActorType GetType(const ActorId actor_id) const;
|
||||
|
|
|
|||
|
|
@ -58,9 +58,12 @@ void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buff
|
|||
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();
|
||||
auto waypoint = buffer.at(std::min(i * step_size, buffer_size - 1u));
|
||||
GeoGridId ggid = waypoint->GetGeodesicGridId();
|
||||
current_grids.insert(ggid);
|
||||
|
||||
if (i == 0u && taken_waypoints.find(waypoint->GetId()) == taken_waypoints.end()) {
|
||||
taken_waypoints.insert(waypoint->GetId());
|
||||
}
|
||||
// Add grid entry if not present.
|
||||
if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
|
||||
grid_to_actors.insert({ggid, {}});
|
||||
|
|
@ -76,6 +79,28 @@ void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buff
|
|||
}
|
||||
}
|
||||
|
||||
bool TrackTraffic::IsWaypointFree(const uint64_t waypoint_id) const {
|
||||
return taken_waypoints.find(waypoint_id) == taken_waypoints.end();
|
||||
}
|
||||
|
||||
void TrackTraffic::AddTakenWaypoint(const uint64_t waypoint_id) {
|
||||
if (taken_waypoints.find(waypoint_id) == taken_waypoints.end()) {
|
||||
taken_waypoints.insert(waypoint_id);
|
||||
}
|
||||
}
|
||||
|
||||
void TrackTraffic::ClearTakenWaypoints() {
|
||||
taken_waypoints.clear();
|
||||
}
|
||||
|
||||
void TrackTraffic::SetHeroLocation(const cg::Location _location) {
|
||||
hero_location = _location;
|
||||
}
|
||||
|
||||
cg::Location TrackTraffic::GetHeroLocation() const {
|
||||
return hero_location;
|
||||
}
|
||||
|
||||
ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) const {
|
||||
ActorIdSet actor_id_set;
|
||||
|
||||
|
|
|
|||
|
|
@ -32,6 +32,11 @@ private:
|
|||
std::unordered_map<ActorId, std::unordered_set<GeoGridId>> actor_to_grids;
|
||||
/// Actors currently passing through grids.
|
||||
std::unordered_map<GeoGridId, ActorIdSet> grid_to_actors;
|
||||
/// Waypoints currently taken.
|
||||
WaypointIdSet taken_waypoints;
|
||||
/// Current hero location.
|
||||
cg::Location hero_location = cg::Location(0,0,0);
|
||||
|
||||
|
||||
public:
|
||||
TrackTraffic();
|
||||
|
|
@ -46,6 +51,14 @@ public:
|
|||
const std::vector<SimpleWaypointPtr> waypoints);
|
||||
|
||||
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const;
|
||||
bool IsWaypointFree(const uint64_t waypoint_id) const;
|
||||
void AddTakenWaypoint(const uint64_t waypoint_id);
|
||||
void ClearTakenWaypoints();
|
||||
|
||||
void SetHeroLocation(const cg::Location location);
|
||||
cg::Location GetHeroLocation() const;
|
||||
|
||||
|
||||
/// Method to delete actor data from tracking.
|
||||
void DeleteActor(ActorId actor_id);
|
||||
|
||||
|
|
|
|||
|
|
@ -58,7 +58,6 @@ void TrafficLightStage::Update(const unsigned long index) {
|
|||
|
||||
traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_timestamp);
|
||||
}
|
||||
|
||||
output_array.at(index) = traffic_light_hazard;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -65,6 +65,29 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
/// Method to set if we are automatically respawning vehicles.
|
||||
void SetRespawnDormantVehicles(const bool mode_switch) {
|
||||
TrafficManagerBase* tm_ptr = GetTM(_port);
|
||||
if (tm_ptr != nullptr) {
|
||||
tm_ptr->SetRespawnDormantVehicles(mode_switch);
|
||||
}
|
||||
}
|
||||
/// Method to set boundaries for respawning vehicles.
|
||||
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound) {
|
||||
TrafficManagerBase* tm_ptr = GetTM(_port);
|
||||
if (tm_ptr != nullptr) {
|
||||
tm_ptr->SetBoundariesRespawnDormantVehicles(lower_bound, upper_bound);
|
||||
}
|
||||
}
|
||||
|
||||
/// Method to set boundaries for respawning vehicles.
|
||||
void SetMaxBoundaries(const float lower, const float upper) {
|
||||
TrafficManagerBase* tm_ptr = GetTM(_port);
|
||||
if (tm_ptr != nullptr) {
|
||||
tm_ptr->SetMaxBoundaries(lower, upper);
|
||||
}
|
||||
}
|
||||
|
||||
/// This method sets the hybrid physics mode.
|
||||
void SetHybridPhysicsMode(const bool mode_switch) {
|
||||
TrafficManagerBase* tm_ptr = GetTM(_port);
|
||||
|
|
|
|||
|
|
@ -103,9 +103,19 @@ public:
|
|||
|
||||
/// Method to set randomization seed.
|
||||
virtual void SetRandomDeviceSeed(const uint64_t seed) = 0;
|
||||
|
||||
/// Method to set Open Street Map mode.
|
||||
virtual void SetOSMMode(const bool mode_switch) = 0;
|
||||
|
||||
/// Method to set automatic respawn of dormant vehicles.
|
||||
virtual void SetRespawnDormantVehicles(const bool mode_switch) = 0;
|
||||
|
||||
/// Method to set boundaries for respawning vehicles.
|
||||
virtual void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound) = 0;
|
||||
|
||||
/// Method to set limits for boundaries when respawning vehicles.
|
||||
virtual void SetMaxBoundaries(const float lower, const float upper) = 0;
|
||||
|
||||
virtual void ShutDown() = 0;
|
||||
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -199,6 +199,24 @@ public:
|
|||
_client->call("set_osm_mode", mode_switch);
|
||||
}
|
||||
|
||||
/// Method to set automatic respawn of dormant vehicles.
|
||||
void SetRespawnDormantVehicles(const bool mode_switch) {
|
||||
DEBUG_ASSERT(_client != nullptr);
|
||||
_client->call("set_respawn_dormant_vehicles", mode_switch);
|
||||
}
|
||||
|
||||
/// Method to set boundaries for respawning vehicles.
|
||||
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound) {
|
||||
DEBUG_ASSERT(_client != nullptr);
|
||||
_client->call("set_boundaries_respawn_dormant_vehicles", lower_bound, upper_bound);
|
||||
}
|
||||
|
||||
/// Method to set boundaries for respawning vehicles.
|
||||
void SetMaxBoundaries(const float lower, const float upper) {
|
||||
DEBUG_ASSERT(_client != nullptr);
|
||||
_client->call("set_max_boundaries", lower, upper);
|
||||
}
|
||||
|
||||
void ShutDown() {
|
||||
DEBUG_ASSERT(_client != nullptr);
|
||||
_client->call("shut_down");
|
||||
|
|
|
|||
|
|
@ -71,7 +71,9 @@ TrafficManagerLocal::TrafficManagerLocal(
|
|||
collision_frame,
|
||||
tl_frame,
|
||||
world,
|
||||
control_frame)),
|
||||
control_frame,
|
||||
random_devices,
|
||||
local_map)),
|
||||
|
||||
alsm(ALSM(registered_vehicles,
|
||||
buffer_map,
|
||||
|
|
@ -127,6 +129,7 @@ void TrafficManagerLocal::Run() {
|
|||
|
||||
bool synchronous_mode = parameters.GetSynchronousMode();
|
||||
bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
|
||||
parameters.SetMaxBoundaries(20.0f, episode_proxy.Lock()->GetEpisodeSettings().actor_active_distance);
|
||||
|
||||
// Wait for external trigger to initiate cycle in synchronous mode.
|
||||
if (synchronous_mode) {
|
||||
|
|
@ -194,6 +197,7 @@ void TrafficManagerLocal::Run() {
|
|||
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);
|
||||
}
|
||||
|
|
@ -201,7 +205,6 @@ void TrafficManagerLocal::Run() {
|
|||
collision_stage.Update(index);
|
||||
}
|
||||
collision_stage.ClearCycleCache();
|
||||
|
||||
for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
|
||||
traffic_light_stage.Update(index);
|
||||
motion_plan_stage.Update(index);
|
||||
|
|
@ -285,7 +288,7 @@ void TrafficManagerLocal::Reset() {
|
|||
Release();
|
||||
episode_proxy = episode_proxy.Lock()->GetCurrentEpisode();
|
||||
world = cc::World(episode_proxy);
|
||||
//SetupLocalMap();
|
||||
SetupLocalMap();
|
||||
Start();
|
||||
}
|
||||
|
||||
|
|
@ -293,6 +296,11 @@ void TrafficManagerLocal::RegisterVehicles(const std::vector<ActorPtr> &vehicle_
|
|||
std::lock_guard<std::mutex> registration_lock(registration_mutex);
|
||||
registered_vehicles.Insert(vehicle_list);
|
||||
for (const ActorPtr &vehicle: vehicle_list) {
|
||||
if (!is_custom_seed) {
|
||||
seed = vehicle->GetId() + seed;
|
||||
} else {
|
||||
seed = 1 + seed;
|
||||
}
|
||||
random_devices.insert({vehicle->GetId(), RandomGenerator(seed)});
|
||||
}
|
||||
}
|
||||
|
|
@ -365,6 +373,18 @@ void TrafficManagerLocal::SetOSMMode(const bool mode_switch) {
|
|||
parameters.SetOSMMode(mode_switch);
|
||||
}
|
||||
|
||||
void TrafficManagerLocal::SetRespawnDormantVehicles(const bool mode_switch) {
|
||||
parameters.SetRespawnDormantVehicles(mode_switch);
|
||||
}
|
||||
|
||||
void TrafficManagerLocal::SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound) {
|
||||
parameters.SetBoundariesRespawnDormantVehicles(lower_bound, upper_bound);
|
||||
}
|
||||
|
||||
void TrafficManagerLocal::SetMaxBoundaries(const float lower, const float upper) {
|
||||
parameters.SetMaxBoundaries(lower, upper);
|
||||
}
|
||||
|
||||
bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) {
|
||||
for (auto &elem : tl_to_freeze) {
|
||||
if (!elem->IsFrozen() || elem->GetState() != TLS::Red) {
|
||||
|
|
@ -397,6 +417,7 @@ std::vector<ActorId> TrafficManagerLocal::GetRegisteredVehiclesIDs() {
|
|||
|
||||
void TrafficManagerLocal::SetRandomDeviceSeed(const uint64_t _seed) {
|
||||
seed = _seed;
|
||||
is_custom_seed = true;
|
||||
world.ResetAllTrafficLights();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -112,6 +112,7 @@ private:
|
|||
RandomGeneratorMap random_devices;
|
||||
/// Randomization seed.
|
||||
uint64_t seed {static_cast<uint64_t>(time(NULL))};
|
||||
bool is_custom_seed {false};
|
||||
std::vector<ActorId> marked_for_removal;
|
||||
/// Mutex to prevent vehicle registration during frame array re-allocation.
|
||||
std::mutex registration_mutex;
|
||||
|
|
@ -224,6 +225,15 @@ public:
|
|||
/// Method to set Open Street Map mode.
|
||||
void SetOSMMode(const bool mode_switch);
|
||||
|
||||
/// Method to set automatic respawn of dormant vehicles.
|
||||
void SetRespawnDormantVehicles(const bool mode_switch);
|
||||
|
||||
// Method to set boundaries to respawn of dormant vehicles.
|
||||
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound);
|
||||
|
||||
// Method to set limits for boundaries when respawning dormant vehicles.
|
||||
void SetMaxBoundaries(const float lower, const float upper);
|
||||
|
||||
void ShutDown() {};
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -187,6 +187,18 @@ void TrafficManagerRemote::SetOSMMode(const bool mode_switch) {
|
|||
client.SetOSMMode(mode_switch);
|
||||
}
|
||||
|
||||
void TrafficManagerRemote::SetRespawnDormantVehicles(const bool mode_switch) {
|
||||
client.SetRespawnDormantVehicles(mode_switch);
|
||||
}
|
||||
|
||||
void TrafficManagerRemote::SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound) {
|
||||
client.SetBoundariesRespawnDormantVehicles(lower_bound, upper_bound);
|
||||
}
|
||||
|
||||
void TrafficManagerRemote::SetMaxBoundaries(const float lower, const float upper) {
|
||||
client.SetMaxBoundaries(lower, upper);
|
||||
}
|
||||
|
||||
void TrafficManagerRemote::ShutDown() {
|
||||
client.ShutDown();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -105,6 +105,15 @@ public:
|
|||
/// Method to set Open Street Map mode.
|
||||
void SetOSMMode(const bool mode_switch);
|
||||
|
||||
/// Method to set automatic respawn of dormant vehicles.
|
||||
void SetRespawnDormantVehicles(const bool mode_switch);
|
||||
|
||||
// Method to set boundaries to respawn of dormant vehicles.
|
||||
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound);
|
||||
|
||||
// Method to set boundaries to respawn of dormant vehicles.
|
||||
void SetMaxBoundaries(const float lower, const float upper);
|
||||
|
||||
virtual void ShutDown();
|
||||
|
||||
/// Method to provide synchronous tick
|
||||
|
|
|
|||
|
|
@ -168,6 +168,16 @@ public:
|
|||
tm->SetHybridPhysicsRadius(mode_switch);
|
||||
});
|
||||
|
||||
/// Method to set respawn dormant vehicles mode.
|
||||
server->bind("set_respawn_dormant_vehicles", [=](const bool mode_switch) {
|
||||
tm->SetRespawnDormantVehicles(mode_switch);
|
||||
});
|
||||
|
||||
/// Method to set respawn dormant vehicles mode.
|
||||
server->bind("set_boundaries_respawn_dormant_vehicles", [=](const float lower_bound, const float upper_bound) {
|
||||
tm->SetBoundariesRespawnDormantVehicles(lower_bound, upper_bound);
|
||||
});
|
||||
|
||||
server->bind("shut_down", [=]() {
|
||||
tm->Release();
|
||||
});
|
||||
|
|
|
|||
|
|
@ -36,5 +36,7 @@ void export_trafficmanager() {
|
|||
.def("set_hybrid_physics_radius", &ctm::TrafficManager::SetHybridPhysicsRadius)
|
||||
.def("set_random_device_seed", &ctm::TrafficManager::SetRandomDeviceSeed)
|
||||
.def("set_osm_mode", &carla::traffic_manager::TrafficManager::SetOSMMode)
|
||||
.def("set_respawn_dormant_vehicles", &carla::traffic_manager::TrafficManager::SetRespawnDormantVehicles)
|
||||
.def("set_boundaries_respawn_dormant_vehicles", &carla::traffic_manager::TrafficManager::SetBoundariesRespawnDormantVehicles)
|
||||
.def("shut_down", &ctm::TrafficManager::ShutDown);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,12 +1,12 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de
|
||||
# Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma de
|
||||
# Barcelona (UAB).
|
||||
#
|
||||
# This work is licensed under the terms of the MIT license.
|
||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
"""Spawn NPCs into the simulation"""
|
||||
"""Example script to generate traffic in the simulation"""
|
||||
|
||||
import glob
|
||||
import os
|
||||
|
|
@ -46,53 +46,69 @@ def main():
|
|||
argparser.add_argument(
|
||||
'-n', '--number-of-vehicles',
|
||||
metavar='N',
|
||||
default=10,
|
||||
default=30,
|
||||
type=int,
|
||||
help='number of vehicles (default: 10)')
|
||||
help='Number of vehicles (default: 30)')
|
||||
argparser.add_argument(
|
||||
'-w', '--number-of-walkers',
|
||||
metavar='W',
|
||||
default=50,
|
||||
default=10,
|
||||
type=int,
|
||||
help='number of walkers (default: 50)')
|
||||
help='Number of walkers (default: 10)')
|
||||
argparser.add_argument(
|
||||
'--safe',
|
||||
action='store_true',
|
||||
help='avoid spawning vehicles prone to accidents')
|
||||
help='Avoid spawning vehicles prone to accidents')
|
||||
argparser.add_argument(
|
||||
'--filterv',
|
||||
metavar='PATTERN',
|
||||
default='vehicle.*',
|
||||
help='vehicles filter (default: "vehicle.*")')
|
||||
help='Filter vehicle model (default: "vehicle.*")')
|
||||
argparser.add_argument(
|
||||
'--filterw',
|
||||
metavar='PATTERN',
|
||||
default='walker.pedestrian.*',
|
||||
help='pedestrians filter (default: "walker.pedestrian.*")')
|
||||
help='Filter pedestrian type (default: "walker.pedestrian.*")')
|
||||
argparser.add_argument(
|
||||
'--tm-port',
|
||||
metavar='P',
|
||||
default=8000,
|
||||
type=int,
|
||||
help='port to communicate with TM (default: 8000)')
|
||||
help='Port to communicate with TM (default: 8000)')
|
||||
argparser.add_argument(
|
||||
'--sync',
|
||||
'--asynch',
|
||||
action='store_true',
|
||||
help='Synchronous mode execution')
|
||||
help='Activate asynchronous mode execution')
|
||||
argparser.add_argument(
|
||||
'--hybrid',
|
||||
action='store_true',
|
||||
help='Enanble')
|
||||
help='Activate hybrid mode for Traffic Manager')
|
||||
argparser.add_argument(
|
||||
'-s', '--seed',
|
||||
metavar='S',
|
||||
type=int,
|
||||
help='Random device seed')
|
||||
help='Set random device seed and deterministic mode for Traffic Manager')
|
||||
argparser.add_argument(
|
||||
'--car-lights-on',
|
||||
action='store_true',
|
||||
default=False,
|
||||
help='Enanble car lights')
|
||||
help='Enable car lights')
|
||||
argparser.add_argument(
|
||||
'--hero',
|
||||
action='store_true',
|
||||
default=False,
|
||||
help='Set one of the vehicles as hero')
|
||||
argparser.add_argument(
|
||||
'--respawn',
|
||||
action='store_true',
|
||||
default=False,
|
||||
help='Automatically respawn dormant vehicles (only in large maps)')
|
||||
argparser.add_argument(
|
||||
'--no-rendering',
|
||||
action='store_true',
|
||||
default=False,
|
||||
help='Activate no rendering mode')
|
||||
|
||||
args = argparser.parse_args()
|
||||
|
||||
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
|
||||
|
|
@ -109,23 +125,32 @@ def main():
|
|||
world = client.get_world()
|
||||
|
||||
traffic_manager = client.get_trafficmanager(args.tm_port)
|
||||
traffic_manager.set_global_distance_to_leading_vehicle(1.0)
|
||||
traffic_manager.set_global_distance_to_leading_vehicle(2.5)
|
||||
if args.respawn:
|
||||
traffic_manager.set_respawn_dormant_vehicles(True)
|
||||
if args.hybrid:
|
||||
traffic_manager.set_hybrid_physics_mode(True)
|
||||
traffic_manager.set_hybrid_physics_radius(70.0)
|
||||
if args.seed is not None:
|
||||
traffic_manager.set_random_device_seed(args.seed)
|
||||
|
||||
|
||||
if args.sync:
|
||||
settings = world.get_settings()
|
||||
settings = world.get_settings()
|
||||
if not args.asynch:
|
||||
traffic_manager.set_synchronous_mode(True)
|
||||
if not settings.synchronous_mode:
|
||||
synchronous_master = True
|
||||
settings.synchronous_mode = True
|
||||
settings.fixed_delta_seconds = 0.05
|
||||
world.apply_settings(settings)
|
||||
else:
|
||||
synchronous_master = False
|
||||
else:
|
||||
print("You are currently in asynchronous mode. If this is a traffic simulation, \
|
||||
you could experience some issues. If it's not working correctly, switch to synchronous \
|
||||
mode by using traffic_manager.set_synchronous_mode(True)")
|
||||
|
||||
if args.no_rendering:
|
||||
settings.no_rendering_mode = True
|
||||
world.apply_settings(settings)
|
||||
|
||||
blueprints = world.get_blueprint_library().filter(args.filterv)
|
||||
blueprintsWalkers = world.get_blueprint_library().filter(args.filterw)
|
||||
|
|
@ -161,6 +186,7 @@ def main():
|
|||
# Spawn vehicles
|
||||
# --------------
|
||||
batch = []
|
||||
hero = args.hero
|
||||
for n, transform in enumerate(spawn_points):
|
||||
if n >= args.number_of_vehicles:
|
||||
break
|
||||
|
|
@ -171,7 +197,11 @@ def main():
|
|||
if blueprint.has_attribute('driver_id'):
|
||||
driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
|
||||
blueprint.set_attribute('driver_id', driver_id)
|
||||
blueprint.set_attribute('role_name', 'autopilot')
|
||||
if hero:
|
||||
blueprint.set_attribute('role_name', 'hero')
|
||||
hero = False
|
||||
else:
|
||||
blueprint.set_attribute('role_name', 'autopilot')
|
||||
|
||||
# prepare the light state of the cars to spawn
|
||||
light_state = vls.NONE
|
||||
|
|
@ -243,14 +273,14 @@ def main():
|
|||
logging.error(results[i].error)
|
||||
else:
|
||||
walkers_list[i]["con"] = results[i].actor_id
|
||||
# 4. we put altogether the walkers and controllers id to get the objects from their id
|
||||
# 4. we put together the walkers and controllers id to get the objects from their id
|
||||
for i in range(len(walkers_list)):
|
||||
all_id.append(walkers_list[i]["con"])
|
||||
all_id.append(walkers_list[i]["id"])
|
||||
all_actors = world.get_actors(all_id)
|
||||
|
||||
# wait for a tick to ensure client receives the last transform of the walkers we have just created
|
||||
if not args.sync or not synchronous_master:
|
||||
if args.asynch or not synchronous_master:
|
||||
world.wait_for_tick()
|
||||
else:
|
||||
world.tick()
|
||||
|
|
@ -268,20 +298,21 @@ def main():
|
|||
|
||||
print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list)))
|
||||
|
||||
# example of how to use parameters
|
||||
# Example of how to use Traffic Manager parameters
|
||||
traffic_manager.global_percentage_speed_difference(30.0)
|
||||
|
||||
while True:
|
||||
if args.sync and synchronous_master:
|
||||
if not args.asynch and synchronous_master:
|
||||
world.tick()
|
||||
else:
|
||||
world.wait_for_tick()
|
||||
|
||||
finally:
|
||||
|
||||
if args.sync and synchronous_master:
|
||||
if not args.asynch and synchronous_master:
|
||||
settings = world.get_settings()
|
||||
settings.synchronous_mode = False
|
||||
settings.no_rendering_mode = False
|
||||
settings.fixed_delta_seconds = None
|
||||
world.apply_settings(settings)
|
||||
|
||||
Loading…
Reference in New Issue