104 lines
3.3 KiB
C++
104 lines
3.3 KiB
C++
|
|
#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::UpdateKinematicHybridEndLocation(ActorId actor_id, cg::Location location) {
|
|
kinematic_state_map.at(actor_id).hybrid_end_location = location;
|
|
}
|
|
|
|
void SimulationState::UpdateTrafficLightState(ActorId actor_id, TrafficLightState state) {
|
|
// The green-yellow state transition is not notified to the vehicle. This is done to avoid
|
|
// having vehicles stopped very near the intersection when only the rear part of the vehicle
|
|
// is colliding with the trigger volume of the traffic light.
|
|
auto previous_tl_state = GetTLS(actor_id);
|
|
if (previous_tl_state.at_traffic_light && previous_tl_state.tl_state == TLS::Green) {
|
|
state.tl_state = TLS::Green;
|
|
}
|
|
|
|
tl_state_map.at(actor_id) = state;
|
|
}
|
|
|
|
cg::Location SimulationState::GetLocation(ActorId actor_id) const {
|
|
return kinematic_state_map.at(actor_id).location;
|
|
}
|
|
|
|
cg::Location SimulationState::GetHybridEndLocation(ActorId actor_id) const {
|
|
return kinematic_state_map.at(actor_id).hybrid_end_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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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
|