ALSM.cpp

Go to the documentation of this file.

 
#include <algorithm>
#include <cctype>
#include <string>
 
#include "boost/pointer_cast.hpp"
 
#include "carla/Logging.h"
#include "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
#include "carla/client/Walker.h"
 
#include "carla/trafficmanager/Constants.h"
#include "carla/trafficmanager/LocalizationUtils.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
 
#include "carla/trafficmanager/ALSM.h"
 
namespace carla {
namespace traffic_manager {
 
ALSM::ALSM(
  AtomicActorSet &registered_vehicles,
  BufferMap &buffer_map,
  TrackTraffic &track_traffic,
  std::vector<ActorId>& marked_for_removal,
  const Parameters &parameters,
  const cc::World &world,
  const LocalMapPtr &local_map,
  SimulationState &simulation_state,
  std::unordered_map<ActorId, std::pair<float, bool>> &large_vehicles,
  LocalizationStage &localization_stage,
  CollisionStage &collision_stage,
  TrafficLightStage &traffic_light_stage,
  MotionPlanStage &motion_plan_stage,
  VehicleLightStage &vehicle_light_stage)
  : registered_vehicles(registered_vehicles),
    buffer_map(buffer_map),
    track_traffic(track_traffic),
    marked_for_removal(marked_for_removal),
    parameters(parameters),
    world(world),
    local_map(local_map),
    simulation_state(simulation_state),
    localization_stage(localization_stage),
    collision_stage(collision_stage),
    traffic_light_stage(traffic_light_stage),
    motion_plan_stage(motion_plan_stage),
    vehicle_light_stage(vehicle_light_stage),
    large_vehicles(large_vehicles) {}
 
void ALSM::Update() {
 
  bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
 
  std::set<ActorId> world_vehicle_ids;
  std::set<ActorId> world_pedestrian_ids;
  std::vector<ActorId> unregistered_list_to_be_deleted;
 
  current_timestamp = world.GetSnapshot().GetTimestamp();
  ActorList world_actors = world.GetActors();
 
  // Find destroyed actors and perform clean up.
  const ALSM::DestroyeddActors destroyed_actors = IdentifyDestroyedActors(world_actors);
 
  const ActorIdSet &destroyed_registered = destroyed_actors.first;
  for (const auto &deletion_id: destroyed_registered) {
    RemoveActor(deletion_id, true);
  }
 
  const ActorIdSet &destroyed_unregistered = destroyed_actors.second;
  for (auto deletion_id : destroyed_unregistered) {
    RemoveActor(deletion_id, false);
  }
 
  // Invalidate hero actor if it is not alive anymore.
  if (hero_actors.size() != 0u) {
    ActorIdSet hero_actors_to_delete;
    for (auto &hero_actor_info: hero_actors) {
      if (destroyed_unregistered.find(hero_actor_info.first) != destroyed_unregistered.end()) {
        hero_actors_to_delete.insert(hero_actor_info.first);
      }
      if (destroyed_registered.find(hero_actor_info.first) != destroyed_registered.end()) {
        hero_actors_to_delete.insert(hero_actor_info.first);
      }
    }
 
    for (auto &deletion_id: hero_actors_to_delete) {
      hero_actors.erase(deletion_id);
    }
  }
 
  // Scan for new unregistered actors.
  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);
  UpdateRegisteredActorsData(hybrid_physics_mode, max_idle_time);
 
  // Destroy registered vehicle if stuck at a location for too long.
  if (IsVehicleStuck(max_idle_time.first)
      && (current_timestamp.elapsed_seconds - elapsed_last_actor_destruction) > DELTA_TIME_BETWEEN_DESTRUCTIONS
      && 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;
  }
 
  // Destorying vehicles for marked for removal by stages.
  if (parameters.GetOSMMode()) {
    for (const ActorId& actor_id: marked_for_removal) {
      registered_vehicles.Destroy(actor_id);
      RemoveActor(actor_id, true);
    }
    marked_for_removal.clear();
  }
 
  // Update dynamic state and static attributes for unregistered actors.
  UpdateUnregisteredActorsData();
}
 
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();
    // 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});
    }
  }
}
 
ALSM::DestroyeddActors ALSM::IdentifyDestroyedActors(const ActorList &actor_list) {
 
  ALSM::DestroyeddActors destroyed_actors;
  ActorIdSet &deleted_registered = destroyed_actors.first;
  ActorIdSet &deleted_unregistered = destroyed_actors.second;
 
  // Building hash set of actors present in current frame.
  ActorIdSet current_actors;
  for  (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
    current_actors.insert((*iter)->GetId());
  }
 
  // Searching for destroyed registered actors.
  std::vector<ActorId> registered_ids = registered_vehicles.GetIDList();
  for (const ActorId &actor_id : registered_ids) {
    if (current_actors.find(actor_id) == current_actors.end()) {
      deleted_registered.insert(actor_id);
    }
  }
 
  // Searching for destroyed unregistered actors.
  for (const auto &actor_info: unregistered_actors) {
    const ActorId &actor_id = actor_info.first;
     if (current_actors.find(actor_id) == current_actors.end()
         || registered_vehicles.Contains(actor_id)) {
      deleted_unregistered.insert(actor_id);
    }
  }
 
  return destroyed_actors;
}
 
void ALSM::UpdateRegisteredActorsData(const bool hybrid_physics_mode, ALSM::IdleInfo &max_idle_time) {
 
  std::vector<ActorPtr> vehicle_list = registered_vehicles.GetList();
  bool hero_actor_present = 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));
  }
  // A vehicle can die after the snapshot (destroyed through its handle, or by a map load): drop it, don't throw.
  std::vector<ActorId> vanished_heroes;
  std::vector<ActorId> vanished_vehicles;
  // Update first the information regarding any hero vehicle.
  for (auto &hero_actor_info: hero_actors){
    try {
      if (is_respawn_vehicles) {
        track_traffic.SetHeroLocation(hero_actor_info.second->GetTransform().location);
      }
      UpdateData(hybrid_physics_mode, hero_actor_info.second, hero_actor_present, physics_radius_square);
    } catch (const std::exception &e) {
      log_debug("Traffic Manager: dropping hero vehicle", hero_actor_info.first, ":", e.what());
      vanished_heroes.push_back(hero_actor_info.first);
    }
  }
  // Update information for all other registered vehicles.
  for (const Actor &vehicle : vehicle_list) {
    ActorId actor_id = vehicle->GetId();
    if (hero_actors.find(actor_id) == hero_actors.end()) {
      try {
        UpdateData(hybrid_physics_mode, vehicle, hero_actor_present, physics_radius_square);
      } catch (const std::exception &e) {
        log_debug("Traffic Manager: dropping vehicle", actor_id, ":", e.what());
        vanished_vehicles.push_back(actor_id);
        continue;
      }
      UpdateIdleTime(max_idle_time, actor_id);
    }
  }
  for (const ActorId actor_id : vanished_heroes) {
    RemoveActor(actor_id, false);
  }
  for (const ActorId actor_id : vanished_vehicles) {
    RemoveActor(actor_id, true);
  }
}
 
void ALSM::UpdateData(const bool hybrid_physics_mode, 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();
  bool state_entry_present = simulation_state.ContainsActor(actor_id);
 
  // 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) {
    if (hero_actors.find(actor_id) == hero_actors.end()) {
      vehicle->SetSimulatePhysics(enable_physics);
      has_physics_enabled[actor_id] = enable_physics;
      if (enable_physics == true && state_entry_present) {
        vehicle->SetTargetVelocity(simulation_state.GetVelocity(actor_id));
      }
    }
  }
 
  // If physics are disabled, calculate velocity based on change in position.
  // Do not use 'enable_physics' as turning off the physics in this tick doesn't remove the velocity.
  // To avoid issues with other clients teleporting the actors, use the previous outpout location.
  if (state_entry_present && !simulation_state.IsPhysicsEnabled(actor_id)){
    cg::Location previous_location = simulation_state.GetLocation(actor_id);
    cg::Location previous_end_location = simulation_state.GetHybridEndLocation(actor_id);
    cg::Vector3D displacement = (previous_end_location - previous_location);
    vehicle_velocity = displacement * INV_HYBRID_DT;
  }
 
  // Updated kinematic state object.
  auto vehicle_ptr = std::static_pointer_cast<cc::Vehicle>(vehicle);
  KinematicState kinematic_state{vehicle_location, vehicle_rotation,
                                  vehicle_velocity, vehicle_ptr->GetSpeedLimit(),
                                  enable_physics, vehicle->IsDormant(), cg::Location()};
 
  // 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};
 
    // One-time query of the physical steering lock: the motion-plan stage
    // normalizes steering authority (gains and envelope) by it. Guarded so a
    // failed RPC (e.g. the actor died this frame) falls back to the default.
    try {
      const auto physics_control = vehicle_ptr->GetPhysicsControl();
      float max_steer_angle = 0.0f;
      for (const auto &wheel : physics_control.wheels) {
        max_steer_angle = std::max(max_steer_angle, wheel.max_steer_angle);
      }
      if (max_steer_angle > 1.0f) {
        attributes.max_steer_angle = max_steer_angle;
      }
    } catch (const std::exception &) {}
 
    simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
  }
}
 
 
void ALSM::UpdateUnregisteredActorsData() {
  for (auto &actor_info: unregistered_actors) {
 
    const ActorId actor_id = actor_info.first;
    const ActorPtr actor_ptr = actor_info.second;
    const std::string type_id = actor_ptr->GetTypeId();
 
    const cg::Transform actor_transform = actor_ptr->GetTransform();
    const cg::Location actor_location = actor_transform.location;
    const cg::Rotation actor_rotation = actor_transform.rotation;
    const cg::Vector3D actor_velocity = actor_ptr->GetVelocity();
    const bool actor_is_dormant = actor_ptr->IsDormant();
    KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true, actor_is_dormant, cg::Location()};
 
    TrafficLightState tl_state;
    ActorType actor_type = ActorType::Any;
    cg::Vector3D dimensions;
    std::vector<SimpleWaypointPtr> nearest_waypoints;
 
    bool state_entry_not_present = !simulation_state.ContainsActor(actor_id);
    if (type_id.front() == 'v') {
      auto vehicle_ptr = std::static_pointer_cast<cc::Vehicle>(actor_ptr);
      kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit();
 
      tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
 
      if (state_entry_not_present) {
        dimensions = vehicle_ptr->GetBoundingBox().extent;
        actor_type = ActorType::Vehicle;
        StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
 
        simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
      } else {
        simulation_state.UpdateKinematicState(actor_id, kinematic_state);
        simulation_state.UpdateTrafficLightState(actor_id, tl_state);
      }
 
      // Identify occupied waypoints.
      cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
      cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector();
      std::vector<cg::Location> corners = {actor_location + cg::Location(extent.x * heading_vector),
                                           actor_location,
                                           actor_location + cg::Location(-extent.x * heading_vector)};
      for (cg::Location &vertex: corners) {
        SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(vertex);
        if (nearest_waypoint != nullptr) {
          nearest_waypoints.push_back(nearest_waypoint);
        }
      }
    }
    else if (type_id.front() == 'w') {
      auto walker_ptr = std::static_pointer_cast<cc::Walker>(actor_ptr);
 
      if (state_entry_not_present) {
        dimensions = walker_ptr->GetBoundingBox().extent;
        actor_type = ActorType::Pedestrian;
        StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
 
        simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
      } else {
        simulation_state.UpdateKinematicState(actor_id, kinematic_state);
      }
 
      // Identify occupied waypoints.
      SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(actor_location);
      if (nearest_waypoint != nullptr) {
        nearest_waypoints.push_back(nearest_waypoint);
      }
    }
 
    track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints);
  }
}
 
void ALSM::UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id) {
  if (idle_time.find(actor_id) != idle_time.end()) {
    double &idle_duration = idle_time.at(actor_id);
    if (simulation_state.GetVelocity(actor_id).SquaredLength() > SQUARE(STOPPED_VELOCITY_THRESHOLD)) {
      idle_duration = current_timestamp.elapsed_seconds;
    }
 
    // Checking maximum idle time.
    if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) {
      max_idle_time = std::make_pair(actor_id, idle_duration);
    }
  }
}
 
bool ALSM::IsVehicleStuck(const ActorId& actor_id) {
  if (idle_time.find(actor_id) != idle_time.end()) {
    double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id);
    TrafficLightState tl_state = simulation_state.GetTLS(actor_id);
    if ((delta_idle_time >= RED_TL_BLOCKED_TIME_THRESHOLD)
    || (delta_idle_time >= BLOCKED_TIME_THRESHOLD && tl_state.tl_state != TLS::Red))
    {
      return true;
    }
  }
  return false;
}
 
void ALSM::AddActor(const Actor actor) {
  const ActorId actor_id = actor->GetId();
  for (auto &&attribute : actor->GetAttributes()) {
    if (attribute.GetId() != "base_type") {
      continue;
    }
    std::string value = attribute.GetValue();
    std::transform(
        value.begin(),
        value.end(),
        value.begin(),
        [](unsigned char c) { return std::tolower(c); });
    const std::string_view value_view{value};
    const auto match = std::find(
        large_vehicle_types.begin(),
        large_vehicle_types.end(),
        value_view);
    if (match != large_vehicle_types.end()) {
      large_vehicles[actor_id] = std::make_pair(0.0f, false);
    }
    // There is at most one "base_type" attribute, so stop scanning once it
    // has been handled.
    break;
  }
}
 
void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
  if (registered_actor) {
    registered_vehicles.Remove({actor_id});
    buffer_map.erase(actor_id);
    idle_time.erase(actor_id);
    localization_stage.RemoveActor(actor_id);
    collision_stage.RemoveActor(actor_id);
    traffic_light_stage.RemoveActor(actor_id);
    motion_plan_stage.RemoveActor(actor_id);
    vehicle_light_stage.RemoveActor(actor_id);
 
    large_vehicles.erase(actor_id);
  }
  else {
    unregistered_actors.erase(actor_id);
    hero_actors.erase(actor_id);
  }
 
  track_traffic.DeleteActor(actor_id);
  simulation_state.RemoveActor(actor_id);
}
 
void ALSM::Reset() {
  unregistered_actors.clear();
  idle_time.clear();
  hero_actors.clear();
  elapsed_last_actor_destruction = 0.0;
  current_timestamp = world.GetSnapshot().GetTimestamp();
}
 
} // namespace traffic_manager
} // namespace carla