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

Added map key check

This commit is contained in:
Guillermo 2022-05-31 08:59:58 +02:00 committed by glopezdiest
parent d96dc20852
commit e228aa0fff
1 changed files with 48 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,22 +53,24 @@ void TrafficLightStage::Update(const unsigned long index) {
}
// The vehicle is currently at a non signalized junction, so handle its priorities.
// Don't use the next condition as bounding boxes might switch to green
else if (vehicle_last_junction.find(ego_actor_id) != vehicle_last_junction.end()){
else if (vehicle_last_junction.find(ego_actor_id) != vehicle_last_junction.end())
{
if (!look_ahead_point->CheckJunction()) {
// Very close to the junction exit, forget about it.
RemoveActor(ego_actor_id);
}
else {
auto junction_id = junction->GetId();
auto& exiting_vehicles = exiting_vehicles_map.at(junction_id);
if (std::find(exiting_vehicles.begin(), exiting_vehicles.end(), ego_actor_id) != exiting_vehicles.end()) {
// The vehicle is exitting the junction.
traffic_light_hazard = false;
}
else {
// Vehicle entering the junction, handle it
traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction, waypoint_buffer, current_timestamp);
if (exiting_vehicles_map.find(junction_id) != exiting_vehicles_map.end()){
auto& exiting_vehicles = exiting_vehicles_map.at(junction_id);
if (std::find(exiting_vehicles.begin(), exiting_vehicles.end(), ego_actor_id) != exiting_vehicles.end()) {
// The vehicle is exitting the junction.
traffic_light_hazard = false;
}
else {
// Vehicle entering the junction, handle it
traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction, waypoint_buffer, current_timestamp);
}
}
}
}
@ -115,42 +117,48 @@ bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id,
bool traffic_light_hazard = false;
auto junction_id = junction->GetId();
auto& entering_vehicles = entering_vehicles_map.at(junction_id);
auto& exiting_vehicles = exiting_vehicles_map.at(junction_id);
if (entering_vehicles_map.find(junction_id) == entering_vehicles_map.end())
{
auto& entering_vehicles = entering_vehicles_map.at(junction_id);
if (vehicle_stop_time.find(ego_actor_id) == vehicle_stop_time.end()) {
// Ensure the vehicle stops before doing anything else
if (simulation_state.GetVelocity(ego_actor_id).Length() < EPSILON_RELATIVE_SPEED) {
vehicle_stop_time.insert({ego_actor_id, timestamp});
}
traffic_light_hazard = true;
}
if (exiting_vehicles_map.find(junction_id) == exiting_vehicles_map.end())
{
auto& exiting_vehicles = exiting_vehicles_map.at(junction_id);
else if (entering_vehicles.front() == ego_actor_id) {
auto entry_elapsed_seconds = vehicle_stop_time.at(ego_actor_id).elapsed_seconds;
if (timestamp.elapsed_seconds - entry_elapsed_seconds < MINIMUM_STOP_TIME) {
// Wait at least the minimum amount of time before entering the junction
traffic_light_hazard = true;
}
else {
// Track the first actor until it has passed the mid-point
cg::Transform actor_transform = waypoint_buffer.front()->GetTransform();
cg::Vector3D forward_vec = actor_transform.GetForwardVector();
cg::Vector3D to_center_vec = junction->GetBoundingBox().location - actor_transform.location;
if (vehicle_stop_time.find(ego_actor_id) == vehicle_stop_time.end()) {
// Ensure the vehicle stops before doing anything else
if (simulation_state.GetVelocity(ego_actor_id).Length() < EPSILON_RELATIVE_SPEED) {
vehicle_stop_time.insert({ego_actor_id, timestamp});
}
traffic_light_hazard = true;
}
if (cg::Math::Dot(forward_vec, to_center_vec) < EXIT_JUNCTION_THRESHOLD) {
// Remove it from the entry data, letting the next one enter it
entering_vehicles.pop_front();
vehicle_stop_time.erase(ego_actor_id);
exiting_vehicles.push_back(ego_actor_id);
else if (entering_vehicles.front() == ego_actor_id) {
auto entry_elapsed_seconds = vehicle_stop_time.at(ego_actor_id).elapsed_seconds;
if (timestamp.elapsed_seconds - entry_elapsed_seconds < MINIMUM_STOP_TIME) {
// Wait at least the minimum amount of time before entering the junction
traffic_light_hazard = true;
}
else {
// Track the first actor until it has passed the mid-point
cg::Transform actor_transform = waypoint_buffer.front()->GetTransform();
cg::Vector3D forward_vec = actor_transform.GetForwardVector();
cg::Vector3D to_center_vec = junction->GetBoundingBox().location - actor_transform.location;
if (cg::Math::Dot(forward_vec, to_center_vec) < EXIT_JUNCTION_THRESHOLD) {
// Remove it from the entry data, letting the next one enter it
entering_vehicles.pop_front();
vehicle_stop_time.erase(ego_actor_id);
exiting_vehicles.push_back(ego_actor_id);
}
}
} else {
// Only one vehicle can be entering the junction, so stop the rest.
traffic_light_hazard = true;
}
}
} else {
// Only one vehicle can be entering the junction, so stop the rest.
traffic_light_hazard = true;
}
return traffic_light_hazard;
}