From b9fbbf7fd03ee2a4f3a7baf4343a28381cfe81ef Mon Sep 17 00:00:00 2001 From: bernat Date: Sat, 21 Dec 2019 04:59:22 +0100 Subject: [PATCH] Release candiadate (#2310) --- LibCarla/source/carla/client/Vehicle.cpp | 4 +- .../source/carla/client/detail/Client.cpp | 2 +- .../trafficmanager/BatchControlStage.cpp | 14 +- .../carla/trafficmanager/BatchControlStage.h | 2 - .../trafficmanager/CarlaDataAccessLayer.cpp | 21 -- .../trafficmanager/CarlaDataAccessLayer.h | 39 --- .../carla/trafficmanager/CollisionStage.cpp | 254 +++++--------- .../carla/trafficmanager/CollisionStage.h | 34 +- .../carla/trafficmanager/InMemoryMap.cpp | 238 ++++++++----- .../source/carla/trafficmanager/InMemoryMap.h | 23 +- .../trafficmanager/LocalizationStage.cpp | 328 ++++++++---------- .../carla/trafficmanager/LocalizationStage.h | 22 +- .../trafficmanager/LocalizationUtils.cpp | 140 +++++++- .../carla/trafficmanager/LocalizationUtils.h | 21 +- .../source/carla/trafficmanager/Messenger.h | 80 ++--- .../trafficmanager/MessengerAndDataTypes.h | 13 +- .../trafficmanager/MotionPlannerStage.cpp | 70 ++-- .../carla/trafficmanager/MotionPlannerStage.h | 5 - .../carla/trafficmanager/PIDController.cpp | 7 +- .../carla/trafficmanager/Parameters.cpp | 30 +- .../source/carla/trafficmanager/Parameters.h | 1 + .../trafficmanager/PerformanceDiagnostics.cpp | 23 +- .../trafficmanager/PerformanceDiagnostics.h | 55 ++- .../carla/trafficmanager/PipelineStage.cpp | 93 +---- .../carla/trafficmanager/PipelineStage.h | 33 +- .../carla/trafficmanager/SimpleWaypoint.cpp | 31 +- .../carla/trafficmanager/SimpleWaypoint.h | 19 + .../trafficmanager/TrafficLightStage.cpp | 28 +- .../carla/trafficmanager/TrafficLightStage.h | 3 - .../carla/trafficmanager/TrafficManager.cpp | 27 +- .../carla/trafficmanager/TrafficManager.h | 8 +- .../carla/trafficmanager/VicinityGrid.cpp | 97 ------ .../carla/trafficmanager/VicinityGrid.h | 67 ---- PythonAPI/carla/source/libcarla/Client.cpp | 71 +++- .../carla/source/libcarla/TrafficManager.cpp | 23 +- PythonAPI/examples/tm_spawn_npc.py | 4 - .../Source/Carla/Traffic/TrafficLightBase.cpp | 13 + .../Source/Carla/Traffic/TrafficLightBase.h | 3 + .../Vehicle/WheeledVehicleAIController.cpp | 318 +---------------- .../Vehicle/WheeledVehicleAIController.h | 22 -- Util/ContentVersions.txt | 2 +- 41 files changed, 944 insertions(+), 1344 deletions(-) delete mode 100644 LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp delete mode 100644 LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h delete mode 100644 LibCarla/source/carla/trafficmanager/VicinityGrid.cpp delete mode 100644 LibCarla/source/carla/trafficmanager/VicinityGrid.h diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 980a9f03a9..556002f76a 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/LibCarla/source/carla/client/Vehicle.cpp @@ -34,9 +34,11 @@ namespace client { _is_control_sticky(GetControlIsSticky(GetAttributes())) {} void Vehicle::SetAutopilot(bool enabled) { + TM &tm = TM::GetInstance(TM::GetUniqueLocalClient()); if (enabled) { - TM &tm = TM::GetInstance(TM::GetUniqueLocalClient()); tm.RegisterVehicles({shared_from_this()}); + } else { + tm.UnregisterVehicles({shared_from_this()}); } } diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 436f1319ae..ff0c70d019 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/LibCarla/source/carla/client/detail/Client.cpp @@ -47,7 +47,7 @@ namespace detail { : endpoint(host + ":" + std::to_string(port)), rpc_client(host, port), streaming_client(host) { - rpc_client.set_timeout(1000u); + rpc_client.set_timeout(5000u); streaming_client.AsyncRun( worker_threads > 0u ? worker_threads : std::thread::hardware_concurrency()); } diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp index e67a462f7a..1af6f29cae 100644 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp +++ b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp @@ -17,8 +17,6 @@ namespace traffic_manager { messenger(messenger), carla_client(carla_client) { - // Initializing messenger state. - messenger_state = messenger->GetState(); // Initializing number of vehicles to zero in the beginning. number_of_vehicles = 0u; } @@ -28,7 +26,7 @@ namespace traffic_manager { void BatchControlStage::Action() { // Looping over registered actors. - for (uint64_t i = 0u; i < number_of_vehicles; ++i) { + for (uint64_t i = 0u; i < number_of_vehicles && data_frame != nullptr; ++i) { cr::VehicleControl vehicle_control; @@ -44,15 +42,13 @@ namespace traffic_manager { void BatchControlStage::DataReceiver() { - auto packet = messenger->ReceiveData(messenger_state); - data_frame = packet.data; - messenger_state = packet.id; + data_frame = messenger->Peek(); // Allocating new containers for the changed number of registered vehicles. if (data_frame != nullptr && number_of_vehicles != (*data_frame.get()).size()) { - number_of_vehicles = static_cast((*data_frame.get()).size()); + number_of_vehicles = static_cast((*data_frame.get()).size()); // Allocating array for command batching. commands = std::make_shared>(number_of_vehicles); } @@ -61,12 +57,16 @@ namespace traffic_manager { void BatchControlStage::DataSender() { + messenger->Pop(); + if (commands != nullptr) { carla_client.ApplyBatch(*commands.get()); + } // 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 5d45a8445a..29acfbac5f 100644 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.h +++ b/LibCarla/source/carla/trafficmanager/BatchControlStage.h @@ -30,8 +30,6 @@ namespace traffic_manager { private: - /// Variable to remember messenger state. - int messenger_state; /// Pointer to frame received from MotionPlanner. std::shared_ptr data_frame; /// Pointer to a messenger from MotionPlanner. diff --git a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp b/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp deleted file mode 100644 index a37b6221e1..0000000000 --- a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp +++ /dev/null @@ -1,21 +0,0 @@ -// 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 carla { -namespace traffic_manager { - - CarlaDataAccessLayer::CarlaDataAccessLayer(carla::SharedPtr _world_map) { - world_map = _world_map; - } - - using WaypointPtr = carla::SharedPtr; - std::vector> CarlaDataAccessLayer::GetTopology() const { - return world_map->GetTopology(); - } -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h b/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h deleted file mode 100644 index 0a8d7aebfc..0000000000 --- a/LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h +++ /dev/null @@ -1,39 +0,0 @@ -// 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 "carla/client/Map.h" -#include "carla/client/Waypoint.h" -#include "carla/Memory.h" - -namespace carla { -namespace traffic_manager { - - namespace cc = carla::client; - - /// This class is responsible for retrieving data from the server. - class CarlaDataAccessLayer { - - private: - - /// Pointer to Carla's map object. - carla::SharedPtr world_map; - - public: - - CarlaDataAccessLayer(carla::SharedPtr world_map); - - /// This method retrieves a list of topology segments from the simulator. - using WaypointPtr = carla::SharedPtr; - std::vector> GetTopology() const; - - }; - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 87b455f92a..223d4019ef 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -12,7 +12,7 @@ namespace traffic_manager { namespace CollisionStageConstants { static const float VERTICAL_OVERLAP_THRESHOLD = 2.0f; - static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f; + static const float BOUNDARY_EXTENSION_MINIMUM = 1.0f; static const float EXTENSION_SQUARE_POINT = 7.5f; static const float TIME_HORIZON = 0.5f; static const float HIGHWAY_SPEED = 50.0f / 3.6f; @@ -20,7 +20,8 @@ namespace CollisionStageConstants { static const float CRAWL_SPEED = 10.0f / 3.6f; static const float BOUNDARY_EDGE_LENGTH = 2.0f; static const float MAX_COLLISION_RADIUS = 100.0f; - + static const float MIN_COLLISION_RADIUS = 15.0f; + static const float WALKER_TIME_EXTENSION = 1.5f; } // namespace CollisionStageConstants using namespace CollisionStageConstants; @@ -29,13 +30,11 @@ namespace CollisionStageConstants { std::string stage_name, std::shared_ptr localization_messenger, std::shared_ptr planner_messenger, - cc::World &world, Parameters ¶meters, cc::DebugHelper &debug_helper) : PipelineStage(stage_name), localization_messenger(localization_messenger), planner_messenger(planner_messenger), - world(world), parameters(parameters), debug_helper(debug_helper){ @@ -43,11 +42,6 @@ namespace CollisionStageConstants { last_world_actors_pass_instance = chr::system_clock::now(); // Initializing output array selector. frame_selector = true; - // Initializing messenger states. - localization_messenger_state = localization_messenger->GetState(); - // Initializing this messenger to preemptively write since it precedes - // motion planner stage. - planner_messenger_state = planner_messenger->GetState() - 1; // Initializing the number of vehicles to zero in the beginning. number_of_vehicles = 0u; } @@ -57,57 +51,18 @@ namespace CollisionStageConstants { void CollisionStage::Action() { const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; - // Handle vehicles not spawned by TrafficManager. - 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) { - - 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()) { - 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}); - } - } - // Scanning for pedestrians. - for (auto walker: *world_walker.get()) { - const auto unregistered_id = walker->GetId(); - if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { - unregistered_actors.insert({unregistered_id, walker}); - } - } - // Regularly update unregistered actors. - std::vector actor_ids_to_erase; - for (auto actor_info: unregistered_actors) { - if (actor_info.second->IsAlive()) { - vicinity_grid.UpdateGrid(actor_info.second); - } else { - vicinity_grid.EraseActor(actor_info.first); - actor_ids_to_erase.push_back(actor_info.first); - } - } - for (auto actor_id: actor_ids_to_erase) { - unregistered_actors.erase(actor_id); - } - - last_world_actors_pass_instance = current_time; - } - // Looping over registered actors. - for (uint64_t i = 0u; i < number_of_vehicles; ++i) { + for (uint64_t i = 0u; i < number_of_vehicles && localization_frame != nullptr; ++i) { - const LocalizationToCollisionData &data = localization_frame->at(i); + LocalizationToCollisionData &data = localization_frame->at(i); const Actor ego_actor = data.actor; const ActorId ego_actor_id = ego_actor->GetId(); + const std::unordered_map overlapping_actors = data.overlapping_actors; + const cg::Location ego_location = ego_actor->GetLocation(); + const SimpleWaypointPtr& closest_point = data.closest_waypoint; + const SimpleWaypointPtr& junction_look_ahead = data.junction_look_ahead_waypoint; // Retrieve actors around the path of the ego vehicle. - std::unordered_set actor_id_list = GetPotentialVehicleObstacles(ego_actor); - bool collision_hazard = false; // Generate number between 0 and 100 @@ -115,35 +70,31 @@ namespace CollisionStageConstants { // 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) { - const ActorId actor_id = *j; - try { - Actor actor = nullptr; - if (vehicle_id_to_index.find(actor_id) != vehicle_id_to_index.end()) { - actor = localization_frame->at(vehicle_id_to_index.at(actor_id)).actor; - } else if (unregistered_actors.find(actor_id) != unregistered_actors.end()) { - actor = unregistered_actors.at(actor_id); - } + // Check every actor in the vicinity if it poses a collision hazard. + for (auto j = overlapping_actors.begin(); (j != overlapping_actors.end()) && !collision_hazard; ++j) { + const Actor actor = j->second; + const ActorId actor_id = j->first; + const 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)) { - - if (parameters.GetCollisionDetection(ego_actor, actor) && - NegotiateCollision(ego_actor, actor)) { - - collision_hazard = true; + try { + // Collision checks increase with speed (Official formula used) + float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f); + collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS); + // Temporary fix to (0,0,0) bug + if (other_location.x != 0 && other_location.y != 0 && other_location.z != 0){ + 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)) { + + if (parameters.GetCollisionDetection(ego_actor, actor) && + NegotiateCollision(ego_actor, actor, closest_point, junction_look_ahead)) { + collision_hazard = true; + } + } } - } - } catch (const std::exception &e) { - carla::log_warning("Encountered problem while determining collision \n"); carla::log_info("Actor might not be alive \n"); } @@ -152,14 +103,11 @@ namespace CollisionStageConstants { CollisionToPlannerData &message = current_planner_frame->at(i); message.hazard = collision_hazard; - } } void CollisionStage::DataReceiver() { - const auto packet = localization_messenger->ReceiveData(localization_messenger_state); - localization_frame = packet.data; - localization_messenger_state = packet.id; + localization_frame = localization_messenger->Peek(); if (localization_frame != nullptr) { // Connecting actor ids to their position indices on data arrays. @@ -175,7 +123,7 @@ namespace CollisionStageConstants { // vehicles. if (number_of_vehicles != (*localization_frame.get()).size()) { - number_of_vehicles = static_cast((*localization_frame.get()).size()); + number_of_vehicles = static_cast((*localization_frame.get()).size()); // Allocating output arrays to be shared with motion planner stage. planner_frame_a = std::make_shared(number_of_vehicles); planner_frame_b = std::make_shared(number_of_vehicles); @@ -185,24 +133,18 @@ namespace CollisionStageConstants { void CollisionStage::DataSender() { - const DataPacket> packet{ - planner_messenger_state, - frame_selector ? planner_frame_a : planner_frame_b - }; + localization_messenger->Pop(); + + planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b); frame_selector = !frame_selector; - planner_messenger_state = planner_messenger->SendData(packet); } - bool CollisionStage::NegotiateCollision(const Actor &reference_vehicle, const Actor &other_vehicle) const { + bool CollisionStage::NegotiateCollision(const Actor &reference_vehicle, const Actor &other_vehicle, + const SimpleWaypointPtr& closest_point, + const SimpleWaypointPtr& junction_look_ahead) { bool hazard = false; - auto& data_packet = localization_frame->at(vehicle_id_to_index.at(reference_vehicle->GetId())); - Buffer& waypoint_buffer = data_packet.buffer; - - auto& other_packet = localization_frame->at(vehicle_id_to_index.at(other_vehicle->GetId())); - Buffer& other_buffer = other_packet.buffer; - const cg::Location reference_location = reference_vehicle->GetLocation(); const cg::Location other_location = other_vehicle->GetLocation(); @@ -210,11 +152,32 @@ namespace CollisionStageConstants { cg::Vector3D reference_to_other = other_location - reference_location; reference_to_other = reference_to_other.MakeUnitVector(); + 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(); + + const auto &waypoint_buffer = localization_frame->at( + vehicle_id_to_index.at(reference_vehicle->GetId())).buffer; + const SimpleWaypointPtr& reference_front_wp = waypoint_buffer.front(); + 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()) { + float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x * 1.414f; + float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * 1.414f; + float inter_vehicle_length = reference_vehicle_length + other_vehicle_length; + + if (!(!reference_front_wp->CheckJunction() && + cg::Math::Dot(reference_heading, reference_to_other) < 0) && + + !(!closest_point->CheckJunction() && junction_look_ahead->CheckJunction() && + reference_vehicle_ptr->GetVelocity().SquaredLength() < 0.1 && + reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green) && + + !(!reference_front_wp->CheckJunction() && + cg::Math::Dot(reference_heading, reference_to_other) > 0 && + (cg::Math::DistanceSquared(reference_location, other_location) > + std::pow(GetBoundingBoxExtention(reference_vehicle) + inter_vehicle_length, 2)))) { const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle)); const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle)); @@ -227,44 +190,25 @@ namespace CollisionStageConstants { const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon); const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); - 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 && + ((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()) { - - 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; - - const float bbox_extension_length = GetBoundingBoxExtention(reference_vehicle); - - 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))) { + (cg::Math::Dot(reference_heading, reference_to_other) > + cg::Math::Dot(other_heading, other_to_reference)) + )) + ) { hazard = true; } } - return hazard; } - traffic_manager::Polygon CollisionStage::GetPolygon(const LocationList &boundary) const { + traffic_manager::Polygon CollisionStage::GetPolygon(const LocationList &boundary) { std::string boundary_polygon_wkt; for (const cg::Location &location: boundary) { @@ -278,7 +222,7 @@ namespace CollisionStageConstants { return boundary_polygon; } - LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor) const { + LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor) { const LocationList bbox = GetBoundary(actor); @@ -299,7 +243,7 @@ namespace CollisionStageConstants { LocationList right_boundary; const auto vehicle = boost::static_pointer_cast(actor); const float width = vehicle->GetBoundingBox().extent.y; - const float length = vehicle->GetBoundingBox().extent.x; + const float length = vehicle->GetBoundingBox().extent.x*2; SimpleWaypointPtr boundary_start = waypoint_buffer.front(); uint64_t boundary_start_index = 0u; @@ -357,9 +301,10 @@ namespace CollisionStageConstants { return bbox; } + } - float CollisionStage::GetBoundingBoxExtention(const Actor &actor) const { + float CollisionStage::GetBoundingBoxExtention(const Actor &actor) { const float velocity = actor->GetVelocity().Length(); float bbox_extension = BOUNDARY_EXTENSION_MINIMUM; @@ -377,62 +322,45 @@ namespace CollisionStageConstants { return bbox_extension; } - std::unordered_set CollisionStage::GetPotentialVehicleObstacles(const Actor &ego_vehicle) { - - vicinity_grid.UpdateGrid(ego_vehicle); + LocationList CollisionStage::GetBoundary(const Actor &actor) { - 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) { - actor_id_list = vicinity_grid.GetActors(ego_vehicle); - } else { - actor_id_list = data_packet.overlapping_actors; - } - - return actor_id_list; - } - - LocationList CollisionStage::GetBoundary(const Actor &actor) const { const auto actor_type = actor->GetTypeId(); + cg::Location location = actor->GetLocation(); + cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); + heading_vector.z = 0.0f; + heading_vector = heading_vector.MakeUnitVector(); cg::BoundingBox bbox; - cg::Location location; - cg::Vector3D heading_vector; - + float forward_extension = 0.0f; if (actor_type[0] == 'v') { - 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') { - const auto walker = boost::static_pointer_cast(actor); bbox = walker->GetBoundingBox(); - location = walker->GetLocation(); - heading_vector = walker->GetTransform().GetForwardVector(); + // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions. + forward_extension = walker->GetVelocity().Length() * WALKER_TIME_EXTENSION; } 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); + const cg::Vector3D x_boundary_vector = heading_vector * (extent.x + forward_extension); + const cg::Vector3D y_boundary_vector = perpendicular_vector * (extent.y + forward_extension); + // Four corners of the vehicle in top view clockwise order (left-handed // system). - 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.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), + LocationList bbox_boundary = { + 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), }; + + return bbox_boundary; } - void CollisionStage::DrawBoundary(const LocationList &boundary) const { + void CollisionStage::DrawBoundary(const LocationList &boundary) { for (uint64_t i = 0u; i < boundary.size(); ++i) { debug_helper.DrawLine( boundary[i] + cg::Location(0.0f, 0.0f, 1.0f), diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index 1ca968eed1..488652fbc5 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -31,8 +31,8 @@ #include "carla/trafficmanager/MessengerAndDataTypes.h" #include "carla/trafficmanager/Parameters.h" +#include "carla/trafficmanager/PerformanceDiagnostics.h" #include "carla/trafficmanager/PipelineStage.h" -#include "carla/trafficmanager/VicinityGrid.h" namespace carla { namespace traffic_manager { @@ -56,9 +56,6 @@ namespace traffic_manager { private: - /// Variables to remember messenger states. - int localization_messenger_state; - int planner_messenger_state; /// Selection key for switching between output frames. bool frame_selector; /// Pointer to data received from localization stage. @@ -69,47 +66,41 @@ namespace traffic_manager { /// Pointers to messenger objects. std::shared_ptr localization_messenger; std::shared_ptr planner_messenger; - /// Reference to Carla's world object. - cc::World &world; /// Runtime parameterization object. Parameters ¶meters; /// Reference to Carla's debug helper object. cc::DebugHelper &debug_helper; - /// An object used for grid binning vehicles for faster proximity detection. - VicinityGrid vicinity_grid; /// The map used to connect actor ids to the array index of data frames. - std::unordered_map vehicle_id_to_index; - /// A structure used to keep track of actors spawned outside of traffic - /// manager. - std::unordered_map unregistered_actors; + std::unordered_map vehicle_id_to_index; /// An object used to keep track of time between checking for all world /// actors. chr::time_point last_world_actors_pass_instance; /// Number of vehicles registered with the traffic manager. uint64_t number_of_vehicles; + /// Snippet profiler for measuring execution time. + SnippetProfiler snippet_profiler; /// Returns the bounding box corners of the vehicle passed to the method. - LocationList GetBoundary(const Actor &actor) const; + LocationList GetBoundary(const Actor &actor); /// Returns the extrapolated bounding box of the vehicle along its /// trajectory. - LocationList GetGeodesicBoundary(const Actor &actor) const; + LocationList GetGeodesicBoundary(const Actor &actor); /// Method to construct a boost polygon object. - Polygon GetPolygon(const LocationList &boundary) const; + Polygon GetPolygon(const LocationList &boundary); /// The method returns true if ego_vehicle should stop and wait for /// other_vehicle to pass. - bool NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle) const; + bool NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle, + const SimpleWaypointPtr& closest_point, + const SimpleWaypointPtr& junction_look_ahead); /// Method to calculate the speed dependent bounding box extention for a vehicle. - float GetBoundingBoxExtention(const Actor &ego_vehicle) const; - - /// Method to retreive the set of vehicles around the path of the given vehicle. - std::unordered_set GetPotentialVehicleObstacles(const Actor &ego_vehicle); + float GetBoundingBoxExtention(const Actor &ego_vehicle); /// A simple method used to draw bounding boxes around vehicles - void DrawBoundary(const LocationList &boundary) const; + void DrawBoundary(const LocationList &boundary); public: @@ -117,7 +108,6 @@ namespace traffic_manager { std::string stage_name, std::shared_ptr localization_messenger, std::shared_ptr planner_messenger, - cc::World &world, Parameters ¶meters, cc::DebugHelper &debug_helper); diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index 214fc2a70b..f2a9a35b69 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -11,24 +11,21 @@ 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(); - static const uint64_t LANE_CHANGE_LOOK_AHEAD = 5u; - // Cosine of the angle. - static const float LANE_CHANGE_ANGULAR_THRESHOLD = 0.5f; static const float GRID_SIZE = 4.0f; - + static const float PED_GRID_SIZE = 10.0f; + static const float MAX_GEODESIC_GRID_LENGTH = 20.0f; } // namespace MapConstants + namespace cg = carla::geom; using namespace MapConstants; - InMemoryMap::InMemoryMap(TopologyList topology) { - _topology = topology; + InMemoryMap::InMemoryMap(RawNodeList _raw_dense_topology) { + raw_dense_topology = _raw_dense_topology; } InMemoryMap::~InMemoryMap() {} - void InMemoryMap::SetUp(float sampling_resolution) { + void InMemoryMap::SetUp() { NodeList entry_node_list; NodeList exit_node_list; @@ -39,98 +36,103 @@ namespace MapConstants { }; auto square = [](float input) {return std::pow(input, 2);}; - // Creating dense topology. - for (auto &pair : _topology) { + // Consuming the raw dense topology from cc::Map into SimpleWaypoints. + std::map, std::vector> segment_map; + for (auto& waypoint_ptr: raw_dense_topology) { + auto road_id = waypoint_ptr->GetRoadId(); + auto lane_id = waypoint_ptr->GetLaneId(); + if (segment_map.find({road_id, lane_id}) != segment_map.end()) { + segment_map.at({road_id, lane_id}).push_back(std::make_shared(waypoint_ptr)); + } else { + segment_map.insert({{road_id, lane_id}, {std::make_shared(waypoint_ptr)}}); + } + } - // Looping through every topology segment. - 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; + auto compare_s = [] (const SimpleWaypointPtr& swp1, const SimpleWaypointPtr& swp2) { + return (swp1->GetWaypoint()->GetDistance() < swp2->GetWaypoint()->GetDistance()); + }; - if (distance_squared(begin_location, end_location) > square(ZERO_LENGTH)) { + GeoGridId geodesic_grid_id_counter = -1; + for (auto& segment: segment_map) { - // Adding entry waypoint. - WaypointPtr current_waypoint = begin_waypoint; - dense_topology.push_back(std::make_shared(current_waypoint)); + // Generating geodesic grid ids. + ++geodesic_grid_id_counter; - entry_node_list.push_back(dense_topology.back()); + // Ordering waypoints to be consecutive. + auto& segment_waypoints = segment.second; + std::sort(segment_waypoints.begin(), segment_waypoints.end(), compare_s); - // Populating waypoints from begin_waypoint to end_waypoint. - while (distance_squared(current_waypoint->GetTransform().location, - end_location) > square(sampling_resolution)) { + if (segment_waypoints.front()->DistanceSquared(segment_waypoints.back()) > square(0.1f)) { - current_waypoint = current_waypoint->GetNext(sampling_resolution)[0]; - SimpleWaypointPtr previous_wp = dense_topology.back(); - dense_topology.push_back(std::make_shared(current_waypoint)); + SimpleWaypointPtr first_point = segment_waypoints.at(0); + SimpleWaypointPtr second_point = segment_waypoints.at(1); + cg::Vector3D first_to_second = second_point->GetLocation() - first_point->GetLocation(); + first_to_second = first_to_second.MakeUnitVector(); + cg::Vector3D first_heading = first_point->GetForwardVector(); - previous_wp->SetNextWaypoint({dense_topology.back()}); + if (cg::Math::Dot(first_heading, first_to_second) < 0.0f) { + std::reverse(segment_waypoints.begin(), segment_waypoints.end()); } - // Adding exit waypoint. - SimpleWaypointPtr previous_wp = dense_topology.back(); - dense_topology.push_back(std::make_shared(end_waypoint)); + // Registering segment end points. + entry_node_list.push_back(segment_waypoints.front()); + exit_node_list.push_back(segment_waypoints.back()); - previous_wp->SetNextWaypoint({dense_topology.back()}); - exit_node_list.push_back(dense_topology.back()); - } - } + // Placing intra-segment connections. + cg::Location grid_edge_location = segment_waypoints.front()->GetLocation(); + for (uint64_t i=0; i< segment_waypoints.size() -1; ++i) { - // Linking segments. - uint64_t 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) && i != j) { - end_point->SetNextWaypoint({begin_point}); + // Assigning grid id. + if (distance_squared(grid_edge_location, segment_waypoints.at(i)->GetLocation()) > + square(MAX_GEODESIC_GRID_LENGTH)) { + ++geodesic_grid_id_counter; + grid_edge_location = segment_waypoints.at(i)->GetLocation(); + } + segment_waypoints.at(i)->SetGeodesicGridId(geodesic_grid_id_counter); + + segment_waypoints.at(i)->SetNextWaypoint({segment_waypoints.at(i+1)}); + segment_waypoints.at(i+1)->SetPreviousWaypoint({segment_waypoints.at(i)}); + } + segment_waypoints.back()->SetGeodesicGridId(geodesic_grid_id_counter); + + // Adding to processed dense topology. + for (auto swp: segment_waypoints) { + dense_topology.push_back(swp); } - ++j; } - ++i; } - // Tying up loose ends. - // Loop through all exit nodes of topology segments, - // connect any dangling endpoints to the nearest entry point - // of another topology segment. - i = 0u; - for (auto &end_point : exit_node_list) { - if (end_point->GetNextWaypoint().size() == 0) { - j = 0u; - float min_distance = INFINITE_DISTANCE; - SimpleWaypointPtr closest_connection; - for (auto &begin_point : entry_node_list) { - float new_distance = end_point->DistanceSquared(begin_point); - if (new_distance < min_distance && i != j) { - min_distance = new_distance; - closest_connection = begin_point; - } - ++j; + // Localizing waypoints into grids. + for (auto &simple_waypoint: dense_topology) { + if (simple_waypoint != nullptr) { + const cg::Location loc = simple_waypoint->GetLocation(); + const std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, true)); + if (waypoint_grid.find(grid_key) == waypoint_grid.end()) { + waypoint_grid.insert({grid_key, {simple_waypoint}}); + } else { + waypoint_grid.at(grid_key).insert(simple_waypoint); } - const cg::Vector3D end_point_vector = end_point->GetForwardVector(); - cg::Vector3D relative_vector = closest_connection->GetLocation() - end_point->GetLocation(); - relative_vector = relative_vector.MakeUnitVector(); - const float relative_dot = cg::Math::Dot(end_point_vector, relative_vector); - if (relative_dot < LANE_CHANGE_ANGULAR_THRESHOLD) { - uint64_t count = LANE_CHANGE_LOOK_AHEAD; - while (count > 0u) { - closest_connection = closest_connection->GetNextWaypoint()[0]; - --count; - } + const std::string ped_grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, false)); + if (ped_waypoint_grid.find(ped_grid_key) == ped_waypoint_grid.end()) { + ped_waypoint_grid.insert({ped_grid_key, {simple_waypoint}}); + } else { + ped_waypoint_grid.at(ped_grid_key).insert(simple_waypoint); } - end_point->SetNextWaypoint({closest_connection}); } - ++i; } - // Localizing waypoints into grids. - for (auto &simple_waypoint: dense_topology) { - 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 { - waypoint_grid.at(grid_key).insert(simple_waypoint); + // Linking segments. + uint64_t i = 0u, j = 0u; + for (SimpleWaypointPtr end_point : exit_node_list) { + j = 0u; + for (SimpleWaypointPtr begin_point: entry_node_list) { + if ((end_point->DistanceSquared(begin_point) < square(2.0f)) && (i != j)) { + end_point->SetNextWaypoint({begin_point}); + begin_point->SetPreviousWaypoint({end_point}); + } + ++j; } + ++i; } // Linking lane change connections. @@ -139,10 +141,26 @@ namespace MapConstants { FindAndLinkLaneChange(simple_waypoint); } } + + // Linking any unconnected segments. + for (auto& swp: dense_topology) { + if (swp->GetNextWaypoint().size() == 0) { + SimpleWaypointPtr nearest_sample = GetWaypointInVicinity(swp->GetLocation() + + cg::Location(swp->GetForwardVector() * 0.2f)); + swp->SetNextWaypoint({nearest_sample}); + nearest_sample->SetPreviousWaypoint({swp}); + } + } + + MakeGeodesiGridCenters(); } - std::pair InMemoryMap::MakeGridId (float x, float y) { - return {static_cast(std::floor(x/GRID_SIZE)), static_cast(std::floor(y/GRID_SIZE))}; + std::pair InMemoryMap::MakeGridId (float x, float y, bool vehicle_or_pedestrian) { + if (vehicle_or_pedestrian) { + return {static_cast(std::floor(x/GRID_SIZE)), static_cast(std::floor(y/GRID_SIZE))}; + } else { + return {static_cast(std::floor(x/PED_GRID_SIZE)), static_cast(std::floor(y/PED_GRID_SIZE))}; + } } std::string InMemoryMap::MakeGridKey (std::pair grid_key) { @@ -151,7 +169,7 @@ namespace MapConstants { SimpleWaypointPtr InMemoryMap::GetWaypointInVicinity(cg::Location location) { - const std::pair grid_ids = MakeGridId(location.x, location.y); + const std::pair grid_ids = MakeGridId(location.x, location.y, true); SimpleWaypointPtr closest_waypoint = nullptr; float closest_distance = INFINITE_DISTANCE; @@ -189,6 +207,38 @@ namespace MapConstants { return closest_waypoint; } + SimpleWaypointPtr InMemoryMap::GetPedWaypoint(cg::Location location) { + + const std::pair grid_ids = MakeGridId(location.x, location.y, false); + SimpleWaypointPtr closest_waypoint = nullptr; + float closest_distance = INFINITE_DISTANCE; + + // Search all surrounding grids for closest waypoint. + for (int i = -1; i <= 1; ++i) { + for (int j = -1; j <= 1; ++j) { + + const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j}); + if (ped_waypoint_grid.find(grid_key) != ped_waypoint_grid.end()) { + + const auto& waypoint_set = ped_waypoint_grid.at(grid_key); + if (closest_waypoint == nullptr) { + closest_waypoint = *waypoint_set.begin(); + } + + for (auto &simple_waypoint: waypoint_set) { + + if (simple_waypoint->DistanceSquared(location) < std::pow(closest_distance, 2)) { + closest_waypoint = simple_waypoint; + closest_distance = simple_waypoint->DistanceSquared(location); + } + } + } + } + } + + return closest_waypoint; + } + SimpleWaypointPtr InMemoryMap::GetWaypoint(const cg::Location &location) const { SimpleWaypointPtr closest_waypoint; @@ -255,5 +305,27 @@ namespace MapConstants { } } + void InMemoryMap::MakeGeodesiGridCenters() { + for (auto& swp: dense_topology) { + GeoGridId ggid = swp->CheckJunction()? swp->GetJunctionId(): swp->GetGeodesicGridId(); + if (geodesic_grid_center.find(ggid) == geodesic_grid_center.end()) { + geodesic_grid_center.insert({ggid, swp->GetLocation()}); + } else { + cg::Location& grid_loc = geodesic_grid_center.at(ggid); + grid_loc = (grid_loc + swp->GetLocation())/2; + } + } + } + + cg::Location InMemoryMap::GetGeodesicGridCenter(GeoGridId ggid) { + cg::Location grid_center; + if (geodesic_grid_center.find(ggid) != geodesic_grid_center.end()) { + grid_center = geodesic_grid_center.at(ggid); + } else { + grid_center = cg::Location(); + } + return grid_center; + } + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.h b/LibCarla/source/carla/trafficmanager/InMemoryMap.h index 0d79b855b4..401bbc1122 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.h +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.h @@ -18,6 +18,7 @@ #include "carla/geom/Math.h" #include "carla/Memory.h" #include "carla/road/Lane.h" +#include "carla/road/RoadTypes.h" #include "carla/trafficmanager/SimpleWaypoint.h" @@ -32,6 +33,8 @@ namespace traffic_manager { using TopologyList = std::vector>; using SimpleWaypointPtr = std::shared_ptr; using NodeList = std::vector; + using RawNodeList = std::vector; + using GeoGridId = crd::JuncId; /// This class builds a discretized local map-cache. /// Instantiate the class with map topology from the simulator @@ -41,15 +44,20 @@ namespace traffic_manager { private: /// Object to hold sparse topology received by the constructor. - TopologyList _topology; + RawNodeList raw_dense_topology; /// Structure to hold all custom waypoint objects after /// interpolation of sparse topology. NodeList dense_topology; /// Grid localization map for all waypoints in the system. - std::unordered_map> waypoint_grid; + using WaypointGrid = std::unordered_map>; + WaypointGrid waypoint_grid; + /// Larger localization map for all waypoints to be used for localizing pedestrians. + WaypointGrid ped_waypoint_grid; + /// Geodesic grid topology. + std::unordered_map geodesic_grid_center; /// Method to generate the grid ids for given co-ordinates. - std::pair MakeGridId(float x, float y); + std::pair MakeGridId(float x, float y, bool vehicle_or_pedestrian); /// Method to generate map key for waypoint_grid. std::string MakeGridKey(std::pair gird_id); @@ -59,12 +67,12 @@ namespace traffic_manager { public: - InMemoryMap(TopologyList topology); + InMemoryMap(RawNodeList _raw_dense_topology); ~InMemoryMap(); /// This method constructs the local map with a resolution of /// sampling_resolution. - void SetUp(float sampling_resolution); + void SetUp(); /// This method returns the closest waypoint to a given location on the map. SimpleWaypointPtr GetWaypoint(const cg::Location &location) const; @@ -72,10 +80,15 @@ namespace traffic_manager { /// This method returns closest waypoint in the vicinity of the given co-ordinates. SimpleWaypointPtr GetWaypointInVicinity(cg::Location location); + SimpleWaypointPtr GetPedWaypoint(cg::Location location); + /// This method returns the full list of discrete samples of the map in the /// local cache. NodeList GetDenseTopology() const; + void MakeGeodesiGridCenters(); + cg::Location GetGeodesicGridCenter(GeoGridId ggid); + }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index f98ac737b6..77bdc70923 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -11,14 +11,15 @@ namespace traffic_manager { namespace LocalizationConstants { - static const float WAYPOINT_TIME_HORIZON = 3.0f; + static const float WAYPOINT_TIME_HORIZON = 5.0f; static const float MINIMUM_HORIZON_LENGTH = 30.0f; static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f; - static const float TARGET_WAYPOINT_HORIZON_LENGTH = 4.0f; + static const float TARGET_WAYPOINT_HORIZON_LENGTH = 5.0f; static const float MINIMUM_JUNCTION_LOOK_AHEAD = 10.0f; 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; + static const float MINIMUM_LANE_CHANGE_DISTANCE = 50.0f; + static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f; + static const uint64_t UNREGISTERED_ACTORS_SCAN_INTERVAL = 10; } // namespace LocalizationConstants @@ -32,7 +33,8 @@ namespace LocalizationConstants { AtomicActorSet ®istered_actors, InMemoryMap &local_map, Parameters ¶meters, - cc::DebugHelper &debug_helper) + cc::DebugHelper &debug_helper, + cc::World& world) : PipelineStage(stage_name), planner_messenger(planner_messenger), collision_messenger(collision_messenger), @@ -40,36 +42,33 @@ namespace LocalizationConstants { registered_actors(registered_actors), local_map(local_map), parameters(parameters), - debug_helper(debug_helper) { + debug_helper(debug_helper), + world(world) { // Initializing various output frame selectors. planner_frame_selector = true; collision_frame_selector = true; - collision_frame_ready = false; traffic_light_frame_selector = true; // Initializing the number of vehicles to zero in the begining. number_of_vehicles = 0u; - // Initializing messenger states to initiate data writes - // preemptively since this is the first stage in the pipeline. - planner_messenger_state = planner_messenger->GetState() - 1; - collision_messenger_state = collision_messenger->GetState() - 1; - traffic_light_messenger_state = traffic_light_messenger->GetState() - 1; // Initializing the registered actors container state. registered_actors_state = -1; - } LocalizationStage::~LocalizationStage() {} void LocalizationStage::Action() { + ScanUnregisteredVehicles(); + // Selecting output frames based on selector keys. 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 (uint64_t i = 0u; i < actor_list.size(); ++i) { @@ -142,12 +141,23 @@ namespace LocalizationConstants { uint64_t selection_index = 0u; // Pseudo-randomized path selection if found more than one choice. if (next_waypoints.size() > 1) { - selection_index = static_cast(rand()) % next_waypoints.size(); + selection_index = static_cast(rand()) % next_waypoints.size(); } - - PushWaypoint(waypoint_buffer, actor_id, next_waypoints.at(selection_index)); + SimpleWaypointPtr next_wp = next_waypoints.at(selection_index); + if (next_wp == nullptr) { + for (auto& wp: next_waypoints) { + if (wp != nullptr) { + next_wp = wp; + break; + } + } + } + PushWaypoint(waypoint_buffer, actor_id, next_wp); } + // Updating geodesic grid position for actor. + track_traffic.UpdateGridPosition(actor_id, waypoint_buffer); + // Generating output. const float target_point_distance = std::max(std::ceil(vehicle_velocity * TARGET_WAYPOINT_TIME_HORIZON), TARGET_WAYPOINT_HORIZON_LENGTH); @@ -188,7 +198,8 @@ namespace LocalizationConstants { } bool approaching_junction = false; - if (waypoint_buffer.front()->CheckJunction() || (look_ahead_point->CheckJunction() && !(waypoint_buffer.front()->CheckJunction()))) { + if (waypoint_buffer.front()->CheckJunction() || + (look_ahead_point->CheckJunction() && !(waypoint_buffer.front()->CheckJunction()))) { if (speed_limit > HIGHWAY_SPEED) { for (uint64_t j = 0u; (j < look_ahead_index) && !approaching_junction; ++j) { SimpleWaypointPtr swp = waypoint_buffer.at(j); @@ -201,30 +212,6 @@ namespace LocalizationConstants { } } - // Clean up tracking list by remove vehicles that are too far away. - const ActorIdSet current_tracking_list = track_traffic.GetOverlappingVehicles(actor_id); - for (ActorId tracking_id: current_tracking_list) { - if (!waypoint_buffer.empty()) { - - const cg::Location tracking_location = actor_list.at( - vehicle_id_to_index.at(tracking_id))->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(); - - 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); - track_traffic.RemoveOverlappingVehicle(tracking_id, actor_id); - } - } - } - // Editing output frames. LocalizationToPlannerData &planner_message = current_planner_frame->at(i); planner_message.actor = vehicle; @@ -232,28 +219,28 @@ namespace LocalizationConstants { planner_message.distance = distance; planner_message.approaching_true_junction = approaching_junction; - // Reading current messenger state of the collision stage before modifying it's frame. - if ((collision_messenger->GetState() != collision_messenger_state) && - !collision_frame_ready) { - - LocalizationToCollisionData &collision_message = current_collision_frame->at(i); - collision_message.actor = vehicle; - collision_message.buffer = waypoint_buffer; - collision_message.overlapping_actors = track_traffic.GetOverlappingVehicles(actor_id); + LocalizationToCollisionData &collision_message = current_collision_frame->at(i); + collision_message.actor = vehicle; + collision_message.buffer = waypoint_buffer; + collision_message.overlapping_actors.clear(); + ActorIdSet overlapping_actor_set = track_traffic.GetOverlappingVehicles(actor_id); + for (ActorId overlapping_actor_id: overlapping_actor_set) { + Actor actor_ptr = nullptr; + if (vehicle_id_to_index.find(overlapping_actor_id) != vehicle_id_to_index.end()) { + actor_ptr = actor_list.at(vehicle_id_to_index.at(overlapping_actor_id)); + } else if (unregistered_actors.find(overlapping_actor_id) != unregistered_actors.end()) { + actor_ptr = unregistered_actors.at(overlapping_actor_id); + } + collision_message.overlapping_actors.insert({overlapping_actor_id, actor_ptr}); } + collision_message.closest_waypoint = waypoint_buffer.front(); + collision_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index); LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i); traffic_light_message.actor = vehicle; traffic_light_message.closest_waypoint = waypoint_buffer.front(); traffic_light_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index); } - - if ((collision_messenger->GetState() != collision_messenger_state) - && !collision_frame_ready) { - - collision_frame_ready = true; - } - } void LocalizationStage::DataReceiver() { @@ -264,21 +251,18 @@ namespace LocalizationConstants { if (registered_actors_state != registered_actors.GetState()) { actor_list = registered_actors.GetList(); - uint64_t index = 0u; for (auto &actor: actor_list) { - vehicle_id_to_index.insert({actor->GetId(), index}); ++index; } - registered_actors_state = registered_actors.GetState(); } // Allocating new containers for the changed number of registered vehicles. if (number_of_vehicles != actor_list.size()) { - number_of_vehicles = static_cast(actor_list.size()); + number_of_vehicles = static_cast(actor_list.size()); // Allocating the buffer lists. buffer_list = std::make_shared(number_of_vehicles); // Allocating output frames to be shared with the motion planner stage. @@ -296,46 +280,15 @@ namespace LocalizationConstants { void LocalizationStage::DataSender() { - // Since send/receive calls on messenger objects can block if the other - // end hasn't received/sent data, choose to block on only those stages - // which takes the most priority (which needs the highest rate of data feed) - // to run the system well. - - const DataPacket> planner_data_packet = { - planner_messenger_state, - planner_frame_selector ? planner_frame_a : planner_frame_b - }; + planner_messenger->Push(planner_frame_selector ? planner_frame_a : planner_frame_b); planner_frame_selector = !planner_frame_selector; - planner_messenger_state = planner_messenger->SendData(planner_data_packet); - - // Send data to collision stage only if it has finished - // processing, received the previous message and started processing it. - int collision_messenger_current_state = collision_messenger->GetState(); - if ((collision_messenger_current_state != collision_messenger_state) && - collision_frame_ready) { - - 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; - collision_frame_ready = false; - } - // Send data to traffic light stage only if it has finished - // processing, received the previous message and started processing it. - const int traffic_light_messenger_current_state = traffic_light_messenger->GetState(); - if (traffic_light_messenger_current_state != traffic_light_messenger_state) { - 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; - } + const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b; + collision_messenger->Push(current_collision_frame); + collision_frame_selector = !collision_frame_selector; + + traffic_light_messenger->Push(traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b); + traffic_light_frame_selector = !traffic_light_frame_selector; } void LocalizationStage::DrawBuffer(Buffer &buffer) { @@ -350,55 +303,66 @@ namespace LocalizationConstants { const uint64_t waypoint_id = waypoint->GetId(); buffer.push_back(waypoint); track_traffic.UpdatePassingVehicle(waypoint_id, actor_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()) - ); - - for (auto new_actor_id: actor_set_difference) { - - track_traffic.UpdateOverlappingVehicle(actor_id, new_actor_id); - track_traffic.UpdateOverlappingVehicle(new_actor_id, actor_id); - } } void LocalizationStage::PopWaypoint(Buffer& buffer, ActorId actor_id) { - const uint64_t removed_waypoint_id = buffer.front()->GetId(); + SimpleWaypointPtr removed_waypoint = buffer.front(); + SimpleWaypointPtr remaining_waypoint = nullptr; + const uint64_t removed_waypoint_id = removed_waypoint->GetId(); buffer.pop_front(); track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id); + } - if (!buffer.empty()) { - - 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()) - ); - - for (auto& old_actor_id: actor_set_difference) { - - track_traffic.RemoveOverlappingVehicle(actor_id, old_actor_id); - track_traffic.RemoveOverlappingVehicle(old_actor_id, actor_id); + void LocalizationStage::ScanUnregisteredVehicles() { + ++unregistered_scan_duration; + // Periodically check for actors not spawned by TrafficManager. + if (unregistered_scan_duration == UNREGISTERED_ACTORS_SCAN_INTERVAL) { + unregistered_scan_duration = 0; + + 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()) { + 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}); + } } - } else { + // Scanning for pedestrians. + for (auto walker: *world_walker.get()) { + const auto unregistered_id = walker->GetId(); + if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { + unregistered_actors.insert({unregistered_id, walker}); + } + } + } - 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); + // Regularly update unregistered actors. + std::vector actor_ids_to_erase; + for (auto& actor_info: unregistered_actors) { + if (actor_info.second->IsAlive()) { + cg::Location actor_location = actor_info.second->GetLocation(); + SimpleWaypointPtr nearest_waypoint = nullptr; + const auto unregistered_type = actor_info.second->GetTypeId(); + if (unregistered_type[0] == 'v') { + nearest_waypoint = local_map.GetWaypointInVicinity(actor_location); + } else if (unregistered_type[0] == 'w') { + nearest_waypoint = local_map.GetPedWaypoint(actor_location); + } + if (nearest_waypoint == nullptr) { + nearest_waypoint = local_map.GetWaypoint(actor_location); + } + track_traffic.UpdateUnregisteredGridPosition(actor_info.first, nearest_waypoint); + } else { + track_traffic.DeleteActor(actor_info.first); + actor_ids_to_erase.push_back(actor_info.first); } } + for (auto actor_id: actor_ids_to_erase) { + unregistered_actors.erase(actor_id); + } } SimpleWaypointPtr LocalizationStage::AssignLaneChange(Actor vehicle, bool force, bool direction) { @@ -406,6 +370,7 @@ namespace LocalizationConstants { const ActorId actor_id = vehicle->GetId(); const cg::Location vehicle_location = vehicle->GetLocation(); const float vehicle_velocity = vehicle->GetVelocity().Length(); + const float speed_limit = boost::static_pointer_cast(vehicle)->GetSpeedLimit(); const Buffer& waypoint_buffer = buffer_list->at(vehicle_id_to_index.at(actor_id)); const SimpleWaypointPtr& current_waypoint = waypoint_buffer.front(); @@ -414,7 +379,7 @@ namespace LocalizationConstants { const auto left_waypoint = current_waypoint->GetLeftWaypoint(); const auto right_waypoint = current_waypoint->GetRightWaypoint(); - if (!force) { + if (!force && current_waypoint != nullptr) { const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id); @@ -424,46 +389,55 @@ namespace LocalizationConstants { ++i) { const ActorId &other_vehicle_id = *i; - 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; - 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 && - track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0 && - vehicle_velocity < HIGHWAY_SPEED) { - distant_lane_availability = true; - } - } - - 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) { - - 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) { - - if (distant_lane_availability && - squared_vehicle_distance > std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) { - - need_to_change_lane = true; - } else if (squared_vehicle_distance < std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) { - - need_to_change_lane = false; - abort_lane_change = true; + // This is totally ignoring unregistered actors during lane changes. + // Need to fix this. Only a temporary solution. + if (vehicle_id_to_index.find(other_vehicle_id) != vehicle_id_to_index.end()) { + const Buffer& other_buffer = buffer_list->at(vehicle_id_to_index.at(other_vehicle_id)); + + if (!other_buffer.empty()) { + const SimpleWaypointPtr& other_current_waypoint = other_buffer.front(); + const cg::Location other_location = other_current_waypoint->GetLocation(); + + bool distant_lane_availability = false; + const auto other_neighbouring_lanes = { + other_current_waypoint->GetLeftWaypoint(), + other_current_waypoint->GetRightWaypoint()}; + if (other_current_waypoint != nullptr) { + for (auto& candidate_lane_wp: other_neighbouring_lanes) { + if (candidate_lane_wp != nullptr && + track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0 && + speed_limit < HIGHWAY_SPEED) { + distant_lane_availability = true; + } + } + + 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) { + + 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) { + + if (distant_lane_availability && + squared_vehicle_distance > std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) { + + need_to_change_lane = true; + } else if (squared_vehicle_distance < std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) { + + need_to_change_lane = false; + abort_lane_change = true; + } + + } + } } - } } } @@ -481,9 +455,9 @@ namespace LocalizationConstants { std::vector candidate_points; if (force) { - if (direction) { + if (direction && left_waypoint != nullptr) { candidate_points.push_back(left_waypoint); - } else { + } else if (!direction && right_waypoint != nullptr) { candidate_points.push_back(right_waypoint); } } else { diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index cf9b2cc6b0..e587aad192 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -31,6 +31,7 @@ #include "carla/trafficmanager/Parameters.h" #include "carla/trafficmanager/PipelineStage.h" #include "carla/trafficmanager/SimpleWaypoint.h" +#include "carla/trafficmanager/PerformanceDiagnostics.h" namespace carla { namespace traffic_manager { @@ -49,14 +50,9 @@ namespace traffic_manager { private: - /// Variables to remember messenger states. - int planner_messenger_state; - int collision_messenger_state; - int traffic_light_messenger_state; /// Section keys to switch between the output data frames. bool planner_frame_selector; bool collision_frame_selector; - bool collision_frame_ready; bool traffic_light_frame_selector; /// Output data frames to be shared with the motion planner stage. std::shared_ptr planner_frame_a; @@ -86,11 +82,13 @@ namespace traffic_manager { Parameters ¶meters; /// Reference to Carla's debug helper object. cc::DebugHelper &debug_helper; + /// Carla world object; + cc::World& world; /// Structures to hold waypoint buffers for all vehicles. /// These are shared with the collisions stage. std::shared_ptr buffer_list; /// Map connecting actor ids to indices of data arrays. - std::unordered_map vehicle_id_to_index; + std::unordered_map vehicle_id_to_index; /// Number of vehicles currently registered with the traffic manager. uint64_t number_of_vehicles; /// Used to only calculate the extended buffer once at junctions @@ -101,6 +99,13 @@ namespace traffic_manager { TrackTraffic track_traffic; /// Map of all vehicles' idle time std::unordered_map> idle_time; + /// Counter to track unregistered actors' scan interval. + uint64_t unregistered_scan_duration = 0; + /// A structure used to keep track of actors spawned outside of traffic + /// manager. + std::unordered_map unregistered_actors; + /// Code snippet execution time profiler. + SnippetProfiler snippet_profiler; /// A simple method used to draw waypoint buffer ahead of a vehicle. void DrawBuffer(Buffer &buffer); @@ -110,6 +115,8 @@ namespace traffic_manager { /// Methods to modify waypoint buffer and track traffic. void PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint); void PopWaypoint(Buffer& buffer, ActorId actor_id); + /// Method to scan for unregistered actors and update their grid positioning. + void ScanUnregisteredVehicles(); public: @@ -121,7 +128,8 @@ namespace traffic_manager { AtomicActorSet ®istered_actors, InMemoryMap &local_map, Parameters ¶meters, - cc::DebugHelper &debug_helper); + cc::DebugHelper &debug_helper, + cc::World& world); ~LocalizationStage(); diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp index fa7b92361b..71b46c759c 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp @@ -9,6 +9,12 @@ namespace carla { namespace traffic_manager { +namespace LocalizationConstants { + const static uint64_t BUFFER_STEP_THROUGH = 10u; +} // namespace LocalizationConstants + + using namespace LocalizationConstants; + float DeviationCrossProduct(Actor actor, const cg::Location &target_location) { cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); @@ -53,37 +59,137 @@ namespace traffic_manager { TrackTraffic::TrackTraffic() {} - void TrackTraffic::UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id) { + void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint) { + + // Add actor entry if not present. + if (actor_to_grids.find(actor_id) == actor_to_grids.end()) { + actor_to_grids.insert({actor_id, {}}); + } + + std::unordered_set& current_grids = actor_to_grids.at(actor_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); + // Clear current actor from all grids containing current actor. + for (auto& grid_id: current_grids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + ActorIdSet& actor_ids = grid_to_actors.at(grid_id); + if (actor_ids.find(actor_id) != actor_ids.end()) { + actor_ids.erase(actor_id); + } + } + } + + // Clear all grids current actor is tracking. + current_grids.clear(); + + // Step through buffer and update grid list for actor and actor list for grids. + if (waypoint != nullptr) { + + GeoGridId ggid = waypoint->GetGeodesicGridId(); + current_grids.insert(ggid); + + // Add grid entry if not present. + if (grid_to_actors.find(ggid) == grid_to_actors.end()) { + grid_to_actors.insert({ggid, {}}); + } + + ActorIdSet& actor_ids = grid_to_actors.at(ggid); + if (actor_ids.find(actor_id) == actor_ids.end()) { + actor_ids.insert(actor_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::UpdateGridPosition(const ActorId actor_id, const Buffer& buffer) { + + // Add actor entry if not present. + if (actor_to_grids.find(actor_id) == actor_to_grids.end()) { + actor_to_grids.insert({actor_id, {}}); + } + + std::unordered_set& current_grids = actor_to_grids.at(actor_id); + + // Clear current actor from all grids containing current actor. + for (auto& grid_id: current_grids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + ActorIdSet& actor_ids = grid_to_actors.at(grid_id); + if (actor_ids.find(actor_id) != actor_ids.end()) { + actor_ids.erase(actor_id); + } + } + } + + // Clear all grids current actor is tracking. + current_grids.clear(); + + // Step through buffer and update grid list for actor and actor list for grids. + if (!buffer.empty()) { + uint64_t buffer_size = buffer.size(); + uint64_t step_size = static_cast(std::floor(buffer_size/BUFFER_STEP_THROUGH)); + for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) { + GeoGridId ggid = buffer.at(std::min(i* step_size, buffer_size-1u))->GetGeodesicGridId(); + current_grids.insert(ggid); + + // Add grid entry if not present. + if (grid_to_actors.find(ggid) == grid_to_actors.end()) { + grid_to_actors.insert({ggid, {}}); + } + + ActorIdSet& actor_ids = grid_to_actors.at(ggid); + if (actor_ids.find(actor_id) == actor_ids.end()) { + actor_ids.insert(actor_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(); + ActorIdSet actor_id_set; + + if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { + std::unordered_set& grid_ids = actor_to_grids.at(actor_id); + for (auto& grid_id: grid_ids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + ActorIdSet& actor_ids = grid_to_actors.at(grid_id); + actor_id_set.insert(actor_ids.begin(), actor_ids.end()); + } + } + } + + return actor_id_set; + } + + void TrackTraffic::DeleteActor(ActorId actor_id) { + + if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { + std::unordered_set& grid_ids = actor_to_grids.at(actor_id); + for (auto& grid_id: grid_ids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + ActorIdSet& actor_ids = grid_to_actors.at(grid_id); + if (actor_ids.find(actor_id) != actor_ids.end()) { + actor_ids.erase(actor_id); + } + } + } + actor_to_grids.erase(actor_id); } } + std::unordered_set TrackTraffic::GetGridIds(ActorId actor_id) { + + std::unordered_set grid_ids; + + if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { + grid_ids = actor_to_grids.at(actor_id); + } + + return grid_ids; + } + + std::unordered_map TrackTraffic::GetGridActors() { + return grid_to_actors; + } void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) { @@ -97,7 +203,6 @@ namespace traffic_manager { waypoint_overlap_tracker.insert({waypoint_id, {actor_id}}); } - } void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) { @@ -122,6 +227,7 @@ namespace traffic_manager { } else { return ActorIdSet(); } + } } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h index d55382c860..f3e786aa44 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h @@ -7,8 +7,11 @@ #pragma once #include "carla/client/Actor.h" +#include "carla/client/ActorList.h" #include "carla/client/Vehicle.h" +#include "carla/client/World.h" #include "carla/geom/Location.h" +#include "carla/road/RoadTypes.h" #include "carla/rpc/ActorId.h" #include "carla/trafficmanager/SimpleWaypoint.h" @@ -23,6 +26,7 @@ namespace traffic_manager { using ActorIdSet = std::unordered_set; using SimpleWaypointPtr = std::shared_ptr; using Buffer = std::deque; + using GeoGridId = carla::road::JuncId; class TrackTraffic{ @@ -30,10 +34,12 @@ namespace traffic_manager { /// 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; + /// Geodesic grids occupied by actors's paths. + std::unordered_map> actor_to_grids; + /// Actors currently passing through grids. + std::unordered_map grid_to_actors; public: TrackTraffic(); @@ -43,11 +49,16 @@ namespace traffic_manager { 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); + void UpdateGridPosition(const ActorId actor_id, const Buffer& buffer); + void UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint); + ActorIdSet GetOverlappingVehicles(ActorId actor_id); + /// Method to delete actor data from tracking. + void DeleteActor(ActorId actor_id); + + std::unordered_set GetGridIds(ActorId actor_id); + std::unordered_map GetGridActors(); }; /// Returns the cross product (z component value) between the vehicle's diff --git a/LibCarla/source/carla/trafficmanager/Messenger.h b/LibCarla/source/carla/trafficmanager/Messenger.h index 3e9cad66c5..e05ee758c4 100644 --- a/LibCarla/source/carla/trafficmanager/Messenger.h +++ b/LibCarla/source/carla/trafficmanager/Messenger.h @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -16,18 +17,6 @@ namespace traffic_manager { using namespace std::chrono_literals; - template - struct DataPacket { - int id; - Data data; - }; - - /// This class is the template for messaging functionality between - /// different stage classes to send and receive data. - /// One object of this type can only facilitate receiving data from - /// a sender stage and passing the data onto a receiver stage. - /// The class maintains state internally and blocks send or receive - /// requests until data is available/successfully passed on. template class Messenger { @@ -35,11 +24,8 @@ namespace traffic_manager { /// Flag used to wake up and join any waiting function calls on this object. std::atomic stop_messenger; - /// State variable that will progress upon every successful communication - /// between the sender and receiver. - std::atomic state_counter; /// Member used to hold data sent by the sender. - Data data; + std::deque d_queue; /// Mutex used to manage contention between the sender and receiver. std::mutex data_modification_mutex; /// Variable to conditionally block sender in case waiting for the receiver. @@ -50,58 +36,60 @@ namespace traffic_manager { public: Messenger() { - state_counter.store(0); stop_messenger.store(false); } ~Messenger() {} - /// This method receives data from a sender, stores in a member and - /// increments state. - int SendData(DataPacket packet) { + void Push(Data data) { std::unique_lock lock(data_modification_mutex); - while (state_counter.load() == packet.id && !stop_messenger.load()) { - send_condition.wait_for(lock, 1ms, [=] {return state_counter.load() != packet.id;}); + while (!d_queue.empty() && !stop_messenger.load()) { + send_condition.wait_for(lock, 1ms, [=] { + return (d_queue.empty() && stop_messenger.load()); + }); + } + if(!stop_messenger.load()){ + d_queue.push_front(data); + receive_condition.notify_one(); } - data = packet.data; - state_counter.store(state_counter.load() + 1); - int present_state = state_counter.load(); - receive_condition.notify_one(); - - return present_state; } - /// This method presents stored data to the receiver and increments state. - DataPacket ReceiveData(int old_state) { + Data Peek() { std::unique_lock lock(data_modification_mutex); - while (state_counter.load() == old_state && !stop_messenger.load()) { - receive_condition.wait_for(lock, 1ms, [=] {return state_counter.load() != old_state;}); + while (d_queue.empty() && !stop_messenger.load()) { + receive_condition.wait_for(lock, 1ms, [=] { + return (!d_queue.empty() && stop_messenger.load()); + }); } - state_counter.store(state_counter.load() + 1); - DataPacket packet = {state_counter.load(), data}; - send_condition.notify_one(); - return packet; + if(!stop_messenger.load()) { + Data data = d_queue.back(); + return data; + } + return Data(); } - /// This method returns the current value of the state counter. - int GetState() { - return state_counter.load(); - } + void Pop() { - /// This method unblocks any waiting calls on this object. - void Stop() { - stop_messenger.store(true); + std::unique_lock lock(data_modification_mutex); + if (!(d_queue.empty() && stop_messenger.load())) { + d_queue.pop_back(); + send_condition.notify_one(); + } } - /// This method restores regular functionality of the messenger. - /// This method needs to be called if the messenger has to be - /// used again after a call to the Stop() method. void Start() { stop_messenger.store(false); } + void Stop() { + stop_messenger.store(true); + d_queue.clear(); + send_condition.notify_one(); + receive_condition.notify_one(); + } + }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h index cd240536be..26ec5c47e7 100644 --- a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h +++ b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h @@ -28,11 +28,14 @@ namespace traffic_manager { /// Alias used for the list of buffers in the localization stage. using BufferList = std::vector; + using Actor = carla::SharedPtr; + using ActorId = carla::ActorId; + /// Data types. /// Type of data sent by the localization stage to the motion planner stage. struct LocalizationToPlannerData { - carla::SharedPtr actor; + Actor actor; float deviation; float distance; bool approaching_true_junction; @@ -48,9 +51,11 @@ namespace traffic_manager { /// Type of data sent by the localization stage to the collision stage. struct LocalizationToCollisionData { - carla::SharedPtr actor; + Actor actor; Buffer buffer; - std::unordered_set overlapping_actors; + std::unordered_map overlapping_actors; + std::shared_ptr closest_waypoint; + std::shared_ptr junction_look_ahead_waypoint; }; /// Type of data sent by the collision stage to the motion planner stage. @@ -60,7 +65,7 @@ namespace traffic_manager { /// Type of data sent by the localization stage to the traffic light stage. struct LocalizationToTrafficLightData { - carla::SharedPtr actor; + Actor actor; std::shared_ptr closest_waypoint; std::shared_ptr junction_look_ahead_waypoint; }; diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp index 6f5595617f..e7608b0c19 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp @@ -44,14 +44,6 @@ namespace PlannerConstants { // Initializing the output frame selector. frame_selector = true; - // Initializing messenger states. - localization_messenger_state = localization_messenger->GetState(); - collision_messenger_state = collision_messenger->GetState(); - traffic_light_messenger_state = traffic_light_messenger->GetState(); - // Initializing this messenger to preemptively write since it precedes - // batch control stage. - control_messenger_state = control_messenger->GetState() - 1; - // Initializing number of vehicles to zero in the beginning. number_of_vehicles = 0u; } @@ -64,7 +56,12 @@ namespace PlannerConstants { const auto current_control_frame = frame_selector ? control_frame_a : control_frame_b; // Looping over all vehicles. - for (uint64_t i = 0u; i < number_of_vehicles; ++i) { + for (uint64_t i = 0u; + i < number_of_vehicles && + localization_frame != nullptr && + collision_frame != nullptr && + traffic_light_frame != nullptr; + ++i) { const LocalizationToPlannerData &localization_data = localization_frame->at(i); const Actor actor = localization_data.actor; @@ -87,12 +84,11 @@ namespace PlannerConstants { traffic_manager::StateEntry previous_state; previous_state = pid_state_map.at(actor_id); - // Increase speed if on highway. - const float speed_limit = vehicle->GetSpeedLimit() / 3.6f; + // Change PID parameters if on highway. const float dynamic_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f; - if (speed_limit > HIGHWAY_SPEED) { + if (current_velocity > HIGHWAY_SPEED) { longitudinal_parameters = highway_longitudinal_parameters; lateral_parameters = highway_lateral_parameters; } else { @@ -117,10 +113,8 @@ namespace PlannerConstants { lateral_parameters); // In case of collision or traffic light - if ((collision_frame != nullptr && traffic_light_frame != nullptr) && - ((collision_messenger_state != 0 && collision_frame->at(i).hazard) || - (traffic_light_messenger_state != 0 && - traffic_light_frame->at(i).traffic_light_hazard))) { + if ((collision_frame != nullptr && collision_frame->at(i).hazard) || + (traffic_light_frame != nullptr && traffic_light_frame->at(i).traffic_light_hazard)) { current_state.deviation_integral = 0.0f; current_state.velocity_integral = 0.0f; @@ -144,31 +138,15 @@ namespace PlannerConstants { void MotionPlannerStage::DataReceiver() { - 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. - const int collision_messenger_current_state = collision_messenger->GetState(); - if (collision_messenger_current_state != 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. - const int traffic_light_messenger_current_state = traffic_light_messenger->GetState(); - if (traffic_light_messenger_current_state != 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; - } + localization_frame = localization_messenger->Peek(); + collision_frame = collision_messenger->Peek(); + traffic_light_frame = traffic_light_messenger->Peek(); // Allocating new containers for the changed number of registered vehicles. if (localization_frame != nullptr && number_of_vehicles != (*localization_frame.get()).size()) { - number_of_vehicles = static_cast((*localization_frame.get()).size()); + number_of_vehicles = static_cast((*localization_frame.get()).size()); // Allocate output frames. control_frame_a = std::make_shared(number_of_vehicles); control_frame_b = std::make_shared(number_of_vehicles); @@ -177,17 +155,21 @@ namespace PlannerConstants { void MotionPlannerStage::DataSender() { - DataPacket> data_packet = { - control_messenger_state, - frame_selector ? control_frame_a : control_frame_b - }; + localization_messenger->Pop(); + collision_messenger->Pop(); + traffic_light_messenger->Pop(); + + control_messenger->Push(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.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); + void MotionPlannerStage::DrawPIDValues(const boost::shared_ptr vehicle, + const float throttle, const float brake) { + + 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 b5094ab969..b09fb7d48e 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h @@ -38,11 +38,6 @@ namespace traffic_manager { /// Selection key to switch between the output frames. bool frame_selector; - /// Variables to remember messenger states. - int localization_messenger_state; - int control_messenger_state; - int collision_messenger_state; - int traffic_light_messenger_state; /// Pointers to data frames to be shared with the batch control stage std::shared_ptr control_frame_a; std::shared_ptr control_frame_b; diff --git a/LibCarla/source/carla/trafficmanager/PIDController.cpp b/LibCarla/source/carla/trafficmanager/PIDController.cpp index 67034d5b9d..5c7dd55b31 100644 --- a/LibCarla/source/carla/trafficmanager/PIDController.cpp +++ b/LibCarla/source/carla/trafficmanager/PIDController.cpp @@ -41,7 +41,10 @@ namespace PIDControllerConstants { // Calculating dt for 'D' and 'I' controller components. const chr::duration duration = current_state.time_instance - previous_state.time_instance; - const float dt = duration.count(); + (void) duration; //const float dt = duration.count(); Remove. + + // PID will be stable only over 20 FPS. + const float dt = 1/20.0f; // Calculating integrals. current_state.deviation_integral = angular_deviation * dt + previous_state.deviation_integral; @@ -86,7 +89,7 @@ namespace PIDControllerConstants { lateral_parameters[2] * (present_state.deviation - previous_state.deviation) / dt; - steer = std::max(-1.0f, std::min(steer, 1.0f)); + steer = std::max(-0.8f, std::min(steer, 0.8f)); return ActuationSignal{throttle, brake, steer}; } diff --git a/LibCarla/source/carla/trafficmanager/Parameters.cpp b/LibCarla/source/carla/trafficmanager/Parameters.cpp index ed327b28e9..7a22501f80 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.cpp +++ b/LibCarla/source/carla/trafficmanager/Parameters.cpp @@ -16,11 +16,13 @@ Parameters::Parameters() {} Parameters::~Parameters() {} void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { - percentage_difference_from_speed_limit.AddEntry({actor->GetId(), percentage}); + float new_percentage = std::min(100.0f,percentage); + percentage_difference_from_speed_limit.AddEntry({actor->GetId(), new_percentage}); } void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) { - global_percentage_difference_from_limit = percentage; + float new_percentage = std::min(100.0f,percentage); + global_percentage_difference_from_limit = new_percentage; } void Parameters::SetCollisionDetection( @@ -69,10 +71,10 @@ void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) { } 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); - } + + float new_distance = std::max(0.0f,distance); + const auto entry = std::make_pair(actor->GetId(), new_distance); + distance_to_leading_vehicle.AddEntry(entry); } float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) { @@ -142,17 +144,17 @@ float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) { } 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 new_perc = cg::Math::Clamp(perc,0.0f,100.0f); + const auto entry = std::make_pair(actor->GetId(), new_perc); + perc_run_traffic_light.AddEntry(entry); } void Parameters::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); - } + + float new_perc = cg::Math::Clamp(perc,0.0f,100.0f); + const auto entry = std::make_pair(actor->GetId(), new_perc); + perc_ignore_actors.AddEntry(entry); } float Parameters::GetPercentageRunningLight(const ActorPtr &actor) { diff --git a/LibCarla/source/carla/trafficmanager/Parameters.h b/LibCarla/source/carla/trafficmanager/Parameters.h index 82f93e00d4..b4e97ab92f 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.h +++ b/LibCarla/source/carla/trafficmanager/Parameters.h @@ -18,6 +18,7 @@ namespace carla { namespace traffic_manager { namespace cc = carla::client; + namespace cg = carla::geom; using ActorPtr = carla::SharedPtr; using ActorId = carla::ActorId; diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp index ce97e6f263..26aba983e0 100644 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp +++ b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp @@ -18,22 +18,29 @@ namespace traffic_manager { inter_update_duration = chr::duration(0); } - void PerformanceDiagnostics::RegisterUpdate() { + void PerformanceDiagnostics::RegisterUpdate(bool begin_or_end) { 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) { + if (begin_or_end) { + 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 { + //std::cout << "Stage: " + stage_name + ", throughput: " << throughput_counter + // << ", avg. cycle duration: " << 1000* inter_update_duration.count() + // << " ms" << std::endl; + + throughput_clock = current_time; + throughput_counter = 0u; + } else { - ++throughput_counter; + ++throughput_counter; + } + inter_update_clock = current_time; + } else { 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; } } diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h index 0f26668ea2..03018e8634 100644 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h +++ b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h @@ -8,6 +8,7 @@ #include #include +#include #include "carla/Logging.h" @@ -16,24 +17,72 @@ namespace traffic_manager { namespace chr = std::chrono; + using TimePoint = chr::time_point; + class PerformanceDiagnostics { private: /// Stage name. std::string stage_name; /// Throughput clock. - chr::time_point throughput_clock; + TimePoint throughput_clock; /// Throughput counter. uint64_t throughput_counter; /// Inter-update clock. - chr::time_point inter_update_clock; + TimePoint inter_update_clock; /// Inter-update duration. chr::duration inter_update_duration; public: PerformanceDiagnostics(std::string name); - void RegisterUpdate(); + void RegisterUpdate(bool begin_or_end); + }; + + class SnippetProfiler { + + private: + + std::unordered_map print_clocks; + std::unordered_map snippet_clocks; + std::unordered_map> snippet_durations; + + public: + + SnippetProfiler(){}; + + void MeasureExecutionTime(std::string snippet_name, bool begin_or_end) { + + if (print_clocks.find(snippet_name) == print_clocks.end()) { + print_clocks.insert({snippet_name, chr::system_clock::now()}); + } + if (snippet_clocks.find(snippet_name) == snippet_clocks.end()) { + snippet_clocks.insert({snippet_name, TimePoint()}); + } + if (snippet_durations.find(snippet_name) == snippet_durations.end()) { + snippet_durations.insert({snippet_name, chr::duration()}); + } + + TimePoint current_time = chr::system_clock::now(); + TimePoint& print_clock = print_clocks.at(snippet_name); + TimePoint& snippet_clock = snippet_clocks.at(snippet_name); + chr::duration& snippet_duration = snippet_durations.at(snippet_name); + + if (begin_or_end) { + snippet_clock = current_time; + } else { + chr::duration measured_duration = current_time - snippet_clock; + snippet_duration = (measured_duration + snippet_duration) / 2.0f; + } + + chr::duration print_duration = current_time - print_clock; + if (print_duration.count() > 1.0f) { + std::cout << "Snippet name : " << snippet_name << ", " + << "avg. duration : " << 1000 * snippet_duration.count() + << " ms" << std::endl; + print_clock = current_time; + } + } }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp b/LibCarla/source/carla/trafficmanager/PipelineStage.cpp index deef33c4df..87aeda5117 100644 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp +++ b/LibCarla/source/carla/trafficmanager/PipelineStage.cpp @@ -10,99 +10,42 @@ namespace carla { namespace traffic_manager { PipelineStage::PipelineStage(std::string stage_name) - : performance_diagnostics(PerformanceDiagnostics(stage_name)) { - - run_stage.store(true); - run_receiver.store(true); - run_action.store(false); - run_sender.store(false); + : stage_name(stage_name), + performance_diagnostics(PerformanceDiagnostics(stage_name)) { + run_stage.store(false); } - PipelineStage::~PipelineStage() {} + PipelineStage::~PipelineStage() { + worker_thread->join(); + worker_thread.release(); + } void PipelineStage::Start() { - - data_receiver = std::make_unique(&PipelineStage::ReceiverThreadManager, this); - action_thread = std::make_unique(&PipelineStage::ActionThreadManager, this); - data_sender = std::make_unique(&PipelineStage::SenderThreadManager, this); + run_stage.store(true); + worker_thread = std::make_unique(&PipelineStage::Update, this); } void PipelineStage::Stop() { - run_stage.store(false); - data_receiver->join(); - action_thread->join(); - data_sender->join(); } - void PipelineStage::ReceiverThreadManager() { - - while (run_stage.load()) { - std::unique_lock lock(thread_coordination_mutex); - // Wait for notification from sender thread and - // break waiting if the stage is stopped. - while (!run_receiver.load() && run_stage.load()) { - wake_receiver_notifier.wait_for(lock, 1ms, [=] {return run_receiver.load();}); - } - lock.unlock(); - run_receiver.store(false); - + void PipelineStage::Update() { + while (run_stage.load()){ // Receive data. - if (run_stage.load()) { - DataReceiver(); - } - - // Notify action thread. - run_action.store(true); - wake_action_notifier.notify_one(); - } - } + DataReceiver(); - void PipelineStage::ActionThreadManager() { - - while (run_stage.load()) { - performance_diagnostics.RegisterUpdate(); - - std::unique_lock lock(thread_coordination_mutex); - - // Wait for notification from receiver thread. - while (!run_action.load() && run_stage.load()) { - wake_action_notifier.wait_for(lock, 1ms, [=] {return run_action.load();}); - } - lock.unlock(); - run_action.store(false); - - // Run action. - if (run_stage.load()) { + // Receive data. + if(run_stage.load()){ + performance_diagnostics.RegisterUpdate(true); Action(); + performance_diagnostics.RegisterUpdate(false); } - // Notify sender. - run_sender.store(true); - wake_sender_notifier.notify_one(); - } - } - - void PipelineStage::SenderThreadManager() { - - while (run_stage.load()) { - std::unique_lock lock(thread_coordination_mutex); - - // Wait for notification from action thread. - while (!run_sender.load() && run_stage.load()) { - wake_sender_notifier.wait_for(lock, 1ms, [=] {return run_sender.load();}); - } - lock.unlock(); - run_sender.store(false); - - // Send data. - if (run_stage.load()) { + // Receive data. + if(run_stage.load()) { DataSender(); } - // Notify receiver. - run_receiver.store(true); - wake_receiver_notifier.notify_one(); } } diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.h b/LibCarla/source/carla/trafficmanager/PipelineStage.h index 4e278bde4a..d4f6ac62e3 100644 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.h +++ b/LibCarla/source/carla/trafficmanager/PipelineStage.h @@ -17,6 +17,7 @@ #include #include +#include "carla/Logging.h" #include "carla/rpc/ActorId.h" #include "carla/trafficmanager/Messenger.h" @@ -34,37 +35,17 @@ namespace traffic_manager { private: - /// Pointer to receiver thread instance. - std::unique_ptr data_receiver; - /// Pointer to sender thread instance. - std::unique_ptr data_sender; - /// Pointer to worker thread instance. - std::unique_ptr action_thread; - /// Flag to allow/block receiver. - std::atomic run_receiver; - /// Flag to allow/block sender. - std::atomic run_sender; - /// Flag to allow/block workers. - std::atomic run_action; + std::unique_ptr worker_thread; /// Flag to start/stop stage. std::atomic run_stage; - /// Mutex used to co-ordinate between receiver, workers, and sender. - std::mutex thread_coordination_mutex; - /// Variables to conditionally block receiver, workers, and sender. - std::condition_variable wake_receiver_notifier; - std::condition_variable wake_action_notifier; - std::condition_variable wake_sender_notifier; + /// Stage name string. + std::string stage_name; + + private: /// Object to track stage performance. PerformanceDiagnostics performance_diagnostics; - /// Method to manage receiver thread. - void ReceiverThreadManager(); - - /// Method to manage worker threads. - void ActionThreadManager(); - - /// Method to manage sender thread. - void SenderThreadManager(); + void Update(); protected: diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp index 0c6a118388..d3f18ae1cf 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp @@ -22,6 +22,10 @@ namespace traffic_manager { return next_waypoints; } + std::vector SimpleWaypoint::GetPreviousWaypoint() const { + return previous_waypoints; + } + WaypointPtr SimpleWaypoint::GetWaypoint() const { return waypoint; } @@ -50,7 +54,14 @@ namespace traffic_manager { for (auto &simple_waypoint: waypoints) { next_waypoints.push_back(simple_waypoint); } - return static_cast(waypoints.size()); + return static_cast(waypoints.size()); + } + + uint64_t SimpleWaypoint::SetPreviousWaypoint(const std::vector &waypoints) { + for (auto &simple_waypoint: waypoints) { + previous_waypoints.push_back(simple_waypoint); + } + return static_cast(waypoints.size()); } void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr _waypoint) { @@ -99,5 +110,23 @@ namespace traffic_manager { return (next_waypoints.size() > 1); } + void SimpleWaypoint::SetGeodesicGridId(GeoGridId _geodesic_grid_id) { + geodesic_grid_id = _geodesic_grid_id; + } + + GeoGridId SimpleWaypoint::GetGeodesicGridId() { + GeoGridId grid_id; + if (waypoint->IsJunction()) { + grid_id = waypoint->GetJunctionId(); + } else { + grid_id = geodesic_grid_id; + } + return grid_id; + } + + GeoGridId SimpleWaypoint::GetJunctionId() const { + return waypoint->GetJunctionId(); + } + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h index 1923cb40be..c46b745ae7 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h @@ -14,6 +14,7 @@ #include "carla/geom/Math.h" #include "carla/geom/Vector3D.h" #include "carla/Memory.h" +#include "carla/road/RoadTypes.h" namespace carla { namespace traffic_manager { @@ -21,6 +22,7 @@ namespace traffic_manager { namespace cc = carla::client; namespace cg = carla::geom; using WaypointPtr = carla::SharedPtr; + using GeoGridId = carla::road::JuncId; /// This is a simple wrapper class on Carla's waypoint object. /// The class is used to represent discrete samples of the world map. @@ -34,10 +36,14 @@ namespace traffic_manager { WaypointPtr waypoint; /// List of pointers to next connecting waypoints. std::vector next_waypoints; + /// List of pointers to previous connecting waypoints. + std::vector previous_waypoints; /// Pointer to left lane change waypoint. SimpleWaypointPtr next_left_waypoint; /// Pointer to right lane change waypoint. SimpleWaypointPtr next_right_waypoint; + /// Integer placing the waypoint into a geodesic grid. + GeoGridId geodesic_grid_id = 0; public: @@ -53,6 +59,9 @@ namespace traffic_manager { /// Returns the list of next waypoints. std::vector GetNextWaypoint() const; + /// Returns the list of previous waypoints. + std::vector GetPreviousWaypoint() const; + /// Returns the vector along the waypoint's direction. cg::Vector3D GetForwardVector() const; @@ -62,6 +71,9 @@ namespace traffic_manager { /// This method is used to set the next waypoints. uint64_t SetNextWaypoint(const std::vector &next_waypoints); + /// This method is used to set the previous waypoints. + uint64_t SetPreviousWaypoint(const std::vector &next_waypoints); + /// This method is used to set the closest left waypoint for a lane change. void SetLeftWaypoint(SimpleWaypointPtr waypoint); @@ -74,6 +86,13 @@ namespace traffic_manager { /// This method is used to get the closest right waypoint for a lane change. SimpleWaypointPtr GetRightWaypoint(); + /// Accessor methods for geodesic grid id. + void SetGeodesicGridId(GeoGridId _geodesic_grid_id); + GeoGridId GetGeodesicGridId(); + + /// Metod to retreive junction id of the waypoint. + GeoGridId GetJunctionId() const; + /// Calculates the distance from the object's waypoint to the passed /// location. float Distance(const cg::Location &location) const; diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index 9eace92a44..e909e4e0b2 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -28,13 +28,6 @@ namespace traffic_manager { // Initializing output frame selector. frame_selector = true; - // Initializing messenger state. - localization_messenger_state = localization_messenger->GetState(); - - // Initializing this messenger state to preemptively write - // since this stage precedes motion planner stage. - planner_messenger_state = planner_messenger->GetState() - 1; - // Initializing number of vehicles to zero in the beginning. number_of_vehicles = 0u; } @@ -46,7 +39,7 @@ namespace traffic_manager { // Selecting the output frame based on the selection key. const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; // Looping over registered actors. - for (uint64_t i = 0u; i < number_of_vehicles; ++i) { + for (uint64_t i = 0u; i < number_of_vehicles && localization_frame != nullptr; ++i) { bool traffic_light_hazard = false; const LocalizationToTrafficLightData &data = localization_frame->at(i); @@ -68,11 +61,8 @@ namespace traffic_manager { traffic_light_state = TLS::Green; // 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 - // is inside the junction and there is a red or yellow light. + // junction and there is a red or yellow light. if (!closest_waypoint->CheckJunction() && - look_ahead_point->CheckJunction() && ego_vehicle->IsAtTrafficLight() && traffic_light_state != TLS::Green) { @@ -151,15 +141,13 @@ namespace traffic_manager { } void TrafficLightStage::DataReceiver() { - const auto packet = localization_messenger->ReceiveData(localization_messenger_state); - localization_frame = packet.data; - localization_messenger_state = packet.id; + localization_frame = localization_messenger->Peek(); // Allocating new containers for the changed number of registered vehicles. if (localization_frame != nullptr && number_of_vehicles != (*localization_frame.get()).size()) { - number_of_vehicles = static_cast((*localization_frame.get()).size()); + number_of_vehicles = static_cast((*localization_frame.get()).size()); // Allocating output frames. planner_frame_a = std::make_shared(number_of_vehicles); planner_frame_b = std::make_shared(number_of_vehicles); @@ -168,12 +156,10 @@ namespace traffic_manager { void TrafficLightStage::DataSender() { - const DataPacket> packet{ - planner_messenger_state, - frame_selector ? planner_frame_a : planner_frame_b - }; + localization_messenger->Pop(); + + planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b); frame_selector = !frame_selector; - planner_messenger_state = planner_messenger->SendData(packet); } void TrafficLightStage::DrawLight(TLS traffic_light_state, const Actor &ego_actor) const { diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h index 0226598c9f..04ca10396a 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h @@ -44,9 +44,6 @@ namespace traffic_manager { private: - /// Variables to remember messenger states. - int localization_messenger_state; - int planner_messenger_state; /// Selection key to switch between output frames. bool frame_selector; /// Pointer data frame received from localization stage. diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp index afc4e62e04..1ee04cef4e 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp @@ -28,11 +28,9 @@ namespace traffic_manager { using WorldMap = carla::SharedPtr; const WorldMap world_map = world.GetMap(); - const auto dao = CarlaDataAccessLayer(world_map); - using Topology = std::vector>; - const Topology topology = dao.GetTopology(); - local_map = std::make_shared(topology); - local_map->SetUp(0.1f); + const RawNodeList raw_dense_topology = world_map->GenerateWaypoints(0.1f); + local_map = std::make_shared(raw_dense_topology); + local_map->SetUp(); parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit); @@ -48,12 +46,13 @@ namespace traffic_manager { localization_planner_messenger, localization_collision_messenger, localization_traffic_light_messenger, registered_actors, *local_map.get(), - parameters, debug_helper); + parameters, debug_helper, + world); collision_stage = std::make_unique( "Collision stage", localization_collision_messenger, collision_planner_messenger, - world, parameters, debug_helper); + parameters, debug_helper); traffic_light_stage = std::make_unique( "Traffic light stage", @@ -81,6 +80,7 @@ namespace traffic_manager { } TrafficManager::~TrafficManager() { + Stop(); } @@ -90,10 +90,10 @@ namespace traffic_manager { if (singleton_pointer == nullptr) { - 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 std::vector longitudinal_param = {2.0f, 0.05f, 0.07f}; + const std::vector longitudinal_highway_param = {4.0f, 0.02f, 0.03f}; + const std::vector lateral_param = {10.0f, 0.02f, 1.0f}; + const std::vector lateral_highway_param = {9.0f, 0.02f, 1.0f}; const float perc_difference_from_limit = 30.0f; TrafficManager* tm_ptr = new TrafficManager( @@ -127,10 +127,6 @@ namespace traffic_manager { registered_actors.Remove(actor_list); } - void TrafficManager::DestroyVehicle(const ActorPtr &actor) { - registered_actors.Destroy(actor); - } - void TrafficManager::Start() { localization_collision_messenger->Start(); @@ -161,6 +157,7 @@ namespace traffic_manager { traffic_light_stage->Stop(); planner_stage->Stop(); control_stage->Stop(); + } void TrafficManager::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.h b/LibCarla/source/carla/trafficmanager/TrafficManager.h index 8fdf0793b6..486e8ec39b 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.h @@ -23,7 +23,6 @@ #include "carla/trafficmanager/AtomicActorSet.h" #include "carla/trafficmanager/AtomicMap.h" #include "carla/trafficmanager/BatchControlStage.h" -#include "carla/trafficmanager/CarlaDataAccessLayer.h" #include "carla/trafficmanager/CollisionStage.h" #include "carla/trafficmanager/InMemoryMap.h" #include "carla/trafficmanager/LocalizationStage.h" @@ -110,8 +109,8 @@ namespace traffic_manager { /// This method unregisters a vehicle from traffic manager. void UnregisterVehicles(const std::vector &actor_list); - /// This method kills a vehicle. - void DestroyVehicle(const ActorPtr &actor); + /// This method kills a vehicle. (Not working right now) + /// void DestroyVehicle(const ActorPtr &actor); /// Set target velocity specific to a vehicle. void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); @@ -148,6 +147,9 @@ namespace traffic_manager { /// Method to reset all traffic lights. void ResetAllTrafficLights(); + /// Return the world object + const cc::World &GetWorld() { return world; }; + /// Destructor. ~TrafficManager(); diff --git a/LibCarla/source/carla/trafficmanager/VicinityGrid.cpp b/LibCarla/source/carla/trafficmanager/VicinityGrid.cpp deleted file mode 100644 index 16a80eadea..0000000000 --- a/LibCarla/source/carla/trafficmanager/VicinityGrid.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// 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 carla { -namespace traffic_manager { - - static const float GRID_SIZE = 10.0f; - - VicinityGrid::VicinityGrid() {} - - VicinityGrid::~VicinityGrid() {} - - std::string VicinityGrid::MakeKey(std::pair grid_ids) { - return std::to_string(grid_ids.first) + std::to_string(grid_ids.second); - } - - std::pair VicinityGrid::UpdateGrid(Actor actor) { - - 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)); - - 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()) { - - 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) { - std::unique_lock lock(modification_mutex); - - // Update the actor's grid id. - actor_to_grid_id.at(actor_id) = new_grid_id; - - // Remove the actor from the old grid position and update the new position. - grid_to_actor_id.at(old_grid_id).erase(actor_id); - if (grid_to_actor_id.find(new_grid_id) != grid_to_actor_id.end()) { - grid_to_actor_id.at(new_grid_id).insert(actor_id); - } else { - grid_to_actor_id.insert({new_grid_id, {actor_id}}); - } - } - } - // If the actor is new, then add entries to map. - else { - - std::unique_lock lock(modification_mutex); - actor_to_grid_id.insert({actor_id, new_grid_id}); - if (grid_to_actor_id.find(new_grid_id) != grid_to_actor_id.end()) { - grid_to_actor_id.at(new_grid_id).insert(actor_id); - } else { - grid_to_actor_id.insert({new_grid_id, {actor_id}}); - } - } - - // Return updated grid position. - return {first, second}; - } - - std::unordered_set VicinityGrid::GetActors(Actor actor) { - - const std::pair grid_ids = UpdateGrid(actor); - - std::shared_lock lock(modification_mutex); - std::unordered_set actors; - - // Search all surrounding grids and find any vehicles in them. - for (int i = -1; i <= 1; ++i) { - for (int j = -1; j <= 1; ++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()) { - const auto &grid_actor_set = grid_to_actor_id.at(grid_key); - actors.insert(grid_actor_set.begin(), grid_actor_set.end()); - } - } - } - - return actors; - } - - void VicinityGrid::EraseActor(ActorId 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 -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/VicinityGrid.h b/LibCarla/source/carla/trafficmanager/VicinityGrid.h deleted file mode 100644 index 9d532cef76..0000000000 --- a/LibCarla/source/carla/trafficmanager/VicinityGrid.h +++ /dev/null @@ -1,67 +0,0 @@ -// 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 "carla/client/Actor.h" -#include "carla/geom/Location.h" -#include "carla/rpc/Actor.h" - -#include "carla/trafficmanager/SimpleWaypoint.h" - -namespace carla { -namespace traffic_manager { - - namespace cc = carla::client; - namespace cg = carla::geom; - - using ActorId = carla::ActorId; - using Actor = carla::SharedPtr; - using SimpleWaypointPtr = std::shared_ptr; - using Buffer = std::deque; - - /// This class maintains vehicle positions in grid segments. - /// This is used in the collision stage to filter vehicles. - class VicinityGrid { - - private: - - /// Mutex to manage contention between modifiers and readers. - std::shared_timed_mutex modification_mutex; - /// Map connecting grid id to set of actor ids. - std::unordered_map> grid_to_actor_id; - /// Map connecting actor id to grid key. - std::unordered_map actor_to_grid_id; - - /// Key generator for a given grid. - std::string MakeKey(std::pair grid_ids); - - public: - - VicinityGrid(); - ~VicinityGrid(); - - /// Returns a set of actors in the vicinity of a given actor. - std::unordered_set GetActors(Actor actor); - - /// Updates the grid position of the given actor and returns new grid id. - std::pair UpdateGrid(Actor actor); - - /// Removes actor. - void EraseActor(ActorId actor_id); - - }; - -} // namespace traffic_manager -} // namespace carla diff --git a/PythonAPI/carla/source/libcarla/Client.cpp b/PythonAPI/carla/source/libcarla/Client.cpp index 547b598057..a63fbd8948 100644 --- a/PythonAPI/carla/source/libcarla/Client.cpp +++ b/PythonAPI/carla/source/libcarla/Client.cpp @@ -4,9 +4,12 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -#include -#include -#include +#include "carla/PythonUtil.h" +#include "carla/client/Client.h" +#include "carla/client/World.h" +#include "carla/Logging.h" +#include "carla/rpc/ActorId.h" +#include "carla/trafficmanager/TrafficManager.h" #include @@ -43,9 +46,69 @@ static auto ApplyBatchCommandsSync( boost::python::stl_input_iterator(commands), boost::python::stl_input_iterator()}; boost::python::list result; - for (auto &response : self.ApplyBatchSync(std::move(cmds), do_tick)) { + auto responses = self.ApplyBatchSync(cmds, do_tick); + for (auto &response : responses) { result.append(std::move(response)); } + // check for autopilot command + carla::traffic_manager::TrafficManager *tm = nullptr; + std::vector vehicles_to_enable; + std::vector vehicles_to_disable; + for (size_t i=0; i(cmds[i].command); + for (auto &cmd : spawn.do_after) { + if (cmd.command.type() == typeid(carla::rpc::Command::SetAutopilot)) { + autopilotValue = boost::get(cmd.command).enabled; + isAutopilot = true; + } + } + } + + // check SetAutopilot command + if (cmds[i].command.type() == typeid(carla::rpc::Command::SetAutopilot)) { + autopilotValue = boost::get(cmds[i].command).enabled; + isAutopilot = true; + } + + // check if found any SetAutopilot command + if (isAutopilot) { + // get the id + carla::rpc::ActorId id = static_cast(responses[i].Get()); + // get traffic manager instance + if (!tm) { + tm = &carla::traffic_manager::TrafficManager::GetInstance( + carla::traffic_manager::TrafficManager::GetUniqueLocalClient()); + } + // get all actors + carla::SharedPtr actor; + if (tm) { + actor = tm->GetWorld().GetActor(id); + } + // check to enable or disable + if (actor) { + if (autopilotValue) { + vehicles_to_enable.push_back(actor); + } else { + vehicles_to_disable.push_back(actor); + } + } + } + } + } + // check if any autopilot command was sent + if ((vehicles_to_enable.size() || vehicles_to_disable.size()) && tm) { + tm->RegisterVehicles(vehicles_to_enable); + tm->UnregisterVehicles(vehicles_to_disable); + } + return result; } diff --git a/PythonAPI/carla/source/libcarla/TrafficManager.cpp b/PythonAPI/carla/source/libcarla/TrafficManager.cpp index 24fd0fb363..ee8ceebcc7 100644 --- a/PythonAPI/carla/source/libcarla/TrafficManager.cpp +++ b/PythonAPI/carla/source/libcarla/TrafficManager.cpp @@ -23,18 +23,17 @@ void export_trafficmanager() { class_("TM_Parameters").def(vector_indexing_suite()); class_("TrafficManager", no_init) - .def("register_vehicles", &carla::traffic_manager::TrafficManager::RegisterVehicles) - .def("unregister_vehicles", &carla::traffic_manager::TrafficManager::UnregisterVehicles) - .def("set_vehicle_max_speed_difference", &carla::traffic_manager::TrafficManager::SetPercentageSpeedDifference) - .def("set_global_max_speed_difference", &carla::traffic_manager::TrafficManager::SetGlobalPercentageSpeedDifference) - .def("set_collision_detection", &carla::traffic_manager::TrafficManager::SetCollisionDetection) - .def("force_lane_change", &carla::traffic_manager::TrafficManager::SetForceLaneChange) - .def("set_auto_lane_change", &carla::traffic_manager::TrafficManager::SetAutoLaneChange) - .def("set_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetDistanceToLeadingVehicle) - .def("reset_traffic_lights", &carla::traffic_manager::TrafficManager::ResetAllTrafficLights) - .def("destroy_vehicle", &carla::traffic_manager::TrafficManager::DestroyVehicle) - .def("ignore_actors_percentage", &carla::traffic_manager::TrafficManager::SetPercentageIgnoreActors) - .def("ignore_lights_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningLight); + .def("register_vehicles", &carla::traffic_manager::TrafficManager::RegisterVehicles) + .def("unregister_vehicles", &carla::traffic_manager::TrafficManager::UnregisterVehicles) + .def("set_vehicle_max_speed_difference", &carla::traffic_manager::TrafficManager::SetPercentageSpeedDifference) + .def("set_global_max_speed_difference", &carla::traffic_manager::TrafficManager::SetGlobalPercentageSpeedDifference) + .def("set_collision_detection", &carla::traffic_manager::TrafficManager::SetCollisionDetection) + .def("force_lane_change", &carla::traffic_manager::TrafficManager::SetForceLaneChange) + .def("set_auto_lane_change", &carla::traffic_manager::TrafficManager::SetAutoLaneChange) + .def("set_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetDistanceToLeadingVehicle) + .def("reset_traffic_lights", &carla::traffic_manager::TrafficManager::ResetAllTrafficLights) + .def("ignore_actors_percentage", &carla::traffic_manager::TrafficManager::SetPercentageIgnoreActors) + .def("ignore_lights_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningLight); def("GetTrafficManager", &carla::traffic_manager::TrafficManager::GetInstance, return_value_policy()); diff --git a/PythonAPI/examples/tm_spawn_npc.py b/PythonAPI/examples/tm_spawn_npc.py index d219f53265..05726dde87 100644 --- a/PythonAPI/examples/tm_spawn_npc.py +++ b/PythonAPI/examples/tm_spawn_npc.py @@ -76,7 +76,6 @@ def main(): client.set_timeout(2.0) try: - traffic_manager = None world = client.get_world() blueprints = world.get_blueprint_library().filter(args.filterv) blueprintsWalkers = world.get_blueprint_library().filter(args.filterw) @@ -193,7 +192,6 @@ def main(): print('Spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) - traffic_manager = carla.GetTrafficManager(client) time.sleep(1) for v in vehicles_list: @@ -203,8 +201,6 @@ def main(): time.sleep(1) finally: - if traffic_manager: - del traffic_manager print('Destroying %d vehicles.\n' % len(vehicles_list)) client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp index 5461012c5c..659a181848 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp @@ -161,6 +161,19 @@ void ATrafficLightBase::NotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle) } } +void ATrafficLightBase::UnNotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle) +{ + if (IsValid(Vehicle)) + { + auto Controller = Cast(Vehicle->GetController()); + if (Controller != nullptr) + { + Controller->SetTrafficLight(nullptr); + Controller->SetTrafficLightState(ETrafficLightState::Green); + } + } +} + void ATrafficLightBase::SetGreenTime(float InGreenTime) { GreenTime = InGreenTime; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h index 697b7c3d99..fb9dc0669c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h @@ -54,6 +54,9 @@ class CARLA_API ATrafficLightBase : public ATrafficSignBase UFUNCTION(Category = "Traffic Light", BlueprintCallable) void NotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle); + UFUNCTION(Category = "Traffic Light", BlueprintCallable) + void UnNotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle); + UFUNCTION(Category = "Traffic Light", BlueprintCallable) void SetGreenTime(float InGreenTime); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp index 2bebae9369..bff4bbc9c2 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp @@ -135,11 +135,7 @@ void AWheeledVehicleAIController::Tick(const float DeltaTime) return; } - if (bAutopilotEnabled) - { - Vehicle->ApplyVehicleControl(TickAutopilotController(), EVehicleInputPriority::Autopilot); - } - else if (!bControlIsSticky) + if (!bAutopilotEnabled && !bControlIsSticky) { Vehicle->ApplyVehicleControl(FVehicleControl{}, EVehicleInputPriority::Relaxation); } @@ -170,29 +166,6 @@ void AWheeledVehicleAIController::ConfigureAutopilot(const bool Enable, const bo } TrafficLightState = ETrafficLightState::Green; - - /// @todo Workaround for a race condition between client and server when - /// enabling autopilot right after initializing a vehicle. - if (bAutopilotEnabled) - { - for (TActorIterator It(GetWorld()); It; ++It) - { - ARoutePlanner *RoutePlanner = *It; - // Check if we are inside this route planner. - TSet OverlappingActors; - RoutePlanner->TriggerVolume->GetOverlappingActors( - OverlappingActors, - ACarlaWheeledVehicle::StaticClass()); - for (auto *Actor : OverlappingActors) - { - if (Actor == Vehicle) - { - RoutePlanner->AssignRandomRoute(*this); - return; - } - } - } - } } // ============================================================================= @@ -212,292 +185,3 @@ void AWheeledVehicleAIController::SetFixedRoute( TargetLocations.emplace(Location); } } - -// ============================================================================= -// -- AI ----------------------------------------------------------------------- -// ============================================================================= - -FVehicleControl AWheeledVehicleAIController::TickAutopilotController() -{ -#if WITH_EDITOR // This happens in simulation mode in editor. - if (Vehicle == nullptr) - { - bAutopilotEnabled = false; - return {}; - } -#endif // WITH_EDITOR - - check(Vehicle != nullptr); - - FVector Direction; - - float Steering; - if (!TargetLocations.empty()) - { - Steering = GoToNextTargetLocation(Direction); - } - else - { - Steering = RoadMap != nullptr ? CalcStreeringValue(Direction) : 0.0f; - Direction = Vehicle->GetVehicleTransform().GetRotation().Rotator().Vector(); - } - - // Speed in km/h. - const auto Speed = Vehicle->GetVehicleForwardSpeed() * 0.036f; - - float Throttle; - if (TrafficLightState != ETrafficLightState::Green) - { - Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::WaitingForRedLight); - Throttle = Stop(Speed); - } - else if (IsThereAnObstacleAhead(*Vehicle, Speed, Direction)) - { - Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::ObstacleAhead); - Throttle = Stop(Speed); - } - else - { - Throttle = Move(Speed); - } - - FVehicleControl AutopilotControl; - - if (Throttle < 0.001f) - { - AutopilotControl.Brake = 1.0f; - AutopilotControl.Throttle = 0.0f; - } - else - { - AutopilotControl.Brake = 0.0f; - AutopilotControl.Throttle = Throttle; - } - AutopilotControl.Steer = Steering; - - return AutopilotControl; -} - -float AWheeledVehicleAIController::GoToNextTargetLocation(FVector &Direction) -{ - // Get middle point between the two front wheels. - const auto CurrentLocation = [&]() { - const auto &Wheels = Vehicle->GetVehicleMovementComponent()->Wheels; - check((Wheels.Num() > 1) && (Wheels[0u] != nullptr) && (Wheels[1u] != nullptr)); - return (Wheels[0u]->Location + Wheels[1u]->Location) / 2.0f; - } (); - - const auto Target = [&]() { - const auto &Result = TargetLocations.front(); - return FVector{Result.X, Result.Y, CurrentLocation.Z}; - } (); - - if (Target.Equals(CurrentLocation, 200.0f)) - { - TargetLocations.pop(); - if (!TargetLocations.empty()) - { - return GoToNextTargetLocation(Direction); - } - else - { - return RoadMap != nullptr ? CalcStreeringValue(Direction) : 0.0f; - } - } - - Direction = (Target - CurrentLocation).GetSafeNormal(); - - const FVector &Forward = GetPawn()->GetActorForwardVector(); - - float dirAngle = Direction.UnitCartesianToSpherical().Y; - float actorAngle = Forward.UnitCartesianToSpherical().Y; - - dirAngle *= (180.0f / PI); - actorAngle *= (180.0 / PI); - - float angle = dirAngle - actorAngle; - - if (angle > 180.0f) - { - angle -= 360.0f; - } - else if (angle < -180.0f) - { - angle += 360.0f; - } - - float Steering = 0.0f; - if (angle < -MaximumSteerAngle) - { - Steering = -1.0f; - } - else if (angle > MaximumSteerAngle) - { - Steering = 1.0f; - } - else - { - Steering += angle / MaximumSteerAngle; - } - - Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::FollowingFixedRoute); - return Steering; -} - -float AWheeledVehicleAIController::CalcStreeringValue(FVector &direction) -{ - float steering = 0; - FVector BoxExtent = Vehicle->GetVehicleBoundingBoxExtent(); - FVector forward = Vehicle->GetActorForwardVector(); - - FVector rightSensorPosition(BoxExtent.X / 2.0f, (BoxExtent.Y / 2.0f) + 100.0f, 0.0f); - FVector leftSensorPosition(BoxExtent.X / 2.0f, -(BoxExtent.Y / 2.0f) - 100.0f, 0.0f); - - float forwardMagnitude = BoxExtent.X / 2.0f; - - float Magnitude = - (float) sqrt(pow((double) leftSensorPosition.X, 2.0) + pow((double) leftSensorPosition.Y, 2.0)); - - // same for the right and left - float offset = FGenericPlatformMath::Acos(forwardMagnitude / Magnitude); - - float actorAngle = forward.UnitCartesianToSpherical().Y; - - float sinR = FGenericPlatformMath::Sin(actorAngle + offset); - float cosR = FGenericPlatformMath::Cos(actorAngle + offset); - - float sinL = FGenericPlatformMath::Sin(actorAngle - offset); - float cosL = FGenericPlatformMath::Cos(actorAngle - offset); - - rightSensorPosition.Y = sinR * Magnitude; - rightSensorPosition.X = cosR * Magnitude; - - leftSensorPosition.Y = sinL * Magnitude; - leftSensorPosition.X = cosL * Magnitude; - - FVector rightPositon = GetPawn()->GetActorLocation() + FVector(rightSensorPosition.X, - rightSensorPosition.Y, - 0.0f); - FVector leftPosition = GetPawn()->GetActorLocation() + FVector(leftSensorPosition.X, - leftSensorPosition.Y, - 0.0f); - - FRoadMapPixelData rightRoadData = RoadMap->GetDataAt(rightPositon); - if (!rightRoadData.IsRoad()) - { - steering -= 0.2f; - } - - FRoadMapPixelData leftRoadData = RoadMap->GetDataAt(leftPosition); - if (!leftRoadData.IsRoad()) - { - steering += 0.2f; - } - - FRoadMapPixelData roadData = RoadMap->GetDataAt(GetPawn()->GetActorLocation()); - if (!roadData.IsRoad()) - { - steering = 0.0f; - } - else if (roadData.HasDirection()) - { - - direction = roadData.GetDirection(); - FVector right = rightRoadData.GetDirection(); - FVector left = leftRoadData.GetDirection(); - - forward.Z = 0.0f; - - float dirAngle = direction.UnitCartesianToSpherical().Y; - float rightAngle = right.UnitCartesianToSpherical().Y; - float leftAngle = left.UnitCartesianToSpherical().Y; - - dirAngle *= (180.0f / PI); - rightAngle *= (180.0 / PI); - leftAngle *= (180.0 / PI); - actorAngle *= (180.0 / PI); - - float min = dirAngle - 90.0f; - if (min < -180.0f) - { - min = 180.0f + (min + 180.0f); - } - - float max = dirAngle + 90.0f; - if (max > 180.0f) - { - max = -180.0f + (max - 180.0f); - } - - if (dirAngle < -90.0 || dirAngle > 90.0) - { - if (rightAngle < min && rightAngle > max) - { - steering -= 0.2f; - } - if (leftAngle < min && leftAngle > max) - { - steering += 0.2f; - } - } - else - { - if (rightAngle < min || rightAngle > max) - { - steering -= 0.2f; - } - if (leftAngle < min || leftAngle > max) - { - steering += 0.2f; - } - } - - float angle = dirAngle - actorAngle; - - if (angle > 180.0f) - { - angle -= 360.0f; - } - else if (angle < -180.0f) - { - angle += 360.0f; - } - - if (angle < -MaximumSteerAngle) - { - steering = -1.0f; - } - else if (angle > MaximumSteerAngle) - { - steering = 1.0f; - } - else - { - steering += angle / MaximumSteerAngle; - } - } - - Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::FreeDriving); - return steering; -} - -float AWheeledVehicleAIController::Stop(const float Speed) -{ - return (Speed >= 1.0f ? -Speed / SpeedLimit : 0.0f); -} - -float AWheeledVehicleAIController::Move(const float Speed) -{ - if (Speed >= SpeedLimit) - { - return Stop(Speed); - } - else if (Speed >= SpeedLimit - 10.0f) - { - return 0.5f; - } - else - { - return 1.0f; - } -} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h index 7c7333445f..26f1112cf8 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h @@ -210,28 +210,6 @@ class CARLA_API AWheeledVehicleAIController final : public AController /// @} -private: - - FVehicleControl TickAutopilotController(); - - /// Returns steering value. - float GoToNextTargetLocation(FVector &Direction); - - /// Returns steering value. - float CalcStreeringValue(FVector &Direction); - - /// Returns throttle value. - float Stop(float Speed); - - /// Returns throttle value. - float Move(float Speed); - - /// @} - // =========================================================================== - // -- Member variables ------------------------------------------------------- - // =========================================================================== - /// @{ - private: UPROPERTY() diff --git a/Util/ContentVersions.txt b/Util/ContentVersions.txt index 2dffa87eb0..a2e54b97ff 100644 --- a/Util/ContentVersions.txt +++ b/Util/ContentVersions.txt @@ -29,4 +29,4 @@ 0.9.5: 20190404_c7b464a 0.9.6: 20190710_0097e66 -0.9.7: 20191211_f63ea8b +0.9.7: 20191221_c88604b