117 lines
4.2 KiB
C++
117 lines
4.2 KiB
C++
|
|
#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/RandomGenerator.h"
|
|
#include "carla/trafficmanager/SimulationState.h"
|
|
#include "carla/trafficmanager/TrafficLightStage.h"
|
|
#include "carla/trafficmanager/VehicleLightStage.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 ®istered_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;
|
|
// Array of vehicles marked by stages for removal.
|
|
std::vector<ActorId>& marked_for_removal;
|
|
const Parameters ¶meters;
|
|
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;
|
|
VehicleLightStage &vehicle_light_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;
|
|
std::unordered_map<ActorId, bool> has_physics_enabled;
|
|
|
|
// 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);
|
|
|
|
// Method to identify actors newly spawned in the simulation since last tick.
|
|
void 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 UpdateData(const bool hybrid_physics_mode, const Actor &vehicle,
|
|
const bool hero_actor_present, const float physics_radius_square);
|
|
|
|
void UpdateUnregisteredActorsData();
|
|
|
|
public:
|
|
ALSM(AtomicActorSet ®istered_vehicles,
|
|
BufferMap &buffer_map,
|
|
TrackTraffic &track_traffic,
|
|
std::vector<ActorId>& marked_for_removal,
|
|
const Parameters ¶meters,
|
|
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,
|
|
VehicleLightStage &vehicle_light_stage);
|
|
|
|
void Update();
|
|
|
|
// 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);
|
|
|
|
void Reset();
|
|
};
|
|
|
|
} // namespace traffic_manager
|
|
} // namespace carla
|