485 lines
13 KiB
C++
485 lines
13 KiB
C++
// Copyright (c) 2020 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>.
|
|
|
|
#include "carla/trafficmanager/Parameters.h"
|
|
#include "carla/trafficmanager/Constants.h"
|
|
|
|
namespace carla {
|
|
namespace traffic_manager {
|
|
|
|
Parameters::Parameters() {
|
|
|
|
/// Set default synchronous mode time out.
|
|
synchronous_time_out = std::chrono::duration<int, std::milli>(10);
|
|
}
|
|
|
|
Parameters::~Parameters() {}
|
|
|
|
//////////////////////////////////// SETTERS //////////////////////////////////
|
|
|
|
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);
|
|
percentage_difference_from_speed_limit.AddEntry({actor->GetId(), new_percentage});
|
|
if (exact_desired_speed.Contains(actor->GetId())) {
|
|
exact_desired_speed.RemoveEntry(actor->GetId());
|
|
}
|
|
}
|
|
|
|
void Parameters::SetLaneOffset(const ActorPtr &actor, const float offset) {
|
|
const auto entry = std::make_pair(actor->GetId(), offset);
|
|
lane_offset.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetDesiredSpeed(const ActorPtr &actor, const float value) {
|
|
|
|
float new_value = std::max(0.0f, value);
|
|
exact_desired_speed.AddEntry({actor->GetId(), new_value});
|
|
if (percentage_difference_from_speed_limit.Contains(actor->GetId())) {
|
|
percentage_difference_from_speed_limit.RemoveEntry(actor->GetId());
|
|
}
|
|
}
|
|
|
|
void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) {
|
|
float new_percentage = std::min(100.0f, percentage);
|
|
global_percentage_difference_from_limit = new_percentage;
|
|
}
|
|
|
|
void Parameters::SetGlobalLaneOffset(const float offset) {
|
|
global_lane_offset = offset;
|
|
}
|
|
|
|
void Parameters::SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) {
|
|
const ActorId reference_id = reference_actor->GetId();
|
|
const ActorId other_id = other_actor->GetId();
|
|
|
|
if (detect_collision) {
|
|
if (ignore_collision.Contains(reference_id)) {
|
|
std::shared_ptr<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
|
|
if (actor_set->Contains(other_id)) {
|
|
actor_set->Remove({other_id});
|
|
}
|
|
}
|
|
} else {
|
|
if (ignore_collision.Contains(reference_id)) {
|
|
std::shared_ptr<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
|
|
if (!actor_set->Contains(other_id)) {
|
|
actor_set->Insert({other_actor});
|
|
}
|
|
} else {
|
|
std::shared_ptr<AtomicActorSet> actor_set = std::make_shared<AtomicActorSet>();
|
|
actor_set->Insert({other_actor});
|
|
auto entry = std::make_pair(reference_id, actor_set);
|
|
ignore_collision.AddEntry(entry);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Parameters::SetForceLaneChange(const ActorPtr &actor, const bool direction) {
|
|
|
|
const ChangeLaneInfo lane_change_info = {true, direction};
|
|
const auto entry = std::make_pair(actor->GetId(), lane_change_info);
|
|
force_lane_change.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetKeepRightPercentage(const ActorPtr &actor, const float percentage) {
|
|
|
|
const auto entry = std::make_pair(actor->GetId(), percentage);
|
|
perc_keep_right.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) {
|
|
|
|
const auto entry = std::make_pair(actor->GetId(), percentage);
|
|
perc_random_left.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) {
|
|
|
|
const auto entry = std::make_pair(actor->GetId(), percentage);
|
|
perc_random_right.AddEntry(entry);
|
|
|
|
}
|
|
|
|
void Parameters::SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update) {
|
|
|
|
const auto entry = std::make_pair(actor->GetId(), do_update);
|
|
auto_update_vehicle_lights.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
|
|
|
|
const auto entry = std::make_pair(actor->GetId(), enable);
|
|
auto_lane_change.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
|
|
|
|
float new_distance = std::max(0.0f, distance);
|
|
const auto entry = std::make_pair(actor->GetId(), new_distance);
|
|
distance_to_leading_vehicle.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetSynchronousMode(const bool mode_switch) {
|
|
synchronous_mode.store(mode_switch);
|
|
}
|
|
|
|
void Parameters::SetSynchronousModeTimeOutInMiliSecond(const double time) {
|
|
synchronous_time_out = std::chrono::duration<double, std::milli>(time);
|
|
}
|
|
|
|
void Parameters::SetGlobalDistanceToLeadingVehicle(const float dist) {
|
|
|
|
distance_margin.store(dist);
|
|
}
|
|
|
|
void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) {
|
|
|
|
float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f);
|
|
const auto entry = std::make_pair(actor->GetId(), new_perc);
|
|
perc_run_traffic_light.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetPercentageRunningSign(const ActorPtr &actor, const float perc) {
|
|
|
|
float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f);
|
|
const auto entry = std::make_pair(actor->GetId(), new_perc);
|
|
perc_run_traffic_sign.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc) {
|
|
|
|
float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f);
|
|
const auto entry = std::make_pair(actor->GetId(), new_perc);
|
|
perc_ignore_vehicles.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc) {
|
|
|
|
float new_perc = cg::Math::Clamp(perc,0.0f,100.0f);
|
|
const auto entry = std::make_pair(actor->GetId(), new_perc);
|
|
perc_ignore_walkers.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetHybridPhysicsRadius(const float radius) {
|
|
float new_radius = std::max(radius, 0.0f);
|
|
hybrid_physics_radius.store(new_radius);
|
|
}
|
|
|
|
void Parameters::SetOSMMode(const bool mode_switch) {
|
|
osm_mode.store(mode_switch);
|
|
}
|
|
|
|
void Parameters::SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) {
|
|
const auto entry = std::make_pair(actor->GetId(), path);
|
|
custom_path.AddEntry(entry);
|
|
const auto entry2 = std::make_pair(actor->GetId(), empty_buffer);
|
|
upload_path.AddEntry(entry2);
|
|
}
|
|
|
|
void Parameters::RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
|
|
if (!remove_path) {
|
|
upload_path.RemoveEntry(actor_id);
|
|
} else {
|
|
custom_path.RemoveEntry(actor_id);
|
|
}
|
|
}
|
|
|
|
void Parameters::UpdateUploadPath(const ActorId &actor_id, const Path path) {
|
|
custom_path.RemoveEntry(actor_id);
|
|
const auto entry = std::make_pair(actor_id, path);
|
|
custom_path.AddEntry(entry);
|
|
}
|
|
|
|
void Parameters::SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) {
|
|
const auto entry = std::make_pair(actor->GetId(), route);
|
|
custom_route.AddEntry(entry);
|
|
const auto entry2 = std::make_pair(actor->GetId(), empty_buffer);
|
|
upload_route.AddEntry(entry2);
|
|
}
|
|
|
|
void Parameters::RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
|
|
if (!remove_path) {
|
|
upload_route.RemoveEntry(actor_id);
|
|
} else {
|
|
custom_route.RemoveEntry(actor_id);
|
|
}
|
|
}
|
|
|
|
void Parameters::UpdateImportedRoute(const ActorId &actor_id, const Route route) {
|
|
custom_route.RemoveEntry(actor_id);
|
|
const auto entry = std::make_pair(actor_id, route);
|
|
custom_route.AddEntry(entry);
|
|
}
|
|
|
|
//////////////////////////////////// GETTERS //////////////////////////////////
|
|
|
|
float Parameters::GetHybridPhysicsRadius() const {
|
|
|
|
return hybrid_physics_radius.load();
|
|
}
|
|
|
|
bool Parameters::GetSynchronousMode() const {
|
|
return synchronous_mode.load();
|
|
}
|
|
|
|
double Parameters::GetSynchronousModeTimeOutInMiliSecond() const {
|
|
return synchronous_time_out.count();
|
|
}
|
|
|
|
float Parameters::GetVehicleTargetVelocity(const ActorId &actor_id, const float speed_limit) const {
|
|
|
|
float percentage_difference = global_percentage_difference_from_limit;
|
|
|
|
if (percentage_difference_from_speed_limit.Contains(actor_id)) {
|
|
percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id);
|
|
} else if (exact_desired_speed.Contains(actor_id)) {
|
|
return exact_desired_speed.GetValue(actor_id);
|
|
}
|
|
|
|
return speed_limit * (1.0f - percentage_difference / 100.0f);
|
|
}
|
|
|
|
float Parameters::GetLaneOffset(const ActorId &actor_id) const {
|
|
float offset = global_lane_offset;
|
|
|
|
if (lane_offset.Contains(actor_id)) {
|
|
offset = lane_offset.GetValue(actor_id);
|
|
}
|
|
|
|
return offset;
|
|
}
|
|
|
|
bool Parameters::GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const {
|
|
|
|
bool avoid_collision = true;
|
|
|
|
if (ignore_collision.Contains(reference_actor_id) &&
|
|
ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) {
|
|
avoid_collision = false;
|
|
}
|
|
|
|
return avoid_collision;
|
|
}
|
|
|
|
ChangeLaneInfo Parameters::GetForceLaneChange(const ActorId &actor_id) {
|
|
|
|
ChangeLaneInfo change_lane_info {false, false};
|
|
|
|
if (force_lane_change.Contains(actor_id)) {
|
|
change_lane_info = force_lane_change.GetValue(actor_id);
|
|
}
|
|
|
|
force_lane_change.RemoveEntry(actor_id);
|
|
|
|
return change_lane_info;
|
|
}
|
|
|
|
float Parameters::GetKeepRightPercentage(const ActorId &actor_id) {
|
|
|
|
float percentage = -1.0f;
|
|
|
|
if (perc_keep_right.Contains(actor_id)) {
|
|
percentage = perc_keep_right.GetValue(actor_id);
|
|
}
|
|
|
|
return percentage;
|
|
}
|
|
|
|
float Parameters::GetRandomLeftLaneChangePercentage(const ActorId &actor_id) {
|
|
|
|
float percentage = -1.0f;
|
|
|
|
if (perc_random_left.Contains(actor_id)) {
|
|
percentage = perc_random_left.GetValue(actor_id);
|
|
}
|
|
|
|
return percentage;
|
|
}
|
|
|
|
float Parameters::GetRandomRightLaneChangePercentage(const ActorId &actor_id) {
|
|
|
|
float percentage = -1.0f;
|
|
|
|
if (perc_random_right.Contains(actor_id)) {
|
|
percentage = perc_random_right.GetValue(actor_id);
|
|
}
|
|
|
|
return percentage;
|
|
}
|
|
|
|
bool Parameters::GetAutoLaneChange(const ActorId &actor_id) const {
|
|
|
|
bool auto_lane_change_policy = true;
|
|
|
|
if (auto_lane_change.Contains(actor_id)) {
|
|
auto_lane_change_policy = auto_lane_change.GetValue(actor_id);
|
|
}
|
|
|
|
return auto_lane_change_policy;
|
|
}
|
|
|
|
float Parameters::GetDistanceToLeadingVehicle(const ActorId &actor_id) const {
|
|
|
|
float specific_distance_margin = 0.0f;
|
|
if (distance_to_leading_vehicle.Contains(actor_id)) {
|
|
specific_distance_margin = distance_to_leading_vehicle.GetValue(actor_id);
|
|
} else {
|
|
specific_distance_margin = distance_margin;
|
|
}
|
|
|
|
return specific_distance_margin;
|
|
}
|
|
|
|
float Parameters::GetPercentageRunningLight(const ActorId &actor_id) const {
|
|
|
|
float percentage = 0.0f;
|
|
|
|
if (perc_run_traffic_light.Contains(actor_id)) {
|
|
percentage = perc_run_traffic_light.GetValue(actor_id);
|
|
}
|
|
|
|
return percentage;
|
|
}
|
|
|
|
float Parameters::GetPercentageRunningSign(const ActorId &actor_id) const {
|
|
|
|
float percentage = 0.0f;
|
|
|
|
if (perc_run_traffic_sign.Contains(actor_id)) {
|
|
percentage = perc_run_traffic_sign.GetValue(actor_id);
|
|
}
|
|
|
|
return percentage;
|
|
}
|
|
|
|
float Parameters::GetPercentageIgnoreWalkers(const ActorId &actor_id) const {
|
|
|
|
float percentage = 0.0f;
|
|
|
|
if (perc_ignore_walkers.Contains(actor_id)) {
|
|
percentage = perc_ignore_walkers.GetValue(actor_id);
|
|
}
|
|
|
|
return percentage;
|
|
}
|
|
|
|
bool Parameters::GetUpdateVehicleLights(const ActorId &actor_id) const {
|
|
bool do_update = false;
|
|
|
|
if (auto_update_vehicle_lights.Contains(actor_id)) {
|
|
do_update = auto_update_vehicle_lights.GetValue(actor_id);
|
|
}
|
|
|
|
return do_update;
|
|
}
|
|
|
|
float Parameters::GetPercentageIgnoreVehicles(const ActorId &actor_id) const {
|
|
|
|
float percentage = 0.0f;
|
|
|
|
if (perc_ignore_vehicles.Contains(actor_id)) {
|
|
percentage = perc_ignore_vehicles.GetValue(actor_id);
|
|
}
|
|
|
|
return percentage;
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
bool Parameters::GetUploadPath(const ActorId &actor_id) const {
|
|
|
|
bool custom_path_bool = false;
|
|
|
|
if (upload_path.Contains(actor_id)) {
|
|
custom_path_bool = upload_path.GetValue(actor_id);
|
|
}
|
|
|
|
return custom_path_bool;
|
|
}
|
|
|
|
Path Parameters::GetCustomPath(const ActorId &actor_id) const {
|
|
|
|
Path custom_path_import;
|
|
|
|
if (custom_path.Contains(actor_id)) {
|
|
custom_path_import = custom_path.GetValue(actor_id);
|
|
}
|
|
|
|
return custom_path_import;
|
|
}
|
|
|
|
|
|
bool Parameters::GetUploadRoute(const ActorId &actor_id) const {
|
|
|
|
bool custom_route_bool = false;
|
|
|
|
if (upload_route.Contains(actor_id)) {
|
|
custom_route_bool = upload_route.GetValue(actor_id);
|
|
}
|
|
|
|
return custom_route_bool;
|
|
}
|
|
|
|
Route Parameters::GetImportedRoute(const ActorId &actor_id) const {
|
|
|
|
Route custom_route_import;
|
|
|
|
if (custom_route.Contains(actor_id)) {
|
|
custom_route_import = custom_route.GetValue(actor_id);
|
|
}
|
|
|
|
return custom_route_import;
|
|
}
|
|
|
|
|
|
} // namespace traffic_manager
|
|
} // namespace carla
|