From e9a2935a5ac593b94256ef824b8584268e77797f Mon Sep 17 00:00:00 2001 From: dotero Date: Wed, 11 Dec 2019 14:12:17 +0100 Subject: [PATCH] Fixing code formatting --- LibCarla/source/carla/Memory.h | 2 +- LibCarla/source/carla/client/Vehicle.cpp | 6 +- LibCarla/source/carla/client/Vehicle.h | 5 +- .../carla/trafficmanager/AtomicActorSet.h | 100 +++---- .../source/carla/trafficmanager/AtomicMap.h | 58 ++-- .../trafficmanager/BatchControlStage.cpp | 13 +- .../carla/trafficmanager/BatchControlStage.h | 18 +- .../trafficmanager/CarlaDataAccessLayer.cpp | 8 +- .../trafficmanager/CarlaDataAccessLayer.h | 10 +- .../carla/trafficmanager/CollisionStage.cpp | 188 ++++++------- .../carla/trafficmanager/CollisionStage.h | 30 +- .../carla/trafficmanager/InMemoryMap.cpp | 71 +++-- .../source/carla/trafficmanager/InMemoryMap.h | 14 +- .../trafficmanager/LocalizationStage.cpp | 206 ++++++-------- .../carla/trafficmanager/LocalizationStage.h | 37 +-- .../trafficmanager/LocalizationUtils.cpp | 131 ++++----- .../carla/trafficmanager/LocalizationUtils.h | 77 ++--- .../source/carla/trafficmanager/Messenger.h | 8 +- .../trafficmanager/MessengerAndDataTypes.h | 11 +- .../trafficmanager/MotionPlannerStage.cpp | 60 ++-- .../carla/trafficmanager/MotionPlannerStage.h | 34 ++- .../carla/trafficmanager/PIDController.cpp | 39 +-- .../carla/trafficmanager/PIDController.h | 10 +- .../carla/trafficmanager/Parameters.cpp | 263 +++++++++--------- .../source/carla/trafficmanager/Parameters.h | 136 ++++----- .../trafficmanager/PerformanceDiagnostics.cpp | 66 +++-- .../trafficmanager/PerformanceDiagnostics.h | 43 +-- .../carla/trafficmanager/PipelineStage.cpp | 8 +- .../carla/trafficmanager/PipelineStage.h | 8 +- .../carla/trafficmanager/SimpleWaypoint.cpp | 20 +- .../carla/trafficmanager/SimpleWaypoint.h | 12 +- .../trafficmanager/TrafficLightStage.cpp | 82 +++--- .../carla/trafficmanager/TrafficLightStage.h | 32 ++- .../carla/trafficmanager/TrafficManager.cpp | 106 +++---- .../carla/trafficmanager/TrafficManager.h | 17 +- .../carla/trafficmanager/VicinityGrid.cpp | 28 +- .../carla/trafficmanager/VicinityGrid.h | 8 +- .../carla/source/libcarla/TrafficManager.cpp | 6 + PythonAPI/carla/source/libcarla/libcarla.cpp | 2 +- 39 files changed, 1069 insertions(+), 904 deletions(-) diff --git a/LibCarla/source/carla/Memory.h b/LibCarla/source/carla/Memory.h index 9b0798785..9c925216b 100644 --- a/LibCarla/source/carla/Memory.h +++ b/LibCarla/source/carla/Memory.h @@ -1,4 +1,4 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 3f2d6ca2d..d33f640f9 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/LibCarla/source/carla/client/Vehicle.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -6,8 +6,8 @@ #include "carla/client/Vehicle.h" -#include "carla/client/detail/Simulator.h" #include "carla/client/ActorList.h" +#include "carla/client/detail/Simulator.h" #include "carla/client/TrafficLight.h" #include "carla/Memory.h" #include "carla/rpc/TrafficLightState.h" @@ -34,8 +34,6 @@ namespace client { _is_control_sticky(GetControlIsSticky(GetAttributes())) {} void Vehicle::SetAutopilot(bool enabled) { - // GetEpisode().Lock()->SetVehicleAutopilot(*this, enabled); - // std::cout << "Calling SetAutopilot" << std::endl; if (enabled) { TM &tm = TM::GetInstance(TM::GetUniqueLocalClient()); tm.RegisterVehicles({carla::SharedPtr(this)}); diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index 8965cf70e..294eb1249 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/LibCarla/source/carla/client/Vehicle.h @@ -1,4 +1,4 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -7,12 +7,13 @@ #pragma once #include "carla/client/Actor.h" +#include "carla/rpc/TrafficLightState.h" #include "carla/rpc/VehicleControl.h" #include "carla/rpc/VehiclePhysicsControl.h" -#include "carla/rpc/TrafficLightState.h" namespace carla { namespace client { + class TrafficLight; class Vehicle : public Actor { diff --git a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h index 8769048e9..ff02af5a7 100644 --- a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h +++ b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -9,68 +15,68 @@ namespace traffic_manager { -namespace cc = carla::client; - using ActorPtr = carla::SharedPtr; - using ActorId = carla::ActorId; + namespace cc = carla::client; + using ActorPtr = carla::SharedPtr; + using ActorId = carla::ActorId; - class AtomicActorSet { + class AtomicActorSet { - private: + private: - std::mutex modification_mutex; - std::unordered_map actor_set; - int state_counter; + std::mutex modification_mutex; + std::unordered_map actor_set; + int state_counter; - public: + public: - AtomicActorSet(): state_counter(0) {} + AtomicActorSet(): state_counter(0) {} - std::vector GetList() { + std::vector GetList() { - std::lock_guard lock(modification_mutex); - std::vector actor_list; - for (auto it = actor_set.begin(); it != actor_set.end(); ++it) { - actor_list.push_back(it->second); - } - return actor_list; - } + std::lock_guard lock(modification_mutex); + std::vector actor_list; + for (auto it = actor_set.begin(); it != actor_set.end(); ++it) { + actor_list.push_back(it->second); + } + return actor_list; + } - void Insert(std::vector actor_list) { + void Insert(std::vector actor_list) { - std::lock_guard lock(modification_mutex); - for (auto &actor: actor_list) { - actor_set.insert({actor->GetId(), actor}); - } - ++state_counter; - } + std::lock_guard lock(modification_mutex); + for (auto &actor: actor_list) { + actor_set.insert({actor->GetId(), actor}); + } + ++state_counter; + } - void Remove(std::vector actor_list) { + void Remove(std::vector actor_list) { - std::lock_guard lock(modification_mutex); - for (auto& actor: actor_list) { - actor_set.erase(actor->GetId()); - } - ++state_counter; - } + std::lock_guard lock(modification_mutex); + for (auto& actor: actor_list) { + actor_set.erase(actor->GetId()); + } + ++state_counter; + } - void Destroy(ActorPtr actor) { + void Destroy(ActorPtr actor) { - std::lock_guard lock(modification_mutex); - actor_set.erase(actor->GetId()); - actor->Destroy(); - ++state_counter; - } + std::lock_guard lock(modification_mutex); + actor_set.erase(actor->GetId()); + actor->Destroy(); + ++state_counter; + } - int GetState() { + int GetState() { - std::lock_guard lock(modification_mutex); - return state_counter; - } + std::lock_guard lock(modification_mutex); + return state_counter; + } - bool Contains(ActorId id) { + bool Contains(ActorId id) { - return actor_set.find(id) != actor_set.end(); - } - }; + return actor_set.find(id) != actor_set.end(); + } + }; -} \ No newline at end of file +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/AtomicMap.h b/LibCarla/source/carla/trafficmanager/AtomicMap.h index 636a02607..2438b90dd 100644 --- a/LibCarla/source/carla/trafficmanager/AtomicMap.h +++ b/LibCarla/source/carla/trafficmanager/AtomicMap.h @@ -1,42 +1,52 @@ +// Copyright (c) 2019 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 . + #pragma once #include #include -template -class AtomicMap { +namespace traffic_manager { - private: + template + class AtomicMap { - std::mutex map_mutex; - std::unordered_map map; + private: - public: + std::mutex map_mutex; + std::unordered_map map; - AtomicMap() {} + public: - void AddEntry(const std::pair &entry) { + AtomicMap() {} - std::lock_guard lock(map_mutex); - map.insert(entry); - } + void AddEntry(const std::pair &entry) { - bool Contains(const Key &key) { + std::lock_guard lock(map_mutex); + map.insert(entry); + } - std::lock_guard lock(map_mutex); - return map.find(key) != map.end(); - } + bool Contains(const Key &key) { - Value &GetValue(const Key &key) { + std::lock_guard lock(map_mutex); + return map.find(key) != map.end(); + } - std::lock_guard lock(map_mutex); - return map.at(key); - } + Value &GetValue(const Key &key) { - void RemoveEntry(const Key &key) { + std::lock_guard lock(map_mutex); + return map.at(key); + } - std::lock_guard lock(map_mutex); - map.erase(key); - } + void RemoveEntry(const Key &key) { -}; + std::lock_guard lock(map_mutex); + map.erase(key); + } + + }; + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp index 89093cbdd..6765cd1c1 100644 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp +++ b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #include "BatchControlStage.h" namespace traffic_manager { @@ -25,8 +31,8 @@ namespace traffic_manager { cr::VehicleControl vehicle_control; - PlannerToControlData &element = data_frame->at(i); - carla::ActorId actor_id = element.actor_id; + const PlannerToControlData &element = data_frame->at(i); + const carla::ActorId actor_id = element.actor_id; vehicle_control.throttle = element.throttle; vehicle_control.brake = element.brake; vehicle_control.steer = element.steer; @@ -61,4 +67,5 @@ namespace traffic_manager { // Limiting updates to 100 frames per second. std::this_thread::sleep_for(10ms); } -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.h b/LibCarla/source/carla/trafficmanager/BatchControlStage.h index 0addc0980..fbd37901f 100644 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.h +++ b/LibCarla/source/carla/trafficmanager/BatchControlStage.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -13,8 +19,8 @@ namespace traffic_manager { -namespace cc = carla::client; -namespace cr = carla::rpc; + namespace cc = carla::client; + namespace cr = carla::rpc; /// This class receives actuation signals (throttle, brake, steer) /// from MotionPlannerStage class and communicates these signals to @@ -39,9 +45,9 @@ namespace cr = carla::rpc; public: BatchControlStage( - std::string stage_name, - std::shared_ptr messenger, - cc::Client &carla_client); + std::string stage_name, + std::shared_ptr messenger, + cc::Client &carla_client); ~BatchControlStage(); void DataReceiver() override; @@ -52,4 +58,4 @@ namespace cr = carla::rpc; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp b/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp index d1b8645bc..5430665ce 100644 --- a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp +++ b/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #include "CarlaDataAccessLayer.h" namespace traffic_manager { @@ -10,4 +16,4 @@ namespace traffic_manager { std::vector> CarlaDataAccessLayer::GetTopology() const { return world_map->GetTopology(); } -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h b/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h index cf164c971..075f8ada5 100644 --- a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h +++ b/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -8,7 +14,7 @@ namespace traffic_manager { -namespace cc = carla::client; + namespace cc = carla::client; /// This class is responsible for retrieving data from the server. class CarlaDataAccessLayer { @@ -28,4 +34,4 @@ namespace cc = carla::client; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 499203b9d..1b42e729f 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -1,8 +1,15 @@ +// Copyright (c) 2019 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 . + #include "CollisionStage.h" namespace traffic_manager { namespace CollisionStageConstants { + static const float VERTICAL_OVERLAP_THRESHOLD = 2.0f; static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f; static const float EXTENSION_SQUARE_POINT = 7.5f; @@ -11,10 +18,11 @@ namespace CollisionStageConstants { static const float HIGHWAY_TIME_HORIZON = 5.0f; static const float CRAWL_SPEED = 10.0f / 3.6f; static const float BOUNDARY_EDGE_LENGTH = 2.0f; - //static const float MINIMUM_GRID_EXTENSION = 10.0f; static const float MAX_COLLISION_RADIUS = 100.0f; -} - using namespace CollisionStageConstants; + +} // namespace CollisionStageConstants + + using namespace CollisionStageConstants; CollisionStage::CollisionStage( std::string stage_name, @@ -46,20 +54,20 @@ namespace CollisionStageConstants { CollisionStage::~CollisionStage() {} void CollisionStage::Action() { - auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; + const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; // Handle vehicles not spawned by TrafficManager. - auto current_time = chr::system_clock::now(); - chr::duration diff = current_time - last_world_actors_pass_instance; + const auto current_time = chr::system_clock::now(); + const chr::duration diff = current_time - last_world_actors_pass_instance; // Periodically check for actors not spawned by TrafficManager. if (diff.count() > 1.0f) { - auto world_actors = world.GetActors()->Filter("vehicle.*"); - auto world_walker = world.GetActors()->Filter("walker.*"); + const auto world_actors = world.GetActors()->Filter("vehicle.*"); + const auto world_walker = world.GetActors()->Filter("walker.*"); // Scanning for vehicles. for (auto actor: *world_actors.get()) { - auto unregistered_id = actor->GetId(); + const auto unregistered_id = actor->GetId(); if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() && unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { unregistered_actors.insert({unregistered_id, actor}); @@ -67,7 +75,7 @@ namespace CollisionStageConstants { } // Scanning for pedestrians. for (auto walker: *world_walker.get()) { - auto unregistered_id = walker->GetId(); + const auto unregistered_id = walker->GetId(); if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { unregistered_actors.insert({unregistered_id, walker}); } @@ -92,9 +100,9 @@ namespace CollisionStageConstants { // Looping over registered actors. for (uint i = 0u; i < number_of_vehicles; ++i) { - LocalizationToCollisionData &data = localization_frame->at(i); - Actor ego_actor = data.actor; - ActorId ego_actor_id = ego_actor->GetId(); + const LocalizationToCollisionData &data = localization_frame->at(i); + const Actor ego_actor = data.actor; + const ActorId ego_actor_id = ego_actor->GetId(); // Retrieve actors around the path of the ego vehicle. std::unordered_set actor_id_list = GetPotentialVehicleObstacles(ego_actor); @@ -102,13 +110,13 @@ namespace CollisionStageConstants { bool collision_hazard = false; // Generate number between 0 and 100 - int r = rand() % 101; + const int r = rand() % 101; // Continue only if random number is lower than our %, default is 0. if (parameters.GetPercentageIgnoreActors(boost::shared_ptr(ego_actor)) <= r) { // Check every actor in the vicinity if it poses a collision hazard. for (auto j = actor_id_list.begin(); (j != actor_id_list.end()) && !collision_hazard; ++j) { - ActorId actor_id = *j; + const ActorId actor_id = *j; try { Actor actor = nullptr; @@ -118,20 +126,14 @@ namespace CollisionStageConstants { actor = unregistered_actors.at(actor_id); } - cg::Location ego_location = ego_actor->GetLocation(); - cg::Location other_location = actor->GetLocation(); + const cg::Location ego_location = ego_actor->GetLocation(); + const cg::Location other_location = actor->GetLocation(); if (actor_id != ego_actor_id && (cg::Math::DistanceSquared(ego_location, other_location) < std::pow(MAX_COLLISION_RADIUS, 2)) && (std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) { - // debug_helper.DrawLine( - // ego_location + cg::Location(0, 0, 2), - // other_location + cg::Location(0, 0, 2), - // 0.2f, {255u, 0u, 0u}, 0.1f - // ); THE PROBLEM IS SOMEWHERE HERE WITH COLLISIONS, IT RETURNS FALSE FOR COLLISION HAZARD - if (parameters.GetCollisionDetection(ego_actor, actor) && NegotiateCollision(ego_actor, actor)) { @@ -154,7 +156,7 @@ namespace CollisionStageConstants { } void CollisionStage::DataReceiver() { - auto packet = localization_messenger->ReceiveData(localization_messenger_state); + const auto packet = localization_messenger->ReceiveData(localization_messenger_state); localization_frame = packet.data; localization_messenger_state = packet.id; @@ -182,7 +184,7 @@ namespace CollisionStageConstants { void CollisionStage::DataSender() { - DataPacket> packet{ + const DataPacket> packet{ planner_messenger_state, frame_selector ? planner_frame_a : planner_frame_b }; @@ -200,65 +202,57 @@ namespace CollisionStageConstants { auto& other_packet = localization_frame->at(vehicle_id_to_index.at(other_vehicle->GetId())); Buffer& other_buffer = other_packet.buffer; - cg::Location reference_location = reference_vehicle->GetLocation(); - cg::Location other_location = other_vehicle->GetLocation(); + const cg::Location reference_location = reference_vehicle->GetLocation(); + const cg::Location other_location = other_vehicle->GetLocation(); - cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector(); + const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector(); cg::Vector3D reference_to_other = other_location - reference_location; reference_to_other = reference_to_other.MakeUnitVector(); - auto reference_vehicle_ptr = boost::static_pointer_cast(reference_vehicle); - auto other_vehicle_ptr = boost::static_pointer_cast(other_vehicle); + const auto reference_vehicle_ptr = boost::static_pointer_cast(reference_vehicle); + const auto other_vehicle_ptr = boost::static_pointer_cast(other_vehicle); if (waypoint_buffer.front()->CheckJunction() && other_buffer.front()->CheckJunction()) { - Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle)); - Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle)); - Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle)); - Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle)); + const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle)); + const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle)); + const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle)); + const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle)); - double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon); - double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon); + const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon); + const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon); - auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon); - auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); + const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon); + const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); - cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector(); + const cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector(); cg::Vector3D other_to_reference = reference_vehicle->GetLocation() - other_vehicle->GetLocation(); other_to_reference = other_to_reference.MakeUnitVector(); // Whichever vehicle's path is farthest away from the other vehicle gets // priority to move. - if (inter_geodesic_distance < 0.1 - && - ( - ( - inter_bbox_distance > 0.1 - && - reference_vehicle_to_other_geodesic > other_vehicle_to_reference_geodesic - ) - || - ( - inter_bbox_distance < 0.1 - && - cg::Math::Dot(reference_heading, reference_to_other) > cg::Math::Dot(other_heading, other_to_reference) - ) - ) - ) { + if (inter_geodesic_distance < 0.1 && + (( + inter_bbox_distance > 0.1 && + reference_vehicle_to_other_geodesic > other_vehicle_to_reference_geodesic + ) || ( + inter_bbox_distance < 0.1 && + cg::Math::Dot(reference_heading, reference_to_other) > cg::Math::Dot(other_heading, other_to_reference) + )) ) { hazard = true; } } else if (!waypoint_buffer.front()->CheckJunction()) { - float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x; - float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x; - float vehicle_length_sum = reference_vehicle_length + other_vehicle_length; + const float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x; + const float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x; + const float vehicle_length_sum = reference_vehicle_length + other_vehicle_length; - float bbox_extension_length = GetBoundingBoxExtention(reference_vehicle); + const float bbox_extension_length = GetBoundingBoxExtention(reference_vehicle); - if ((cg::Math::Dot(reference_heading, reference_to_other) > 0) && + if ((cg::Math::Dot(reference_heading, reference_to_other) > 0.0f) && (cg::Math::DistanceSquared(reference_location, other_location) < std::pow(bbox_extension_length+vehicle_length_sum, 2))) { @@ -285,26 +279,26 @@ namespace CollisionStageConstants { LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor) const { - LocationList bbox = GetBoundary(actor); + const LocationList bbox = GetBoundary(actor); if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) { - cg::Location vehicle_location = actor->GetLocation(); + const cg::Location vehicle_location = actor->GetLocation(); float bbox_extension = GetBoundingBoxExtention(actor); - float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor); - if (specific_distance_margin > 0) { + const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor); + if (specific_distance_margin > 0.0f) { bbox_extension = std::max(specific_distance_margin, bbox_extension); } - auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer; + const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer; LocationList left_boundary; LocationList right_boundary; - auto vehicle = boost::static_pointer_cast(actor); - float width = vehicle->GetBoundingBox().extent.y; - float length = vehicle->GetBoundingBox().extent.x; + const auto vehicle = boost::static_pointer_cast(actor); + const float width = vehicle->GetBoundingBox().extent.y; + const float length = vehicle->GetBoundingBox().extent.x; SimpleWaypointPtr boundary_start = waypoint_buffer.front(); uint boundary_start_index = 0u; @@ -316,7 +310,7 @@ namespace CollisionStageConstants { SimpleWaypointPtr boundary_end = nullptr; SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index); - auto vehicle_reference = boost::static_pointer_cast(actor); + const auto vehicle_reference = boost::static_pointer_cast(actor); // At non-signalized junctions, we extend the boundary across the junction // and in all other situations, boundary length is velocity-dependent. bool reached_distance = false; @@ -330,14 +324,14 @@ namespace CollisionStageConstants { boundary_end->DistanceSquared(current_point) > std::pow(BOUNDARY_EDGE_LENGTH, 2) || reached_distance) { - cg::Vector3D heading_vector = current_point->GetForwardVector(); - cg::Location location = current_point->GetLocation(); - cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0); + const cg::Vector3D heading_vector = current_point->GetForwardVector(); + const cg::Location location = current_point->GetLocation(); + cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f); perpendicular_vector = perpendicular_vector.MakeUnitVector(); // Direction determined for the left-handed system. - cg::Vector3D scaled_perpendicular = perpendicular_vector * width; + const cg::Vector3D scaled_perpendicular = perpendicular_vector * width; left_boundary.push_back(location + cg::Location(scaled_perpendicular)); - right_boundary.push_back(location + cg::Location(-1 * scaled_perpendicular)); + right_boundary.push_back(location + cg::Location(-1.0f * scaled_perpendicular)); boundary_end = current_point; } @@ -366,16 +360,17 @@ namespace CollisionStageConstants { float CollisionStage::GetBoundingBoxExtention(const Actor &actor) const { - float velocity = actor->GetVelocity().Length(); + const float velocity = actor->GetVelocity().Length(); float bbox_extension = BOUNDARY_EXTENSION_MINIMUM; if (velocity > HIGHWAY_SPEED) { bbox_extension = HIGHWAY_TIME_HORIZON * velocity; } else if (velocity < CRAWL_SPEED) { bbox_extension = BOUNDARY_EXTENSION_MINIMUM; } else { - bbox_extension = std::sqrt(EXTENSION_SQUARE_POINT * velocity) + - velocity * TIME_HORIZON + - BOUNDARY_EXTENSION_MINIMUM; + bbox_extension = std::sqrt( + EXTENSION_SQUARE_POINT * velocity) + + velocity * TIME_HORIZON + + BOUNDARY_EXTENSION_MINIMUM; } return bbox_extension; @@ -385,9 +380,9 @@ namespace CollisionStageConstants { vicinity_grid.UpdateGrid(ego_vehicle); - auto& data_packet = localization_frame->at(vehicle_id_to_index.at(ego_vehicle->GetId())); - Buffer &waypoint_buffer = data_packet.buffer; - float velocity = ego_vehicle->GetVelocity().Length(); + const auto& data_packet = localization_frame->at(vehicle_id_to_index.at(ego_vehicle->GetId())); + const Buffer &waypoint_buffer = data_packet.buffer; + const float velocity = ego_vehicle->GetVelocity().Length(); std::unordered_set actor_id_list = data_packet.overlapping_actors; if (waypoint_buffer.front()->CheckJunction() && velocity < HIGHWAY_SPEED) { @@ -400,7 +395,7 @@ namespace CollisionStageConstants { } LocationList CollisionStage::GetBoundary(const Actor &actor) const { - auto actor_type = actor->GetTypeId(); + const auto actor_type = actor->GetTypeId(); cg::BoundingBox bbox; cg::Location location; @@ -408,40 +403,41 @@ namespace CollisionStageConstants { if (actor_type[0] == 'v') { - auto vehicle = boost::static_pointer_cast(actor); + const auto vehicle = boost::static_pointer_cast(actor); bbox = vehicle->GetBoundingBox(); location = vehicle->GetLocation(); heading_vector = vehicle->GetTransform().GetForwardVector(); } else if (actor_type[0] == 'w') { - auto walker = boost::static_pointer_cast(actor); + const auto walker = boost::static_pointer_cast(actor); bbox = walker->GetBoundingBox(); location = walker->GetLocation(); heading_vector = walker->GetTransform().GetForwardVector(); } - cg::Vector3D extent = bbox.extent; - heading_vector.z = 0; - cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0); + const cg::Vector3D extent = bbox.extent; + heading_vector.z = 0.0f; + const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f); // Four corners of the vehicle in top view clockwise order (left-handed // system). - cg::Vector3D x_boundary_vector = heading_vector * extent.x; - cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y; + const cg::Vector3D x_boundary_vector = heading_vector * extent.x; + const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y; return { - location + cg::Location(x_boundary_vector - y_boundary_vector), - location + cg::Location(-1 * x_boundary_vector - y_boundary_vector), - location + cg::Location(-1 * x_boundary_vector + y_boundary_vector), - location + cg::Location(x_boundary_vector + y_boundary_vector), + location + cg::Location(x_boundary_vector - y_boundary_vector), + location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector), + location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector), + location + cg::Location(x_boundary_vector + y_boundary_vector), }; } void CollisionStage::DrawBoundary(const LocationList &boundary) const { for (uint i = 0u; i < boundary.size(); ++i) { debug_helper.DrawLine( - boundary[i] + cg::Location(0, 0, 1), - boundary[(i + 1) % boundary.size()] + cg::Location(0, 0, 1), + boundary[i] + cg::Location(0.0f, 0.0f, 1.0f), + boundary[(i + 1) % boundary.size()] + cg::Location(0.0f, 0.0f, 1.0f), 0.1f, {255u, 0u, 0u}, 0.1f); } } -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index d56852a84..dce515b14 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -1,12 +1,18 @@ +// Copyright (c) 2019 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 . + #pragma once #include #include #include +#include #include #include #include -#include #include "boost/geometry.hpp" #include "boost/geometry/geometries/point_xy.hpp" @@ -30,10 +36,10 @@ namespace traffic_manager { -namespace cc = carla::client; -namespace cg = carla::geom; -namespace chr = std::chrono; -namespace bg = boost::geometry; + namespace cc = carla::client; + namespace cg = carla::geom; + namespace chr = std::chrono; + namespace bg = boost::geometry; using ActorId = carla::ActorId; using Actor = carla::SharedPtr; @@ -107,12 +113,12 @@ namespace bg = boost::geometry; public: CollisionStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr planner_messenger, - cc::World &world, - Parameters ¶meters, - cc::DebugHelper &debug_helper); + std::string stage_name, + std::shared_ptr localization_messenger, + std::shared_ptr planner_messenger, + cc::World &world, + Parameters ¶meters, + cc::DebugHelper &debug_helper); ~CollisionStage(); @@ -124,4 +130,4 @@ namespace bg = boost::geometry; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index 6e04bb70f..8d8fd91c4 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -1,8 +1,15 @@ +// Copyright (c) 2019 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 . + #include "InMemoryMap.h" namespace traffic_manager { namespace MapConstants { + // Very important that this is less than 10^-4. static const float ZERO_LENGTH = 0.0001f; static const float INFINITE_DISTANCE = std::numeric_limits::max(); @@ -10,7 +17,9 @@ namespace MapConstants { // Cosine of the angle. static const float LANE_CHANGE_ANGULAR_THRESHOLD = 0.5f; static const float GRID_SIZE = 4.0f; -} + +} // namespace MapConstants + using namespace MapConstants; InMemoryMap::InMemoryMap(TopologyList topology) { @@ -23,7 +32,8 @@ namespace MapConstants { NodeList entry_node_list; NodeList exit_node_list; - auto distance_squared = [](cg::Location l1, cg::Location l2) { + auto distance_squared = + [](cg::Location l1, cg::Location l2) { return cg::Math::DistanceSquared(l1, l2); }; auto square = [](float input) {return std::pow(input, 2);}; @@ -32,10 +42,10 @@ namespace MapConstants { for (auto &pair : _topology) { // Looping through every topology segment. - WaypointPtr begin_waypoint = pair.first; - WaypointPtr end_waypoint = pair.second; - cg::Location begin_location = begin_waypoint->GetTransform().location; - cg::Location end_location = end_waypoint->GetTransform().location; + const WaypointPtr begin_waypoint = pair.first; + const WaypointPtr end_waypoint = pair.second; + const cg::Location begin_location = begin_waypoint->GetTransform().location; + const cg::Location end_location = end_waypoint->GetTransform().location; if (distance_squared(begin_location, end_location) > square(ZERO_LENGTH)) { @@ -66,7 +76,7 @@ namespace MapConstants { } // Linking segments. - uint i = 0, j = 0; + uint i = 0u, j = 0u; for (SimpleWaypointPtr end_point : exit_node_list) { for (SimpleWaypointPtr begin_point : entry_node_list) { if (end_point->DistanceSquared(begin_point) < square(ZERO_LENGTH) and i != j) { @@ -81,10 +91,10 @@ namespace MapConstants { // Loop through all exit nodes of topology segments, // connect any dangling endpoints to the nearest entry point // of another topology segment. - i = 0; + i = 0u; for (auto &end_point : exit_node_list) { if (end_point->GetNextWaypoint().size() == 0) { - j = 0; + j = 0u; float min_distance = INFINITE_DISTANCE; SimpleWaypointPtr closest_connection; for (auto &begin_point : entry_node_list) { @@ -95,13 +105,13 @@ namespace MapConstants { } ++j; } - cg::Vector3D end_point_vector = end_point->GetForwardVector(); + const cg::Vector3D end_point_vector = end_point->GetForwardVector(); cg::Vector3D relative_vector = closest_connection->GetLocation() - end_point->GetLocation(); relative_vector = relative_vector.MakeUnitVector(); - float relative_dot = cg::Math::Dot(end_point_vector, relative_vector); + const float relative_dot = cg::Math::Dot(end_point_vector, relative_vector); if (relative_dot < LANE_CHANGE_ANGULAR_THRESHOLD) { uint count = LANE_CHANGE_LOOK_AHEAD; - while (count > 0) { + while (count > 0u) { closest_connection = closest_connection->GetNextWaypoint()[0]; --count; } @@ -113,8 +123,8 @@ namespace MapConstants { // Localizing waypoints into grids. for (auto &simple_waypoint: dense_topology) { - cg::Location loc = simple_waypoint->GetLocation(); - std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y)); + const cg::Location loc = simple_waypoint->GetLocation(); + const std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y)); if (waypoint_grid.find(grid_key) == waypoint_grid.end()) { waypoint_grid.insert({grid_key, {simple_waypoint}}); } else { @@ -140,7 +150,7 @@ namespace MapConstants { SimpleWaypointPtr InMemoryMap::GetWaypointInVicinity(cg::Location location) { - std::pair grid_ids = MakeGridId(location.x, location.y); + const std::pair grid_ids = MakeGridId(location.x, location.y); SimpleWaypointPtr closest_waypoint = nullptr; float closest_distance = INFINITE_DISTANCE; @@ -148,10 +158,10 @@ namespace MapConstants { for (int i = -1; i <= 1; ++i) { for (int j = -1; j <= 1; ++j) { - std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j}); + const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j}); if (waypoint_grid.find(grid_key) != waypoint_grid.end()) { - auto& waypoint_set = waypoint_grid.at(grid_key); + const auto& waypoint_set = waypoint_grid.at(grid_key); if (closest_waypoint == nullptr) { closest_waypoint = *waypoint_set.begin(); } @@ -183,7 +193,7 @@ namespace MapConstants { SimpleWaypointPtr closest_waypoint; float min_distance = INFINITE_DISTANCE; for (auto &simple_waypoint : dense_topology) { - float current_distance = simple_waypoint->DistanceSquared(location); + const float current_distance = simple_waypoint->DistanceSquared(location); if (current_distance < min_distance) { min_distance = current_distance; closest_waypoint = simple_waypoint; @@ -198,16 +208,16 @@ namespace MapConstants { void InMemoryMap::FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint) { - WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint(); - crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange(); - auto change_right = crd::element::LaneMarking::LaneChange::Right; - auto change_left = crd::element::LaneMarking::LaneChange::Left; - auto change_both = crd::element::LaneMarking::LaneChange::Both; + const WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint(); + const crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange(); + const auto change_right = crd::element::LaneMarking::LaneChange::Right; + const auto change_left = crd::element::LaneMarking::LaneChange::Left; + const auto change_both = crd::element::LaneMarking::LaneChange::Both; try { if (lane_change == change_right || lane_change == change_both) { - WaypointPtr right_waypoint = raw_waypoint->GetRight(); + const WaypointPtr right_waypoint = raw_waypoint->GetRight(); if (right_waypoint != nullptr && right_waypoint->GetType() == crd::Lane::LaneType::Driving && (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { @@ -222,7 +232,7 @@ namespace MapConstants { if (lane_change == change_left || lane_change == change_both) { - WaypointPtr left_waypoint = raw_waypoint->GetLeft(); + const WaypointPtr left_waypoint = raw_waypoint->GetLeft(); if (left_waypoint != nullptr && left_waypoint->GetType() == crd::Lane::LaneType::Driving && (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { @@ -237,10 +247,11 @@ namespace MapConstants { } catch (const std::invalid_argument &e) { cg::Location loc = reference_waypoint->GetLocation(); carla::log_info( - "Unable to link lane change connection at: " - + std::to_string(loc.x) + " " - + std::to_string(loc.y) + " " - + std::to_string(loc.z)); + "Unable to link lane change connection at: " + + std::to_string(loc.x) + " " + + std::to_string(loc.y) + " " + + std::to_string(loc.z)); } } -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.h b/LibCarla/source/carla/trafficmanager/InMemoryMap.h index 80a397e23..129b53ad4 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.h +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -17,9 +23,9 @@ namespace traffic_manager { -namespace cg = carla::geom; -namespace cc = carla::client; -namespace crd = carla::road; + namespace cg = carla::geom; + namespace cc = carla::client; + namespace crd = carla::road; using WaypointPtr = carla::SharedPtr; using TopologyList = std::vector>; @@ -71,4 +77,4 @@ namespace crd = carla::road; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index 196554187..ff8ce56f4 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -1,8 +1,15 @@ +// Copyright (c) 2019 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 . + #include "LocalizationStage.h" namespace traffic_manager { namespace LocalizationConstants { + static const float WAYPOINT_TIME_HORIZON = 3.0f; static const float MINIMUM_HORIZON_LENGTH = 30.0f; static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f; @@ -11,7 +18,9 @@ namespace LocalizationConstants { static const float HIGHWAY_SPEED = 50 / 3.6f; static const float MINIMUM_LANE_CHANGE_DISTANCE = 20.0f; static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.93969f; -} + +} // namespace LocalizationConstants + using namespace LocalizationConstants; LocalizationStage::LocalizationStage( @@ -55,18 +64,18 @@ namespace LocalizationConstants { void LocalizationStage::Action() { // Selecting output frames based on selector keys. - auto current_planner_frame = planner_frame_selector ? planner_frame_a : planner_frame_b; - auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b; - auto current_traffic_light_frame = + const auto current_planner_frame = planner_frame_selector ? planner_frame_a : planner_frame_b; + const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b; + const auto current_traffic_light_frame = traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b; // Looping over registered actors. for (uint i = 0u; i < actor_list.size(); ++i) { - Actor vehicle = actor_list.at(i); - ActorId actor_id = vehicle->GetId(); - cg::Location vehicle_location = vehicle->GetLocation(); - float vehicle_velocity = vehicle->GetVelocity().Length(); + const Actor vehicle = actor_list.at(i); + const ActorId actor_id = vehicle->GetId(); + const cg::Location vehicle_location = vehicle->GetLocation(); + const float vehicle_velocity = vehicle->GetVelocity().Length(); //TODO: Improve search so it doesn't do it every loop.. auto search = idle_time.find(actor_id); @@ -74,7 +83,7 @@ namespace LocalizationConstants { idle_time[actor_id] = chr::system_clock::now(); } - float horizon_size = std::max( + const float horizon_size = std::max( WAYPOINT_TIME_HORIZON * std::sqrt(vehicle_velocity * 10.0f), MINIMUM_HORIZON_LENGTH); @@ -84,9 +93,8 @@ namespace LocalizationConstants { if (!waypoint_buffer.empty()) { float dot_product = DeviationDotProduct(vehicle, waypoint_buffer.front()->GetLocation(), true); - while (dot_product <= 0 && !waypoint_buffer.empty()) { + while (dot_product <= 0.0f && !waypoint_buffer.empty()) { - //ResetIdleTime(vehicle); PopWaypoint(waypoint_buffer, actor_id); if (!waypoint_buffer.empty()) { dot_product = DeviationDotProduct(vehicle, waypoint_buffer.front()->GetLocation(), true); @@ -104,10 +112,10 @@ namespace LocalizationConstants { } // Assign a lane change. - SimpleWaypointPtr front_waypoint = waypoint_buffer.front(); - ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(vehicle); - bool force_lane_change = lane_change_info.change_lane; - bool lane_change_direction = lane_change_info.direction; + const SimpleWaypointPtr front_waypoint = waypoint_buffer.front(); + const ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(vehicle); + const bool force_lane_change = lane_change_info.change_lane; + const bool lane_change_direction = lane_change_info.direction; if ((parameters.GetAutoLaneChange(vehicle) || force_lane_change) && !front_waypoint->CheckJunction()) { @@ -117,7 +125,7 @@ namespace LocalizationConstants { if (change_over_point != nullptr) { auto number_of_pops = waypoint_buffer.size(); - for (uint j = 0; j < number_of_pops; ++j) { + for (uint j = 0u; j < number_of_pops; ++j) { PopWaypoint(waypoint_buffer, actor_id); } @@ -140,7 +148,7 @@ namespace LocalizationConstants { } // Generating output. - float target_point_distance = std::max(std::ceil(vehicle_velocity * TARGET_WAYPOINT_TIME_HORIZON), + const float target_point_distance = std::max(std::ceil(vehicle_velocity * TARGET_WAYPOINT_TIME_HORIZON), TARGET_WAYPOINT_HORIZON_LENGTH); SimpleWaypointPtr target_waypoint = waypoint_buffer.front(); for (uint j = 0u; @@ -150,38 +158,22 @@ namespace LocalizationConstants { ++j) { target_waypoint = waypoint_buffer.at(j); } - cg::Location target_location = target_waypoint->GetLocation(); + const cg::Location target_location = target_waypoint->GetLocation(); float dot_product = DeviationDotProduct(vehicle, target_location); float cross_product = DeviationCrossProduct(vehicle, target_location); - dot_product = 1 - dot_product; - if (cross_product < 0) { - dot_product *= -1; + dot_product = 1.0f - dot_product; + if (cross_product < 0.0f) { + dot_product *= -1.0f; } - /* Calculate the distance between the car and the trajectory (TODO: Use in the PID) - auto Vehicle = boost::static_pointer_cast(vehicle); - // Calculate the parameters of the line - cg::Vector3D velocity = Vehicle->GetVelocity(); - velocity.z = 0; - cg::Location trajectory_location_ini = waypoint_buffer.front()->GetLocation(); - trajectory_location_ini.z = 0; - cg::Location trajectory_location_fin = target_location; - trajectory_location_fin.z = 0; - cg::Vector3D trajectory_vector = trajectory_location_fin - trajectory_location_ini; - float a = trajectory_vector.x; - float b = -trajectory_vector.y; - float c = b*trajectory_location_ini.y - a*trajectory_location_ini.x; - // Distance from the vehicle to the desired trajectory - float distance = (a*vehicle_location.x + b*vehicle_location.y + c) / - std::sqrt(std::pow(a,2.0f)+std::pow(b,2.0f));*/ - float distance = 0; // TODO: use in PID + float distance = 0.0f; // TODO: use in PID // Filtering out false junctions on highways. // On highways, if there is only one possible path and the section is // marked as intersection, ignore it. - auto vehicle_reference = boost::static_pointer_cast(vehicle); - float speed_limit = vehicle_reference->GetSpeedLimit(); - float look_ahead_distance = std::max(2 * vehicle_velocity, MINIMUM_JUNCTION_LOOK_AHEAD); + const auto vehicle_reference = boost::static_pointer_cast(vehicle); + const float speed_limit = vehicle_reference->GetSpeedLimit(); + const float look_ahead_distance = std::max(2.0f * vehicle_velocity, MINIMUM_JUNCTION_LOOK_AHEAD); SimpleWaypointPtr look_ahead_point = waypoint_buffer.front(); uint look_ahead_index = 0u; @@ -209,20 +201,21 @@ namespace LocalizationConstants { } // Clean up tracking list by remove vehicles that are too far away. - ActorIdSet current_tracking_list = track_traffic.GetOverlappingVehicles(actor_id); + const ActorIdSet current_tracking_list = track_traffic.GetOverlappingVehicles(actor_id); for (ActorId tracking_id: current_tracking_list) { if (!waypoint_buffer.empty()) { - cg::Location tracking_location = actor_list.at( - vehicle_id_to_index.at(tracking_id))->GetLocation(); + const cg::Location tracking_location = actor_list.at( + vehicle_id_to_index.at(tracking_id))->GetLocation(); - cg::Location buffer_front_loc = waypoint_buffer.front()->GetLocation(); - cg::Location buffer_mid_lock = waypoint_buffer.at( - static_cast(std::floor(waypoint_buffer.size()/2)))->GetLocation(); - cg::Location buffer_back_loc = waypoint_buffer.back()->GetLocation(); + const cg::Location buffer_front_loc = waypoint_buffer.front()->GetLocation(); + const cg::Location buffer_mid_lock = waypoint_buffer.at( + static_cast(std::floor(waypoint_buffer.size()/2)))->GetLocation(); + const cg::Location buffer_back_loc = waypoint_buffer.back()->GetLocation(); - double squared_buffer_length = std::pow(buffer_front_loc.Distance(buffer_mid_lock) - + buffer_mid_lock.Distance(buffer_back_loc), 2); + const double squared_buffer_length = std::pow( + buffer_front_loc.Distance(buffer_mid_lock) + + buffer_mid_lock.Distance(buffer_back_loc), 2); if (cg::Math::DistanceSquared(vehicle_location, tracking_location) > squared_buffer_length) { track_traffic.RemoveOverlappingVehicle(actor_id, tracking_id); @@ -246,7 +239,7 @@ namespace LocalizationConstants { collision_message.actor = vehicle; collision_message.buffer = waypoint_buffer; collision_message.overlapping_actors = track_traffic.GetOverlappingVehicles(actor_id); - } + } LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i); traffic_light_message.actor = vehicle; @@ -260,32 +253,8 @@ namespace LocalizationConstants { collision_frame_ready = true; } - //for (uint i = 0u; i < actor_list.size(); ++i) { - // CheckIdleTime(actor_list.at(i)); - //} - } - /*void LocalizationStage::ResetIdleTime(Actor actor) { - idle_time[actor->GetId()] = chr::system_clock::now(); - } - - void LocalizationStage::CheckIdleTime(Actor actor) { - chr::duration time_difference = chr::system_clock::now() - idle_time[actor->GetId()]; - //if (time_difference.count() > 60.0f) { // 60 seconds - //std::cout << "The time difference is: " << time_difference.count() << "s for actor id: " << actor->GetId() << std::endl; - // TODO: Kill it and don't break stuff. Right now, it breaks stuff. - //} - } - - void LocalizationStage::KillVehicle(Actor actor) { - // TODO: Fix this properly, either here or in another (and probably better) loop. - std::vector> to_remove; - to_remove.push_back(actor); - //registered_actors.Destroy(to_remove); - debug_helper.DrawString(actor->GetLocation() + cg::Location(0,0,2), "I should be dead.", false, {255u, 0u, 0u}, 0.1f); - }*/ - void LocalizationStage::DataReceiver() { // Building a list of registered actors and @@ -331,7 +300,7 @@ namespace LocalizationConstants { // which takes the most priority (which needs the highest rate of data feed) // to run the system well. - DataPacket> planner_data_packet = { + const DataPacket> planner_data_packet = { planner_messenger_state, planner_frame_selector ? planner_frame_a : planner_frame_b }; @@ -344,10 +313,10 @@ namespace LocalizationConstants { if ((collision_messenger_current_state != collision_messenger_state) && collision_frame_ready) { - DataPacket> collision_data_packet = { - collision_messenger_state, - collision_frame_selector ? collision_frame_a : collision_frame_b - }; + const DataPacket> collision_data_packet = { + collision_messenger_state, + collision_frame_selector ? collision_frame_a : collision_frame_b + }; collision_messenger_state = collision_messenger->SendData(collision_data_packet); collision_frame_selector = !collision_frame_selector; @@ -356,12 +325,12 @@ namespace LocalizationConstants { // Send data to traffic light stage only if it has finished // processing, received the previous message and started processing it. - int traffic_light_messenger_current_state = traffic_light_messenger->GetState(); + const int traffic_light_messenger_current_state = traffic_light_messenger->GetState(); if (traffic_light_messenger_current_state != traffic_light_messenger_state) { - DataPacket> traffic_light_data_packet = { - traffic_light_messenger_state, - traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b - }; + const DataPacket> traffic_light_data_packet = { + traffic_light_messenger_state, + traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b + }; traffic_light_messenger_state = traffic_light_messenger->SendData(traffic_light_data_packet); traffic_light_frame_selector = !traffic_light_frame_selector; @@ -377,18 +346,18 @@ namespace LocalizationConstants { void LocalizationStage::PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint) { - uint64_t waypoint_id = waypoint->GetId(); + const uint64_t waypoint_id = waypoint->GetId(); buffer.push_back(waypoint); track_traffic.UpdatePassingVehicle(waypoint_id, actor_id); - ActorIdSet current_actors = track_traffic.GetOverlappingVehicles(actor_id); - ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(waypoint_id); + const ActorIdSet current_actors = track_traffic.GetPassingVehicles(actor_id); + const ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(waypoint_id); ActorIdSet actor_set_difference; std::set_difference( - new_overlapping_actors.begin(), new_overlapping_actors.end(), - current_actors.begin(), current_actors.end(), - std::inserter(actor_set_difference, actor_set_difference.end()) + new_overlapping_actors.begin(), new_overlapping_actors.end(), + current_actors.begin(), current_actors.end(), + std::inserter(actor_set_difference, actor_set_difference.end()) ); for (auto new_actor_id: actor_set_difference) { @@ -400,20 +369,20 @@ namespace LocalizationConstants { void LocalizationStage::PopWaypoint(Buffer& buffer, ActorId actor_id) { - uint64_t removed_waypoint_id = buffer.front()->GetId(); + const uint64_t removed_waypoint_id = buffer.front()->GetId(); buffer.pop_front(); track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id); if (!buffer.empty()) { - ActorIdSet current_actors = track_traffic.GetPassingVehicles(removed_waypoint_id); - ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(buffer.front()->GetId()); + const ActorIdSet current_actors = track_traffic.GetPassingVehicles(removed_waypoint_id); + const ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(buffer.front()->GetId()); ActorIdSet actor_set_difference; std::set_difference( - current_actors.begin(), current_actors.end(), - new_overlapping_actors.begin(), new_overlapping_actors.end(), - std::inserter(actor_set_difference, actor_set_difference.end()) + current_actors.begin(), current_actors.end(), + new_overlapping_actors.begin(), new_overlapping_actors.end(), + std::inserter(actor_set_difference, actor_set_difference.end()) ); for (auto& old_actor_id: actor_set_difference) { @@ -423,7 +392,7 @@ namespace LocalizationConstants { } } else { - ActorIdSet currently_tracked_vehicles = track_traffic.GetOverlappingVehicles(actor_id); + const ActorIdSet currently_tracked_vehicles = track_traffic.GetOverlappingVehicles(actor_id); for (auto& tracked_id: currently_tracked_vehicles) { track_traffic.RemoveOverlappingVehicle(actor_id, tracked_id); track_traffic.RemoveOverlappingVehicle(tracked_id, actor_id); @@ -433,20 +402,20 @@ namespace LocalizationConstants { SimpleWaypointPtr LocalizationStage::AssignLaneChange(Actor vehicle, bool force, bool direction) { - ActorId actor_id = vehicle->GetId(); - cg::Location vehicle_location = vehicle->GetLocation(); - float vehicle_velocity = vehicle->GetVelocity().Length(); + const ActorId actor_id = vehicle->GetId(); + const cg::Location vehicle_location = vehicle->GetLocation(); + const float vehicle_velocity = vehicle->GetVelocity().Length(); - Buffer& waypoint_buffer = buffer_list->at(vehicle_id_to_index.at(actor_id)); - SimpleWaypointPtr& current_waypoint = waypoint_buffer.front(); + const Buffer& waypoint_buffer = buffer_list->at(vehicle_id_to_index.at(actor_id)); + const SimpleWaypointPtr& current_waypoint = waypoint_buffer.front(); bool need_to_change_lane = false; - auto left_waypoint = current_waypoint->GetLeftWaypoint(); - auto right_waypoint = current_waypoint->GetRightWaypoint(); + const auto left_waypoint = current_waypoint->GetLeftWaypoint(); + const auto right_waypoint = current_waypoint->GetRightWaypoint(); if (!force) { - auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id); + const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id); bool abort_lane_change = false; for (auto i = blocking_vehicles.begin(); @@ -454,13 +423,14 @@ namespace LocalizationConstants { ++i) { const ActorId &other_vehicle_id = *i; - Buffer& other_buffer = buffer_list->at(vehicle_id_to_index.at(other_vehicle_id)); - SimpleWaypointPtr& other_current_waypoint = other_buffer.front(); - cg::Location other_location = other_current_waypoint->GetLocation(); + const Buffer& other_buffer = buffer_list->at(vehicle_id_to_index.at(other_vehicle_id)); + const SimpleWaypointPtr& other_current_waypoint = other_buffer.front(); + const cg::Location other_location = other_current_waypoint->GetLocation(); bool distant_lane_availability = false; - auto other_neighbouring_lanes = {other_current_waypoint->GetLeftWaypoint(), - other_current_waypoint->GetRightWaypoint()}; + const auto other_neighbouring_lanes = { + other_current_waypoint->GetLeftWaypoint(), + other_current_waypoint->GetRightWaypoint()}; for (auto& candidate_lane_wp: other_neighbouring_lanes) { if (candidate_lane_wp != nullptr && @@ -470,16 +440,16 @@ namespace LocalizationConstants { } } - cg::Vector3D reference_heading = current_waypoint->GetForwardVector(); - cg::Vector3D other_heading = other_current_waypoint->GetForwardVector(); + const cg::Vector3D reference_heading = current_waypoint->GetForwardVector(); + const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector(); if (other_vehicle_id != actor_id && !current_waypoint->CheckJunction() && !other_current_waypoint->CheckJunction() && cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) { - float squared_vehicle_distance = cg::Math::DistanceSquared(other_location, vehicle_location); - float deviation_dot = DeviationDotProduct(vehicle, other_location); + const float squared_vehicle_distance = cg::Math::DistanceSquared(other_location, vehicle_location); + const float deviation_dot = DeviationDotProduct(vehicle, other_location); if (deviation_dot > 0.0f) { @@ -502,7 +472,7 @@ namespace LocalizationConstants { need_to_change_lane = true; } - float change_over_distance = std::max(2.0f*vehicle_velocity, 10.0f); + const float change_over_distance = std::max(2.0f*vehicle_velocity, 10.0f); bool possible_to_lane_change = false; SimpleWaypointPtr change_over_point = nullptr; @@ -537,7 +507,7 @@ namespace LocalizationConstants { } if (need_to_change_lane && possible_to_lane_change) { - auto starting_point = change_over_point; + const auto starting_point = change_over_point; while (change_over_point->DistanceSquared(starting_point) < std::pow(change_over_distance, 2) && !change_over_point->CheckJunction()) { change_over_point = change_over_point->GetNextWaypoint()[0]; @@ -548,4 +518,4 @@ namespace LocalizationConstants { } } -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index a915b3ee8..e51bcda2b 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -1,13 +1,19 @@ +// Copyright (c) 2019 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 . + #pragma once #include +#include #include +#include #include #include #include #include -#include -#include #include "carla/client/Actor.h" #include "carla/client/Vehicle.h" @@ -28,8 +34,8 @@ namespace traffic_manager { -using namespace std::chrono; -namespace cc = carla::client; + using namespace std::chrono; + namespace cc = carla::client; using Actor = carla::SharedPtr; using ActorId = carla::ActorId; using ActorIdSet = std::unordered_set; @@ -95,13 +101,8 @@ namespace cc = carla::client; /// Map of all vehicles' idle time std::unordered_map> idle_time; - /// A simple method used to draw waypoint buffer ahead of a vehicle. void DrawBuffer(Buffer &buffer); - /// Methods for idle vehicle elimination. - /* void ResetIdleTime(Actor actor); - void CheckIdleTime(Actor actor); - void KillVehicle(Actor actor);*/ /// Method to determine lane change and obtain target lane waypoint. SimpleWaypointPtr AssignLaneChange(Actor vehicle, bool force, bool direction); @@ -112,14 +113,14 @@ namespace cc = carla::client; public: LocalizationStage( - std::string stage_name, - std::shared_ptr planner_messenger, - std::shared_ptr collision_messenger, - std::shared_ptr traffic_light_messenger, - AtomicActorSet ®istered_actors, - InMemoryMap &local_map, - Parameters ¶meters, - cc::DebugHelper &debug_helper); + std::string stage_name, + std::shared_ptr planner_messenger, + std::shared_ptr collision_messenger, + std::shared_ptr traffic_light_messenger, + AtomicActorSet ®istered_actors, + InMemoryMap &local_map, + Parameters ¶meters, + cc::DebugHelper &debug_helper); ~LocalizationStage(); @@ -131,4 +132,4 @@ namespace cc = carla::client; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp index a083fc878..380241cb6 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp @@ -1,82 +1,87 @@ +// Copyright (c) 2019 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 . #include "carla/trafficmanager/LocalizationUtils.h" namespace traffic_manager { - float DeviationCrossProduct(Actor actor, const cg::Location &target_location) { + float DeviationCrossProduct(Actor actor, const cg::Location &target_location) { - cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); - heading_vector.z = 0; - heading_vector = heading_vector.MakeUnitVector(); - cg::Location next_vector = target_location - actor->GetLocation(); - next_vector.z = 0; - if (next_vector.Length() > 2.0f * std::numeric_limits::epsilon()) { - next_vector = next_vector.MakeUnitVector(); - float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x; - return cross_z; - } else { - return 0; - } + cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); + heading_vector.z = 0.0f; + heading_vector = heading_vector.MakeUnitVector(); + cg::Location next_vector = target_location - actor->GetLocation(); + next_vector.z = 0.0f; + if (next_vector.Length() > 2.0f * std::numeric_limits::epsilon()) { + next_vector = next_vector.MakeUnitVector(); + const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x; + return cross_z; + } else { + return 0.0f; + } + } + + float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset) { + + cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); + heading_vector.z = 0.0f; + heading_vector = heading_vector.MakeUnitVector(); + cg::Location next_vector; + + if (!rear_offset) { + next_vector = target_location - actor->GetLocation(); + } else { + const auto vehicle_ptr = boost::static_pointer_cast(actor); + const float vehicle_half_length = vehicle_ptr->GetBoundingBox().extent.x; + next_vector = target_location - (cg::Location(-1* vehicle_half_length * heading_vector) + + vehicle_ptr->GetLocation()); } - float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset) { - - cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); - heading_vector.z = 0; - heading_vector = heading_vector.MakeUnitVector(); - cg::Location next_vector; - - if (!rear_offset) { - next_vector = target_location - actor->GetLocation(); - } else { - auto vehicle_ptr = boost::static_pointer_cast(actor); - float vehicle_half_length = vehicle_ptr->GetBoundingBox().extent.x; - next_vector = target_location - (cg::Location(-1* vehicle_half_length * heading_vector) - + vehicle_ptr->GetLocation()); - } - - next_vector.z = 0; - if (next_vector.Length() > 2.0f * std::numeric_limits::epsilon()) { - next_vector = next_vector.MakeUnitVector(); - float dot_product = cg::Math::Dot(next_vector, heading_vector); - return dot_product; - } else { - return 0; - } + next_vector.z = 0.0f; + if (next_vector.Length() > 2.0f * std::numeric_limits::epsilon()) { + next_vector = next_vector.MakeUnitVector(); + const float dot_product = cg::Math::Dot(next_vector, heading_vector); + return dot_product; + } else { + return 0.0f; } + } - TrackTraffic::TrackTraffic() {} + TrackTraffic::TrackTraffic() {} - void TrackTraffic::UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id) { + void TrackTraffic::UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id) { - if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) { - ActorIdSet& actor_set = overlapping_vehicles.at(actor_id); - if (actor_set.find(other_id) == actor_set.end()) { - actor_set.insert(other_id); - } - } else { - overlapping_vehicles.insert({actor_id, {other_id}}); + if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) { + ActorIdSet& actor_set = overlapping_vehicles.at(actor_id); + if (actor_set.find(other_id) == actor_set.end()) { + actor_set.insert(other_id); + } + } else { + overlapping_vehicles.insert({actor_id, {other_id}}); + } + } + + void TrackTraffic::RemoveOverlappingVehicle(ActorId actor_id, ActorId other_id) { + + if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) { + ActorIdSet& actor_set = overlapping_vehicles.at(actor_id); + if (actor_set.find(actor_id) != actor_set.end()) { + actor_set.erase(other_id); } } + } - void TrackTraffic::RemoveOverlappingVehicle(ActorId actor_id, ActorId other_id) { + ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) { - if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) { - ActorIdSet& actor_set = overlapping_vehicles.at(actor_id); - if (actor_set.find(actor_id) != actor_set.end()) { - actor_set.erase(other_id); - } - } - } - - ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) { - - if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) { - return overlapping_vehicles.at(actor_id); - } else { - return ActorIdSet(); - } + if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) { + return overlapping_vehicles.at(actor_id); + } else { + return ActorIdSet(); } + } void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) { @@ -118,4 +123,4 @@ namespace traffic_manager { } } -} \ No newline at end of file +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h index 75d5278f2..15508eee5 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h @@ -1,3 +1,10 @@ +// Copyright (c) 2019 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 . + +#pragma once #include "carla/client/Actor.h" #include "carla/client/Vehicle.h" @@ -8,46 +15,46 @@ namespace traffic_manager { - namespace cc = carla::client; - namespace cg = carla::geom; - using Actor = carla::SharedPtr; - using ActorId = carla::ActorId; - using ActorIdSet = std::unordered_set; - using SimpleWaypointPtr = std::shared_ptr; - using Buffer = std::deque; + namespace cc = carla::client; + namespace cg = carla::geom; + using Actor = carla::SharedPtr; + using ActorId = carla::ActorId; + using ActorIdSet = std::unordered_set; + using SimpleWaypointPtr = std::shared_ptr; + using Buffer = std::deque; - class TrackTraffic{ + class TrackTraffic{ - private: - /// Structure to keep track of overlapping waypoints between vehicles. - using WaypointOverlap = std::unordered_map; - WaypointOverlap waypoint_overlap_tracker; - /// Structure to keep track of vehicles with overlapping paths. - std::unordered_map overlapping_vehicles; - /// Stored vehicle id set record. - ActorIdSet actor_id_set_record; + private: + /// Structure to keep track of overlapping waypoints between vehicles. + using WaypointOverlap = std::unordered_map; + WaypointOverlap waypoint_overlap_tracker; + /// Structure to keep track of vehicles with overlapping paths. + std::unordered_map overlapping_vehicles; + /// Stored vehicle id set record. + ActorIdSet actor_id_set_record; - public: - TrackTraffic(/* args */); + public: + TrackTraffic(); - /// Methods to update, remove and retrieve vehicles passing through a waypoint. - void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id); - void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id); - ActorIdSet GetPassingVehicles(uint64_t waypoint_id); + /// Methods to update, remove and retrieve vehicles passing through a waypoint. + void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id); + void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id); + ActorIdSet GetPassingVehicles(uint64_t waypoint_id); - /// Methods to update, remove and retrieve vehicles with overlapping vehicles. - void UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id); - void RemoveOverlappingVehicle(ActorId actor_id, ActorId other_id); - ActorIdSet GetOverlappingVehicles(ActorId actor_id); + /// Methods to update, remove and retrieve vehicles with overlapping vehicles. + void UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id); + void RemoveOverlappingVehicle(ActorId actor_id, ActorId other_id); + ActorIdSet GetOverlappingVehicles(ActorId actor_id); - }; + }; - /// Returns the cross product (z component value) between the vehicle's - /// heading vector and the vector along the direction to the next - /// target waypoint on the horizon. - float DeviationCrossProduct(Actor actor, const cg::Location &target_location); - /// Returns the dot product between the vehicle's heading vector and - /// the vector along the direction to the next target waypoint on the horizon. - float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset=false); + /// Returns the cross product (z component value) between the vehicle's + /// heading vector and the vector along the direction to the next + /// target waypoint on the horizon. + float DeviationCrossProduct(Actor actor, const cg::Location &target_location); + /// Returns the dot product between the vehicle's heading vector and + /// the vector along the direction to the next target waypoint on the horizon. + float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset=false); -} \ No newline at end of file +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/Messenger.h b/LibCarla/source/carla/trafficmanager/Messenger.h index 7b7a3689c..b4662fd14 100644 --- a/LibCarla/source/carla/trafficmanager/Messenger.h +++ b/LibCarla/source/carla/trafficmanager/Messenger.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -97,4 +103,4 @@ namespace traffic_manager { }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h index a678e8e5f..0b5f9e207 100644 --- a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h +++ b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -12,7 +18,7 @@ namespace traffic_manager { -namespace cc = carla::client; + namespace cc = carla::client; /// Convenience typing. @@ -82,4 +88,5 @@ namespace cc = carla::client; using LocalizationToTrafficLightMessenger = Messenger>; using CollisionToPlannerMessenger = Messenger>; using TrafficLightToPlannerMessenger = Messenger>; -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp index 31ae9f54e..53f717d19 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp @@ -1,11 +1,19 @@ +// Copyright (c) 2019 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 . + #include "MotionPlannerStage.h" namespace traffic_manager { namespace PlannerConstants { - static const float HIGHWAY_SPEED = 50 / 3.6f; -} + static const float HIGHWAY_SPEED = 50.0f / 3.6f; + +} // namespace PlannerConstants + using namespace PlannerConstants; MotionPlannerStage::MotionPlannerStage( @@ -52,25 +60,25 @@ namespace PlannerConstants { void MotionPlannerStage::Action() { // Selecting an output frame. - auto current_control_frame = frame_selector ? control_frame_a : control_frame_b; + const auto current_control_frame = frame_selector ? control_frame_a : control_frame_b; // Looping over all vehicles. for (uint i = 0u; i < number_of_vehicles; ++i) { - LocalizationToPlannerData &localization_data = localization_frame->at(i); - Actor actor = localization_data.actor; - float current_deviation = localization_data.deviation; - float current_distance = localization_data.distance; + const LocalizationToPlannerData &localization_data = localization_frame->at(i); + const Actor actor = localization_data.actor; + const float current_deviation = localization_data.deviation; + const float current_distance = localization_data.distance; - ActorId actor_id = actor->GetId(); + const ActorId actor_id = actor->GetId(); - auto vehicle = boost::static_pointer_cast(actor); - float current_velocity = vehicle->GetVelocity().Length(); + const auto vehicle = boost::static_pointer_cast(actor); + const float current_velocity = vehicle->GetVelocity().Length(); - auto current_time = chr::system_clock::now(); + const auto current_time = chr::system_clock::now(); if (pid_state_map.find(actor_id) == pid_state_map.end()) { - auto initial_state = StateEntry{0.0f, 0.0f, 0.0f, chr::system_clock::now(), 0.0f, 0.0f, 0.0f}; + const auto initial_state = StateEntry{0.0f, 0.0f, 0.0f, chr::system_clock::now(), 0.0f, 0.0f, 0.0f}; pid_state_map.insert({actor_id, initial_state}); } @@ -79,9 +87,9 @@ namespace PlannerConstants { previous_state = pid_state_map.at(actor_id); // Increase speed if on highway. - float speed_limit = vehicle->GetSpeedLimit() / 3.6f; + const float speed_limit = vehicle->GetSpeedLimit() / 3.6f; - float dynamic_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f; + const float dynamic_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f; if (speed_limit > HIGHWAY_SPEED) { longitudinal_parameters = highway_longitudinal_parameters; @@ -130,28 +138,27 @@ namespace PlannerConstants { message.throttle = actuation_signal.throttle; message.brake = actuation_signal.brake; message.steer = actuation_signal.steer; - //DrawPIDValues(vehicle, actuation_signal.throttle, actuation_signal.brake); } } void MotionPlannerStage::DataReceiver() { - auto localization_packet = localization_messenger->ReceiveData(localization_messenger_state); + const auto localization_packet = localization_messenger->ReceiveData(localization_messenger_state); localization_frame = localization_packet.data; localization_messenger_state = localization_packet.id; // Block on receive call only if new data is available on the messenger. - int collision_messenger_current_state = collision_messenger->GetState(); + const int collision_messenger_current_state = collision_messenger->GetState(); if (collision_messenger_current_state != collision_messenger_state) { - auto collision_packet = collision_messenger->ReceiveData(collision_messenger_state); + const auto collision_packet = collision_messenger->ReceiveData(collision_messenger_state); collision_frame = collision_packet.data; collision_messenger_state = collision_packet.id; } // Block on receive call only if new data is available on the messenger. - int traffic_light_messenger_current_state = traffic_light_messenger->GetState(); + const int traffic_light_messenger_current_state = traffic_light_messenger->GetState(); if (traffic_light_messenger_current_state != traffic_light_messenger_state) { - auto traffic_light_packet = traffic_light_messenger->ReceiveData(traffic_light_messenger_state); + const auto traffic_light_packet = traffic_light_messenger->ReceiveData(traffic_light_messenger_state); traffic_light_frame = traffic_light_packet.data; traffic_light_messenger_state = traffic_light_packet.id; } @@ -170,15 +177,16 @@ namespace PlannerConstants { void MotionPlannerStage::DataSender() { DataPacket> data_packet = { - control_messenger_state, - frame_selector ? control_frame_a : control_frame_b - }; + control_messenger_state, + frame_selector ? control_frame_a : control_frame_b + }; frame_selector = !frame_selector; control_messenger_state = control_messenger->SendData(data_packet); } void MotionPlannerStage::DrawPIDValues(const boost::shared_ptr vehicle, const float throttle, const float brake) { - debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0,0,2), std::to_string(throttle), false, {0u, 255u, 0u}, 0.005f); - debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0,0,4), std::to_string(brake), false, {255u, 0u, 0u}, 0.005f); + debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,2.0f), std::to_string(throttle), false, {0u, 255u, 0u}, 0.005f); + debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,4.0f), std::to_string(brake), false, {255u, 0u, 0u}, 0.005f); } -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h index d09fde48e..857f09772 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -14,8 +20,8 @@ namespace traffic_manager { -namespace chr = std::chrono; -namespace cc = carla::client; + namespace chr = std::chrono; + namespace cc = carla::client; using Actor = carla::SharedPtr; using ActorId = carla::rpc::ActorId; @@ -71,17 +77,17 @@ namespace cc = carla::client; public: MotionPlannerStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr collision_messenger, - std::shared_ptr traffic_light_messenger, - std::shared_ptr control_messenger, - Parameters ¶meters, - std::vector longitudinal_parameters, - std::vector highway_longitudinal_parameters, - std::vector lateral_parameters, - std::vector highway_lateral_parameters, - cc::DebugHelper &debug_helper); + std::string stage_name, + std::shared_ptr localization_messenger, + std::shared_ptr collision_messenger, + std::shared_ptr traffic_light_messenger, + std::shared_ptr control_messenger, + Parameters ¶meters, + std::vector longitudinal_parameters, + std::vector highway_longitudinal_parameters, + std::vector lateral_parameters, + std::vector highway_lateral_parameters, + cc::DebugHelper &debug_helper); ~MotionPlannerStage(); @@ -95,4 +101,4 @@ namespace cc = carla::client; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/PIDController.cpp b/LibCarla/source/carla/trafficmanager/PIDController.cpp index bb092d37e..1705cbe18 100644 --- a/LibCarla/source/carla/trafficmanager/PIDController.cpp +++ b/LibCarla/source/carla/trafficmanager/PIDController.cpp @@ -1,11 +1,20 @@ +// Copyright (c) 2019 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 . + #include "PIDController.h" namespace traffic_manager { namespace PIDControllerConstants { + const float MAX_THROTTLE = 0.7f; const float MAX_BRAKE = 1.0f; -} + +} // namespace PIDControllerConstants + using namespace PIDControllerConstants; PIDController::PIDController() {} @@ -29,8 +38,8 @@ namespace PIDControllerConstants { }; // Calculating dt for 'D' and 'I' controller components. - chr::duration duration = current_state.time_instance - previous_state.time_instance; - float dt = duration.count(); + const chr::duration duration = current_state.time_instance - previous_state.time_instance; + const float dt = duration.count(); // Calculating integrals. current_state.deviation_integral = angular_deviation * dt + previous_state.deviation_integral; @@ -47,11 +56,12 @@ namespace PIDControllerConstants { const std::vector &lateral_parameters) const { // Calculating dt for updating the integral component. - chr::duration duration = present_state.time_instance - previous_state.time_instance; - float dt = duration.count(); + const chr::duration duration = present_state.time_instance - previous_state.time_instance; + const float dt = duration.count(); // Longitudinal PID calculation. - float expr_v = longitudinal_parameters[0] * present_state.velocity + + const float expr_v = + longitudinal_parameters[0] * present_state.velocity + longitudinal_parameters[1] * present_state.velocity_integral + longitudinal_parameters[2] * (present_state.velocity - previous_state.velocity) / dt; @@ -68,24 +78,15 @@ namespace PIDControllerConstants { } // Lateral PID calculation. - float steer = lateral_parameters[0] * present_state.deviation + + float steer = + lateral_parameters[0] * present_state.deviation + lateral_parameters[1] * present_state.deviation_integral + lateral_parameters[2] * (present_state.deviation - previous_state.deviation) / dt; - /*float steer = lateral_parameters[0] * present_state.distance + - lateral_parameters[1] * present_state.distance_integral + - lateral_parameters[2] * (present_state.distance - - previous_state.distance) / dt; - std::cout << "--------------------------------------\n"; - std::cout << steer << "\n"; - std::cout << lateral_parameters[0] * present_state.distance << "\n"; - std::cout << lateral_parameters[1] * present_state.distance_integral << "\n"; - std::cout << lateral_parameters[2] * (present_state.distance - previous_state.distance) / dt << "\n"; - */ - steer = std::max(-1.0f, std::min(steer, 1.0f)); return ActuationSignal{throttle, brake, steer}; } -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/PIDController.h b/LibCarla/source/carla/trafficmanager/PIDController.h index e7f3c8dc4..ea2b41cf0 100644 --- a/LibCarla/source/carla/trafficmanager/PIDController.h +++ b/LibCarla/source/carla/trafficmanager/PIDController.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -6,7 +12,7 @@ namespace traffic_manager { -namespace chr = std::chrono; + namespace chr = std::chrono; using TimeInstance = chr::time_point; /// Structure to hold the actuation signals. @@ -57,4 +63,4 @@ namespace chr = std::chrono; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/Parameters.cpp b/LibCarla/source/carla/trafficmanager/Parameters.cpp index 13ab3b934..3f56281f9 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.cpp +++ b/LibCarla/source/carla/trafficmanager/Parameters.cpp @@ -1,174 +1,181 @@ -#include "Parameters.h" +// Copyright (c) 2019 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 . + #include +#include "Parameters.h" + namespace traffic_manager { - Parameters::Parameters() {} +Parameters::Parameters() {} - Parameters::~Parameters() {} +Parameters::~Parameters() {} - void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { - percentage_difference_from_speed_limit.AddEntry({actor->GetId(), percentage}); +void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { + percentage_difference_from_speed_limit.AddEntry({actor->GetId(), percentage}); +} + +void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) { + global_percentage_difference_from_limit = percentage; +} + +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 actor_set = ignore_collision.GetValue(reference_id); + if (actor_set->Contains(other_id)) { + actor_set->Remove({other_actor}); + } } + } else { - void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) { - global_percentage_difference_from_limit = percentage; + if (ignore_collision.Contains(reference_id)) { + std::shared_ptr actor_set = ignore_collision.GetValue(reference_id); + if (!actor_set->Contains(other_id)) { + actor_set->Insert({other_actor}); + } + } else { + std::shared_ptr actor_set = std::make_shared(); + actor_set->Insert({other_actor}); + auto entry = std::make_pair(reference_id, actor_set); + ignore_collision.AddEntry(entry); } + } +} - void Parameters::SetCollisionDetection( - const ActorPtr &reference_actor, - const ActorPtr &other_actor, - const bool detect_collision) { +void Parameters::SetForceLaneChange(const ActorPtr &actor, const bool direction) { - ActorId reference_id = reference_actor->GetId(); - ActorId other_id = other_actor->GetId(); + const ChangeLaneInfo lane_change_info = {true, direction}; + const auto entry = std::make_pair(actor->GetId(), lane_change_info); + force_lane_change.AddEntry(entry); +} - if (detect_collision) { +void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) { - if (ignore_collision.Contains(reference_id)) { - std::shared_ptr actor_set = ignore_collision.GetValue(reference_id); - if (actor_set->Contains(other_id)) { - actor_set->Remove({other_actor}); - } - } - } else { + const auto entry = std::make_pair(actor->GetId(), enable); + auto_lane_change.AddEntry(entry); +} - if (ignore_collision.Contains(reference_id)) { - std::shared_ptr actor_set = ignore_collision.GetValue(reference_id); - if (!actor_set->Contains(other_id)) { - actor_set->Insert({other_actor}); - } - } else { - std::shared_ptr actor_set = std::make_shared(); - actor_set->Insert({other_actor}); - auto entry = std::make_pair(reference_id, actor_set); - ignore_collision.AddEntry(entry); - } - } - } +void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) { + if (distance > 0.0f) { + const auto entry = std::make_pair(actor->GetId(), distance); + distance_to_leading_vehicle.AddEntry(entry); + } +} - void Parameters::SetForceLaneChange(const ActorPtr &actor, const bool direction) { +float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) { - ChangeLaneInfo lane_change_info = {true, direction}; - auto entry = std::make_pair(actor->GetId(), lane_change_info); - force_lane_change.AddEntry(entry); - } + const ActorId actor_id = actor->GetId(); + const auto vehicle = boost::static_pointer_cast(actor); + const float speed_limit = vehicle->GetSpeedLimit(); + float percentage_difference = global_percentage_difference_from_limit; - void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) { + if (percentage_difference_from_speed_limit.Contains(actor_id)) { + percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id); + } - auto entry = std::make_pair(actor->GetId(), enable); - auto_lane_change.AddEntry(entry); - } + return speed_limit * (1.0f - percentage_difference/100.0f); +} - void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) { - if (distance > 0.0f) { - auto entry = std::make_pair(actor->GetId(), distance); - distance_to_leading_vehicle.AddEntry(entry); - } - } +bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor) { - float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) { + const ActorId reference_actor_id = reference_actor->GetId(); + const ActorId other_actor_id = other_actor->GetId(); + bool avoid_collision = true; - ActorId actor_id = actor->GetId(); - auto vehicle = boost::static_pointer_cast(actor); - float speed_limit = vehicle->GetSpeedLimit(); - float percentage_difference = global_percentage_difference_from_limit; + if (ignore_collision.Contains(reference_actor_id) && + ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) { + avoid_collision = false; + } - if (percentage_difference_from_speed_limit.Contains(actor_id)) { - percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id); - } + return avoid_collision; +} - return speed_limit * (1.0f - percentage_difference/100.0f); - } +ChangeLaneInfo Parameters::GetForceLaneChange(const ActorPtr &actor) { - bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor) { + const ActorId actor_id = actor->GetId(); + ChangeLaneInfo change_lane_info; - ActorId reference_actor_id = reference_actor->GetId(); - ActorId other_actor_id = other_actor->GetId(); - bool avoid_collision = true; + if (force_lane_change.Contains(actor_id)) { + change_lane_info = force_lane_change.GetValue(actor_id); + } - if (ignore_collision.Contains(reference_actor_id) && - ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) { - avoid_collision = false; - } + force_lane_change.RemoveEntry(actor_id); - return avoid_collision; - } + return change_lane_info; +} - ChangeLaneInfo Parameters::GetForceLaneChange(const ActorPtr &actor) { +bool Parameters::GetAutoLaneChange(const ActorPtr &actor) { - ActorId actor_id = actor->GetId(); - ChangeLaneInfo change_lane_info; + const ActorId actor_id = actor->GetId(); + bool auto_lane_change_policy = true; - if (force_lane_change.Contains(actor_id)) { - change_lane_info = force_lane_change.GetValue(actor_id); - } + if (auto_lane_change.Contains(actor_id)) { + auto_lane_change_policy = auto_lane_change.GetValue(actor_id); + } - force_lane_change.RemoveEntry(actor_id); + return auto_lane_change_policy; +} - return change_lane_info; - } +float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) { - bool Parameters::GetAutoLaneChange(const ActorPtr &actor) { + const ActorId actor_id = actor->GetId(); + float distance_margin = -1.0f; - ActorId actor_id = actor->GetId(); - bool auto_lane_change_policy = true; + if (distance_to_leading_vehicle.Contains(actor_id)) { + distance_margin = distance_to_leading_vehicle.GetValue(actor_id); + } - if (auto_lane_change.Contains(actor_id)) { - auto_lane_change_policy = auto_lane_change.GetValue(actor_id); - } + return distance_margin; +} - return auto_lane_change_policy; - } +void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) { + if (perc > 0.0f) { + const auto entry = std::make_pair(actor->GetId(), perc); + perc_run_traffic_light.AddEntry(entry); + } +} - float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) { +void Parameters::SetPercentageIgnoreActors(const ActorPtr &actor, const float perc) { + if (perc > 0.0f) { + const auto entry = std::make_pair(actor->GetId(), perc); + perc_ignore_actors.AddEntry(entry); + } +} - ActorId actor_id = actor->GetId(); - float distance_margin = -1; +float Parameters::GetPercentageRunningLight(const ActorPtr &actor) { - if (distance_to_leading_vehicle.Contains(actor_id)) { - distance_margin = distance_to_leading_vehicle.GetValue(actor_id); - } + const ActorId actor_id = actor->GetId(); + float percentage = 0.0f; - return distance_margin; - } + if (perc_run_traffic_light.Contains(actor_id)) { + percentage = perc_run_traffic_light.GetValue(actor_id); + } - void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) { - if (perc > 0.0f) { - auto entry = std::make_pair(actor->GetId(), perc); - perc_run_traffic_light.AddEntry(entry); - } - } + return percentage; +} - void Parameters::SetPercentageIgnoreActors(const ActorPtr &actor, const float perc) { - if (perc > 0.0f) { - auto entry = std::make_pair(actor->GetId(), perc); - perc_ignore_actors.AddEntry(entry); - } - } +float Parameters::GetPercentageIgnoreActors(const ActorPtr &actor) { - float Parameters::GetPercentageRunningLight(const ActorPtr &actor) { + const ActorId actor_id = actor->GetId(); + float percentage = 0.0f; - ActorId actor_id = actor->GetId(); - float percentage = 0.0f; + if (perc_ignore_actors.Contains(actor_id)) { + percentage = perc_ignore_actors.GetValue(actor_id); + } - if (perc_run_traffic_light.Contains(actor_id)) { - percentage = perc_run_traffic_light.GetValue(actor_id); - } + return percentage; +} - return percentage; - } - - float Parameters::GetPercentageIgnoreActors(const ActorPtr &actor) { - - ActorId actor_id = actor->GetId(); - float percentage = 0.0f; - - if (perc_ignore_actors.Contains(actor_id)) { - percentage = perc_ignore_actors.GetValue(actor_id); - } - - return percentage; - } - -} \ No newline at end of file +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/Parameters.h b/LibCarla/source/carla/trafficmanager/Parameters.h index ecd810dbc..35649ada5 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.h +++ b/LibCarla/source/carla/trafficmanager/Parameters.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include "carla/client/Actor.h" @@ -10,89 +16,91 @@ namespace traffic_manager { - namespace cc = carla::client; - using ActorPtr = carla::SharedPtr; - using ActorId = carla::ActorId; + namespace cc = carla::client; + using ActorPtr = carla::SharedPtr; + using ActorId = carla::ActorId; - struct ChangeLaneInfo { - bool change_lane = false; - bool direction = false; - }; + struct ChangeLaneInfo { + bool change_lane = false; + bool direction = false; + }; - class Parameters { + class Parameters { - private: + private: - /// Target velocity map for individual vehicles. - AtomicMap percentage_difference_from_speed_limit; - /// Global target velocity limit % difference. - float global_percentage_difference_from_limit = 0; - /// Map containing a set of actors to be ignored during collision detection. - AtomicMap> ignore_collision; - /// Map containing distance to leading vehicle command. - AtomicMap distance_to_leading_vehicle; - /// Map containing force lane change commands. - AtomicMap force_lane_change; - /// Map containing auto lane change commands. - AtomicMap auto_lane_change; - /// Map containing % of running a traffic light. - AtomicMap perc_run_traffic_light; - /// Map containing % of ignoring actors. - AtomicMap perc_ignore_actors; + /// Target velocity map for individual vehicles. + AtomicMap percentage_difference_from_speed_limit; + /// Global target velocity limit % difference. + float global_percentage_difference_from_limit = 0; + /// Map containing a set of actors to be ignored during collision detection. + AtomicMap> ignore_collision; + /// Map containing distance to leading vehicle command. + AtomicMap distance_to_leading_vehicle; + /// Map containing force lane change commands. + AtomicMap force_lane_change; + /// Map containing auto lane change commands. + AtomicMap auto_lane_change; + /// Map containing % of running a traffic light. + AtomicMap perc_run_traffic_light; + /// Map containing % of ignoring actors. + AtomicMap perc_ignore_actors; - public: - Parameters(); - ~Parameters(); + public: + Parameters(); + ~Parameters(); - /// Set target velocity specific to a vehicle. - void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); + /// Set target velocity specific to a vehicle. + void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); - /// Set global target velocity. - void SetGlobalPercentageSpeedDifference(float const percentage); + /// Set global target velocity. + void SetGlobalPercentageSpeedDifference(float const percentage); - /// Set collision detection rules between vehicles. - void SetCollisionDetection(const ActorPtr &reference_actor, - const ActorPtr &other_actor, - const bool detect_collision); + /// Set collision detection rules between vehicles. + void SetCollisionDetection( + const ActorPtr &reference_actor, + const ActorPtr &other_actor, + const bool detect_collision); - /// Method to force lane change on a vehicle. - /// Direction flag can be set to true for left and false for right. - void SetForceLaneChange(const ActorPtr &actor, const bool direction); + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + void SetForceLaneChange(const ActorPtr &actor, const bool direction); - /// Enable / disable automatic lane change on a vehicle. - void SetAutoLaneChange(const ActorPtr &actor, const bool enable); + /// Enable / disable automatic lane change on a vehicle. + void SetAutoLaneChange(const ActorPtr &actor, const bool enable); - /// Method to specify how much distance a vehicle should maintain to - /// the leading vehicle. - void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); - /// Method to query target velocity for a vehicle. - float GetVehicleTargetVelocity(const ActorPtr &actor); + /// Method to query target velocity for a vehicle. + float GetVehicleTargetVelocity(const ActorPtr &actor); - /// Method to query collision avoidance rule between a pair of vehicles. - bool GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor); + /// Method to query collision avoidance rule between a pair of vehicles. + bool GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor); - /// Method to query lane change command for a vehicle. - ChangeLaneInfo GetForceLaneChange(const ActorPtr &actor); + /// Method to query lane change command for a vehicle. + ChangeLaneInfo GetForceLaneChange(const ActorPtr &actor); - /// Method to query auto lane change rule for a vehicle. - bool GetAutoLaneChange(const ActorPtr &actor); + /// Method to query auto lane change rule for a vehicle. + bool GetAutoLaneChange(const ActorPtr &actor); - /// Method to query distance to leading vehicle for a given vehicle. - float GetDistanceToLeadingVehicle(const ActorPtr &actor); + /// Method to query distance to leading vehicle for a given vehicle. + float GetDistanceToLeadingVehicle(const ActorPtr &actor); - /// Method to set % to run any traffic light. - void SetPercentageRunningLight(const ActorPtr &actor, const float perc); + /// Method to set % to run any traffic light. + void SetPercentageRunningLight(const ActorPtr &actor, const float perc); - /// Method to set % to ignore any actor. - void SetPercentageIgnoreActors(const ActorPtr &actor, const float perc); + /// Method to set % to ignore any actor. + void SetPercentageIgnoreActors(const ActorPtr &actor, const float perc); - /// Method to get % to run any traffic light. - float GetPercentageRunningLight(const ActorPtr &actor); + /// Method to get % to run any traffic light. + float GetPercentageRunningLight(const ActorPtr &actor); - /// Method to get % to ignore any actor. - float GetPercentageIgnoreActors(const ActorPtr &actor); + /// Method to get % to ignore any actor. + float GetPercentageIgnoreActors(const ActorPtr &actor); - }; -} + }; + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp index 2fab52193..67ddce27c 100644 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp +++ b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp @@ -1,41 +1,39 @@ +// Copyright (c) 2019 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 . + #include "carla/trafficmanager/PerformanceDiagnostics.h" namespace traffic_manager { - PerformanceDiagnostics::PerformanceDiagnostics(std::string stage_name) - : stage_name(stage_name) { + PerformanceDiagnostics::PerformanceDiagnostics(std::string stage_name) + : stage_name(stage_name) { - throughput_clock = chr::system_clock::now(); - throughput_counter = 0; - inter_update_clock = chr::system_clock::now(); - inter_update_duration = chr::duration(0); + throughput_clock = chr::system_clock::now(); + throughput_counter = 0u; + inter_update_clock = chr::system_clock::now(); + inter_update_duration = chr::duration(0); + } + + void PerformanceDiagnostics::RegisterUpdate() { + + const auto current_time = chr::system_clock::now(); + + const chr::duration throughput_count_duration = current_time - throughput_clock; + if (throughput_count_duration.count() > 1.0f) { + + throughput_clock = current_time; + throughput_counter = 0u; + } else { + + ++throughput_counter; + + const chr::duration last_update_duration = current_time - inter_update_clock; + inter_update_duration = (inter_update_duration + last_update_duration) / 2.0f; + inter_update_clock = current_time; } + } - void PerformanceDiagnostics::RegisterUpdate() { - - auto current_time = chr::system_clock::now(); - - chr::duration throughput_count_duration = current_time - throughput_clock; - if (throughput_count_duration.count() > 1.0f) { - - /* std::cout << ( - "Stage name : " + stage_name - + ", throughput : " - + std::to_string(throughput_counter) + " fps" - + ", average update duration : " - + std::to_string(inter_update_duration.count() * 1000.0f) + " ms" - + "\n" - ); */ - - throughput_clock = current_time; - throughput_counter = 0; - } else { - - ++throughput_counter; - - chr::duration last_update_duration = current_time - inter_update_clock; - inter_update_duration = (inter_update_duration + last_update_duration) / 2.0f; - inter_update_clock = current_time; - } - } -} \ No newline at end of file +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h index ccac4e13a..5a9e8401d 100644 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h +++ b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -7,25 +13,26 @@ namespace traffic_manager { -namespace chr = std::chrono; + namespace chr = std::chrono; - class PerformanceDiagnostics { + class PerformanceDiagnostics { - private: - /// Stage name. - std::string stage_name; - /// Throughput clock. - chr::time_point throughput_clock; - /// Throughput counter. - uint throughput_counter; - /// Inter-update clock. - chr::time_point inter_update_clock; - /// Inter-update duration. - chr::duration inter_update_duration; + private: + /// Stage name. + std::string stage_name; + /// Throughput clock. + chr::time_point throughput_clock; + /// Throughput counter. + uint throughput_counter; + /// Inter-update clock. + chr::time_point inter_update_clock; + /// Inter-update duration. + chr::duration inter_update_duration; - public: - PerformanceDiagnostics(std::string name); + public: + PerformanceDiagnostics(std::string name); - void RegisterUpdate(); - }; -} + void RegisterUpdate(); + }; + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp b/LibCarla/source/carla/trafficmanager/PipelineStage.cpp index 2b17d2435..e3814de95 100644 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp +++ b/LibCarla/source/carla/trafficmanager/PipelineStage.cpp @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #include "PipelineStage.h" namespace traffic_manager { @@ -99,4 +105,4 @@ namespace traffic_manager { } } -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.h b/LibCarla/source/carla/trafficmanager/PipelineStage.h index 0a7b990fe..7de1949ef 100644 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.h +++ b/LibCarla/source/carla/trafficmanager/PipelineStage.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -84,4 +90,4 @@ namespace traffic_manager { }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp index 0295c2433..fd770ed7b 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #include "SimpleWaypoint.h" namespace traffic_manager { @@ -48,9 +54,9 @@ namespace traffic_manager { void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr _waypoint) { - cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); - cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); - if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0) { + const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); + const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); + if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f) { next_left_waypoint = _waypoint; } else { throw std::invalid_argument("Argument not on the left side!"); @@ -59,9 +65,9 @@ namespace traffic_manager { void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr _waypoint) { - cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); - cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); - if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0) { + const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); + const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); + if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0.0f) { next_right_waypoint = _waypoint; } else { throw std::invalid_argument("Argument not on the right side!"); @@ -92,4 +98,4 @@ namespace traffic_manager { return (next_waypoints.size() > 1); } -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h index 8f3811a52..04b53ca7b 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -11,8 +17,8 @@ namespace traffic_manager { -namespace cc = carla::client; -namespace cg = carla::geom; + namespace cc = carla::client; + namespace cg = carla::geom; using WaypointPtr = carla::SharedPtr; /// This is a simple wrapper class on Carla's waypoint object. @@ -88,4 +94,4 @@ namespace cg = carla::geom; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index 2f7a8c238..dc4a84d3b 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -1,5 +1,12 @@ +// Copyright (c) 2019 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 . + +#include + #include "TrafficLightStage.h" -#include "iostream" namespace traffic_manager { @@ -36,31 +43,29 @@ namespace traffic_manager { void TrafficLightStage::Action() { // Selecting the output frame based on the selection key. - auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; + const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; // Looping over registered actors. for (uint i = 0u; i < number_of_vehicles; ++i) { bool traffic_light_hazard = false; - LocalizationToTrafficLightData &data = localization_frame->at(i); - Actor ego_actor = data.actor; - ActorId ego_actor_id = ego_actor->GetId(); - SimpleWaypointPtr closest_waypoint = data.closest_waypoint; - SimpleWaypointPtr look_ahead_point = data.junction_look_ahead_waypoint; + const LocalizationToTrafficLightData &data = localization_frame->at(i); + const Actor ego_actor = data.actor; + const ActorId ego_actor_id = ego_actor->GetId(); + const SimpleWaypointPtr closest_waypoint = data.closest_waypoint; + const SimpleWaypointPtr look_ahead_point = data.junction_look_ahead_waypoint; - JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId(); - TimeInstance current_time = chr::system_clock::now(); + const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId(); + const TimeInstance current_time = chr::system_clock::now(); - auto ego_vehicle = boost::static_pointer_cast(ego_actor); + const auto ego_vehicle = boost::static_pointer_cast(ego_actor); TLS traffic_light_state = ego_vehicle->GetTrafficLightState(); // Generate number between 0 and 100 - int r = rand() % 101; + const int r = rand() % 101; // Set to green if random number is lower than percentage, default is 0 if (parameters.GetPercentageRunningLight(boost::shared_ptr(ego_actor)) > r) traffic_light_state = TLS::Green; - //DrawLight(traffic_light_state, ego_actor); - // We determine to stop if the current position of the vehicle is not a // junction, // a point on the path beyond a threshold (velocity-dependent) distance @@ -96,8 +101,8 @@ namespace traffic_manager { need_to_issue_new_ticket = true; } else { - TimeInstance &previous_ticket = vehicle_last_ticket.at(ego_actor_id); - chr::duration diff = current_time - previous_ticket; + const TimeInstance &previous_ticket = vehicle_last_ticket.at(ego_actor_id); + const chr::duration diff = current_time - previous_ticket; if (diff.count() > NO_SIGNAL_PASSTHROUGH_INTERVAL) { need_to_issue_new_ticket = true; } @@ -110,8 +115,8 @@ namespace traffic_manager { if (junction_last_ticket.find(junction_id) != junction_last_ticket.end()) { TimeInstance &last_ticket = junction_last_ticket.at(junction_id); - chr::duration diff = current_time - last_ticket; - if (diff.count() > 0) { + const chr::duration diff = current_time - last_ticket; + if (diff.count() > 0.0) { last_ticket = current_time + chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL); } else { last_ticket += chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL); @@ -131,9 +136,9 @@ namespace traffic_manager { } // If current time is behind ticket time, then do not enter junction. - TimeInstance ¤t_ticket = vehicle_last_ticket.at(ego_actor_id); - chr::duration diff = current_ticket - current_time; - if (diff.count() > 0) { + const TimeInstance ¤t_ticket = vehicle_last_ticket.at(ego_actor_id); + const chr::duration diff = current_ticket - current_time; + if (diff.count() > 0.0) { traffic_light_hazard = true; } } @@ -145,7 +150,7 @@ namespace traffic_manager { } void TrafficLightStage::DataReceiver() { - auto packet = localization_messenger->ReceiveData(localization_messenger_state); + const auto packet = localization_messenger->ReceiveData(localization_messenger_state); localization_frame = packet.data; localization_messenger_state = packet.id; @@ -162,10 +167,10 @@ namespace traffic_manager { void TrafficLightStage::DataSender() { - DataPacket> packet{ - planner_messenger_state, - frame_selector ? planner_frame_a : planner_frame_b - }; + const DataPacket> packet{ + planner_messenger_state, + frame_selector ? planner_frame_a : planner_frame_b + }; frame_selector = !frame_selector; planner_messenger_state = planner_messenger->SendData(packet); } @@ -175,28 +180,29 @@ namespace traffic_manager { if (traffic_light_state == TLS::Green) { str="Green"; debug_helper.DrawString( - cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1), - str, - false, - {0u, 255u, 0u}, 0.1f, true); + cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f), + str, + false, + {0u, 255u, 0u}, 0.1f, true); } else if (traffic_light_state == TLS::Yellow) { str="Yellow"; debug_helper.DrawString( - cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1), - str, - false, - {255u, 255u, 0u}, 0.1f, true); + cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f), + str, + false, + {255u, 255u, 0u}, 0.1f, true); } else { str="Red"; debug_helper.DrawString( - cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1), - str, - false, - {255u, 0u, 0u}, 0.1f, true); + cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f), + str, + false, + {255u, 0u, 0u}, 0.1f, true); } } -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h index afb58724c..0eba334db 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -5,23 +11,23 @@ #include #include -#include "carla/client/Vehicle.h" -#include "carla/client/TrafficLight.h" -#include "carla/client/World.h" #include "carla/client/ActorList.h" +#include "carla/client/TrafficLight.h" +#include "carla/client/Vehicle.h" +#include "carla/client/World.h" #include "carla/Memory.h" #include "carla/rpc/TrafficLightState.h" #include "carla/trafficmanager/MessengerAndDataTypes.h" -#include "carla/trafficmanager/PipelineStage.h" #include "carla/trafficmanager/Parameters.h" +#include "carla/trafficmanager/PipelineStage.h" namespace traffic_manager { -namespace chr = std::chrono; -namespace cc = carla::client; -namespace cg = carla::geom; + namespace chr = std::chrono; + namespace cc = carla::client; + namespace cg = carla::geom; using ActorId = carla::ActorId; using Actor = carla::SharedPtr; @@ -71,11 +77,11 @@ namespace cg = carla::geom; public: TrafficLightStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr planner_messenger, - Parameters ¶meters, - cc::DebugHelper &debug_helper); + std::string stage_name, + std::shared_ptr localization_messenger, + std::shared_ptr planner_messenger, + Parameters ¶meters, + cc::DebugHelper &debug_helper); ~TrafficLightStage(); void DataReceiver() override; @@ -86,4 +92,4 @@ namespace cg = carla::geom; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp index bc4bdc8ae..8e3708ee5 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp @@ -1,4 +1,11 @@ +// Copyright (c) 2019 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 . + #include "TrafficManager.h" + #include "carla/client/TrafficLight.h" namespace traffic_manager { @@ -19,10 +26,10 @@ namespace traffic_manager { debug_helper(client_connection.GetWorld().MakeDebugHelper()) { using WorldMap = carla::SharedPtr; - WorldMap world_map = world.GetMap(); - auto dao = CarlaDataAccessLayer(world_map); + const WorldMap world_map = world.GetMap(); + const auto dao = CarlaDataAccessLayer(world_map); using Topology = std::vector>; - Topology topology = dao.GetTopology(); + const Topology topology = dao.GetTopology(); local_map = std::make_shared(topology); local_map->SetUp(0.1f); @@ -36,38 +43,38 @@ namespace traffic_manager { planner_control_messenger = std::make_shared(); localization_stage = std::make_unique( - "Localization stage", - localization_planner_messenger, localization_collision_messenger, - localization_traffic_light_messenger, - registered_actors, *local_map.get(), - parameters, debug_helper); + "Localization stage", + localization_planner_messenger, localization_collision_messenger, + localization_traffic_light_messenger, + registered_actors, *local_map.get(), + parameters, debug_helper); collision_stage = std::make_unique( - "Collision stage", - localization_collision_messenger, collision_planner_messenger, - world, parameters, debug_helper); + "Collision stage", + localization_collision_messenger, collision_planner_messenger, + world, parameters, debug_helper); traffic_light_stage = std::make_unique( - "Traffic light stage", - localization_traffic_light_messenger, traffic_light_planner_messenger, - parameters, debug_helper); + "Traffic light stage", + localization_traffic_light_messenger, traffic_light_planner_messenger, + parameters, debug_helper); planner_stage = std::make_unique( - "Motion planner stage", - localization_planner_messenger, - collision_planner_messenger, - traffic_light_planner_messenger, - planner_control_messenger, - parameters, - longitudinal_PID_parameters, - longitudinal_highway_PID_parameters, - lateral_PID_parameters, - lateral_highway_PID_parameters, - debug_helper); + "Motion planner stage", + localization_planner_messenger, + collision_planner_messenger, + traffic_light_planner_messenger, + planner_control_messenger, + parameters, + longitudinal_PID_parameters, + longitudinal_highway_PID_parameters, + lateral_PID_parameters, + lateral_highway_PID_parameters, + debug_helper); control_stage = std::make_unique( - "Batch control stage", - planner_control_messenger, client_connection); + "Batch control stage", + planner_control_messenger, client_connection); Start(); } @@ -79,15 +86,14 @@ namespace traffic_manager { std::unique_ptr TrafficManager::singleton_pointer = nullptr; TrafficManager& TrafficManager::GetInstance(cc::Client &client_connection) { - // std::lock_guard lock(singleton_mutex); if (singleton_pointer == nullptr) { - std::vector longitudinal_param = {2.0f, 0.15f, 0.01f}; - std::vector longitudinal_highway_param = {4.0f, 0.15f, 0.01f}; - std::vector lateral_param = {10.0f, 0.0f, 0.1f}; - std::vector lateral_highway_param = {6.0f, 0.0f, 0.3f}; - float perc_difference_from_limit = 30.0f; + const std::vector longitudinal_param = {2.0f, 0.15f, 0.01f}; + const std::vector longitudinal_highway_param = {4.0f, 0.15f, 0.01f}; + const std::vector lateral_param = {10.0f, 0.0f, 0.1f}; + const std::vector lateral_highway_param = {6.0f, 0.0f, 0.3f}; + const float perc_difference_from_limit = 30.0f; TrafficManager* tm_ptr = new TrafficManager( longitudinal_param, longitudinal_highway_param, lateral_param, lateral_highway_param, @@ -157,11 +163,11 @@ namespace traffic_manager { } void TrafficManager::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { - parameters.SetPercentageSpeedDifference(actor, percentage); + parameters.SetPercentageSpeedDifference(actor, percentage); } void TrafficManager::SetGlobalPercentageSpeedDifference(const float percentage) { - parameters.SetGlobalPercentageSpeedDifference(percentage); + parameters.SetGlobalPercentageSpeedDifference(percentage); } void TrafficManager::SetCollisionDetection( @@ -208,35 +214,37 @@ namespace traffic_manager { } void TrafficManager::ResetAllTrafficLights() { - auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*"); + const auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*"); std::vector list_of_all_groups; TLGroup tl_to_freeze; std::vector list_of_ids; for (auto tl : *world_traffic_lights.get()) { if (!(std::find(list_of_ids.begin(), list_of_ids.end(), tl->GetId()) != list_of_ids.end())) { - TLGroup tl_group = boost::static_pointer_cast(tl)->GetGroupTrafficLights(); - list_of_all_groups.push_back(tl_group); - for (unsigned i=0; iGetId()); - if(i!=0) { - tl_to_freeze.push_back(tl_group.at(i)); - } - } + const TLGroup tl_group = boost::static_pointer_cast(tl)->GetGroupTrafficLights(); + list_of_all_groups.push_back(tl_group); + for (uint i=0u; iGetId()); + if(i!=0u) { + tl_to_freeze.push_back(tl_group.at(i)); + } } } + } for (TLGroup& tl_group : list_of_all_groups) { tl_group.front()->SetState(TLS::Green); - std::for_each(tl_group.begin()+1, tl_group.end(), - [] (auto& tl) {tl->SetState(TLS::Red);}); + std::for_each( + tl_group.begin()+1, tl_group.end(), + [] (auto& tl) {tl->SetState(TLS::Red);}); } while (!CheckAllFrozen(tl_to_freeze)) { for (auto& tln : tl_to_freeze) { - tln->SetState(TLS::Red); - tln->Freeze(true); + tln->SetState(TLS::Red); + tln->Freeze(true); } } } -} + +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.h b/LibCarla/source/carla/trafficmanager/TrafficManager.h index ca90484f4..ee08a8efa 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -27,7 +33,7 @@ namespace traffic_manager { -namespace cc = carla::client; + namespace cc = carla::client; using ActorPtr = carla::SharedPtr; using TLS = carla::rpc::TrafficLightState; @@ -113,9 +119,10 @@ namespace cc = carla::client; void SetGlobalPercentageSpeedDifference(float const percentage); /// Set collision detection rules between vehicles. - void SetCollisionDetection(const ActorPtr &reference_actor, - const ActorPtr &other_actor, - const bool detect_collision); + void SetCollisionDetection( + const ActorPtr &reference_actor, + const ActorPtr &other_actor, + const bool detect_collision); /// Method to force lane change on a vehicle. /// Direction flag can be set to true for left and false for right. @@ -145,4 +152,4 @@ namespace cc = carla::client; }; -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/VicinityGrid.cpp b/LibCarla/source/carla/trafficmanager/VicinityGrid.cpp index 81e0d2f47..5d4670a75 100644 --- a/LibCarla/source/carla/trafficmanager/VicinityGrid.cpp +++ b/LibCarla/source/carla/trafficmanager/VicinityGrid.cpp @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #include "VicinityGrid.h" namespace traffic_manager { @@ -14,17 +20,17 @@ namespace traffic_manager { std::pair VicinityGrid::UpdateGrid(Actor actor) { - ActorId actor_id = actor->GetId(); - cg::Location location = actor->GetLocation(); - int first = static_cast(std::floor(location.x / GRID_SIZE)); - int second = static_cast(std::floor(location.y / GRID_SIZE)); + const ActorId actor_id = actor->GetId(); + const cg::Location location = actor->GetLocation(); + const int first = static_cast(std::floor(location.x / GRID_SIZE)); + const int second = static_cast(std::floor(location.y / GRID_SIZE)); - std::string new_grid_id = MakeKey({first, second}); + const std::string new_grid_id = MakeKey({first, second}); // If the actor exists in the grid. if (actor_to_grid_id.find(actor_id) != actor_to_grid_id.end()) { - std::string old_grid_id = actor_to_grid_id.at(actor_id); + const std::string old_grid_id = actor_to_grid_id.at(actor_id); // If the actor has moved into a new road/section/lane. if (old_grid_id != new_grid_id) { @@ -60,7 +66,7 @@ namespace traffic_manager { std::unordered_set VicinityGrid::GetActors(Actor actor) { - std::pair grid_ids = UpdateGrid(actor); + const std::pair grid_ids = UpdateGrid(actor); std::shared_lock lock(modification_mutex); std::unordered_set actors; @@ -69,9 +75,9 @@ namespace traffic_manager { for (int i = -1; i <= 1; ++i) { for (int j = -1; j <= 1; ++j) { - std::string grid_key = MakeKey({grid_ids.first + i, grid_ids.second + j}); + const std::string grid_key = MakeKey({grid_ids.first + i, grid_ids.second + j}); if (grid_to_actor_id.find(grid_key) != grid_to_actor_id.end()) { - auto &grid_actor_set = grid_to_actor_id.at(grid_key); + const auto &grid_actor_set = grid_to_actor_id.at(grid_key); actors.insert(grid_actor_set.begin(), grid_actor_set.end()); } } @@ -81,9 +87,9 @@ namespace traffic_manager { } void VicinityGrid::EraseActor(ActorId actor_id) { - std::string grid_key = actor_to_grid_id.at(actor_id); + const std::string grid_key = actor_to_grid_id.at(actor_id); actor_to_grid_id.erase(actor_id); grid_to_actor_id.at(grid_key).erase(actor_id); } -} +} // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/VicinityGrid.h b/LibCarla/source/carla/trafficmanager/VicinityGrid.h index 8366d9b8d..1ca67235f 100644 --- a/LibCarla/source/carla/trafficmanager/VicinityGrid.h +++ b/LibCarla/source/carla/trafficmanager/VicinityGrid.h @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #pragma once #include @@ -56,4 +62,4 @@ namespace traffic_manager { }; -} +} // namespace traffic_manager diff --git a/PythonAPI/carla/source/libcarla/TrafficManager.cpp b/PythonAPI/carla/source/libcarla/TrafficManager.cpp index 6c66ada88..fa401349e 100644 --- a/PythonAPI/carla/source/libcarla/TrafficManager.cpp +++ b/PythonAPI/carla/source/libcarla/TrafficManager.cpp @@ -1,3 +1,9 @@ +// Copyright (c) 2019 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 . + #include #include diff --git a/PythonAPI/carla/source/libcarla/libcarla.cpp b/PythonAPI/carla/source/libcarla/libcarla.cpp index 2eff73406..832a802ec 100644 --- a/PythonAPI/carla/source/libcarla/libcarla.cpp +++ b/PythonAPI/carla/source/libcarla/libcarla.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license.