From 151e38ce8d5d7793a33696b0e49d28478c2f93f9 Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com> Date: Mon, 30 Mar 2020 22:53:29 +0200 Subject: [PATCH] Hybrid mode for Traffic Manager (#2674) * Improved braking, collision negotiation. * Improved braking algorithm for smoother approach to lead vehicle. * Implemented smoother path boundary modification to aid smoother braking. * Re-worked collision negotiation algorithm. * Improved collision candidate filtering. * Added safe-guard in case of vehicle removal in collision stage. * Used local variable for heavily referenced object in localization stage. * Implemented vector relative velocities for motion planner's collision consideration. * Moved collision candidate sorting logic from collision stage to localization stage. * Sorting collision candidates using their ids instead of shared pointers to avoid memory corruption. * Improved conditions for collision consideration for greater efficiency. * Removed fps limit in async mode. * Hybrid physics mode * Introduced hybrid physics mode parameter * Implemented physics independent velocity computation * Modified localization stage to be physics agnostic * Fixing velocity compute interval in sync and async mode. Made motion planner stage work with internally computed velocities. * Made collision stage agnostic to actor physics * Sampling waypoint buffer for teleportation window * WIP: Teleportation changes * WIP2: Teleportation changes * Fixes waypoint window and vehicle spawning * hotfix to performance benchmark * comment out debugs * changelog * fixes collision bug * fixes package error and out_of_range bug * changes after review * Left & Right Transit of a lane: Waypoint mapping Co-authored-by: Praveen Kumar Co-authored-by: Soumyadeep --- CHANGELOG.md | 1 + .../trafficmanager/BatchControlStage.cpp | 15 +- .../carla/trafficmanager/CollisionStage.cpp | 98 ++++++---- .../carla/trafficmanager/CollisionStage.h | 39 ++-- .../carla/trafficmanager/InMemoryMap.cpp | 92 +++++++--- .../trafficmanager/LocalizationStage.cpp | 172 ++++++++++++++++-- .../carla/trafficmanager/LocalizationStage.h | 25 ++- .../trafficmanager/LocalizationUtils.cpp | 31 ++-- .../trafficmanager/MessengerAndDataTypes.h | 11 +- .../trafficmanager/MotionPlannerStage.cpp | 129 +++++++++++-- .../carla/trafficmanager/MotionPlannerStage.h | 8 +- .../carla/trafficmanager/Parameters.cpp | 10 + .../source/carla/trafficmanager/Parameters.h | 8 + .../carla/trafficmanager/SimpleWaypoint.cpp | 12 +- .../carla/trafficmanager/SimpleWaypoint.h | 7 +- .../carla/trafficmanager/TrafficManager.h | 8 + .../carla/trafficmanager/TrafficManagerBase.h | 3 + .../trafficmanager/TrafficManagerClient.h | 6 + .../trafficmanager/TrafficManagerLocal.cpp | 4 + .../trafficmanager/TrafficManagerLocal.h | 3 + .../trafficmanager/TrafficManagerRemote.cpp | 4 + .../trafficmanager/TrafficManagerRemote.h | 3 + .../trafficmanager/TrafficManagerServer.h | 5 + .../carla/source/libcarla/TrafficManager.cpp | 3 +- PythonAPI/examples/spawn_npc.py | 10 +- PythonAPI/util/performance_benchmark.py | 2 +- 26 files changed, 565 insertions(+), 144 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 34e2f1e0c..416447beb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,6 @@ ## Latest + * Introduced hybrid mode for Traffic Manager * Upgraded to Unreal Engine 4.24 * Fixed autonomous agents' incorrect detection of red traffic lights affecting them * Added walkable pedestrian crosswalks in OpenDRIVE standalone mode diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp index 687c555ca..fdb817ed9 100644 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp +++ b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp @@ -38,11 +38,18 @@ void BatchControlStage::Action() { } const carla::ActorId actor_id = element.actor->GetId(); - vehicle_control.throttle = element.throttle; - vehicle_control.brake = element.brake; - vehicle_control.steer = element.steer; + // Apply actuation from controller if physics enabled. + if (element.physics_enabled) { + vehicle_control.throttle = element.throttle; + vehicle_control.brake = element.brake; + vehicle_control.steer = element.steer; - commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control); + commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control); + } + // Apply target transform for physics-less vehicles. + else { + commands->at(i) = carla::rpc::Command::ApplyTransform(actor_id, element.transform); + } } } diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 2aeb091d3..68b6e8a43 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -72,14 +72,16 @@ namespace CollisionStageConstants { const Actor ego_actor = data.actor; const ActorId ego_actor_id = ego_actor->GetId(); const cg::Location ego_location = ego_actor->GetLocation(); + const cg::Vector3D ego_velocity = data.velocity; const SimpleWaypointPtr& closest_point = data.closest_waypoint; const SimpleWaypointPtr& junction_look_ahead = data.junction_look_ahead_waypoint; - const std::vector>& collision_candidates = data.overlapping_actors; + using OverlappingActorInfo = std::vector>; + const OverlappingActorInfo &collision_candidates = data.overlapping_actors; bool collision_hazard = false; float available_distance_margin = std::numeric_limits::infinity(); - cg::Vector3D other_vehicle_velocity; + cg::Vector3D obstacle_velocity; const SimpleWaypointPtr safe_point_junction = data.safe_point_after_junction; try { @@ -88,14 +90,17 @@ namespace CollisionStageConstants { actor_info != collision_candidates.end() && !collision_hazard; ++actor_info) { - const ActorId other_actor_id = actor_info->first; - const Actor other_actor = actor_info->second; + ActorId other_actor_id; + Actor other_actor; + cg::Vector3D other_velocity; + std::tie(other_actor_id, other_actor, other_velocity) = *actor_info; + if (!other_actor->IsAlive()) continue; const auto other_actor_type = other_actor->GetTypeId(); const cg::Location other_location = other_actor->GetLocation(); // Collision checks increase with speed - float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f); + float collision_distance = std::pow(floor(ego_velocity.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 @@ -109,19 +114,20 @@ namespace CollisionStageConstants { if (parameters.GetCollisionDetection(ego_actor, other_actor)) { std::pair negotiation_result = NegotiateCollision(ego_actor, other_actor, ego_location, - other_location, closest_point, junction_look_ahead); + other_location, closest_point, junction_look_ahead, + ego_velocity, other_velocity); if ((safe_point_junction != nullptr - && !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction, other_location)) + && !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction, + other_location, other_velocity)) || negotiation_result.first) { if ((other_actor_type[0] == 'v' && parameters.GetPercentageIgnoreVehicles(ego_actor) <= (rand() % 101)) || (other_actor_type[0] == 'w' && parameters.GetPercentageIgnoreWalkers(ego_actor) <= (rand() % 101))) { - collision_hazard = true; available_distance_margin = negotiation_result.second; - other_vehicle_velocity = other_actor->GetVelocity(); + obstacle_velocity = other_velocity; } } } @@ -136,7 +142,7 @@ namespace CollisionStageConstants { CollisionToPlannerData &message = current_planner_frame->at(i); message.hazard = collision_hazard; message.distance_to_other_vehicle = available_distance_margin; - message.other_vehicle_velocity = other_vehicle_velocity; + message.other_vehicle_velocity = obstacle_velocity; } } @@ -185,12 +191,19 @@ namespace CollisionStageConstants { const cg::Location &reference_location, const cg::Location &other_location, const SimpleWaypointPtr &closest_point, - const SimpleWaypointPtr &junction_look_ahead) { + const SimpleWaypointPtr &junction_look_ahead, + const cg::Vector3D reference_velocity, + const cg::Vector3D other_velocity) { + + // Vehicle IDs + const ActorId reference_vehicle_id = reference_vehicle->GetId(); + const ActorId other_vehicle_id = other_vehicle->GetId(); + // Output variables for the method. bool hazard = false; float available_distance_margin = std::numeric_limits::infinity(); - // Ego vehicle heading. + // Ego and other vehicle heading. const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector(); // Vector from ego position to position of the other vehicle. cg::Vector3D reference_to_other = other_location - reference_location; @@ -209,8 +222,9 @@ namespace CollisionStageConstants { float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * SQUARE_ROOT_OF_TWO; float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location); - float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle); - float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle); + float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id, + reference_velocity, reference_heading); + float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle_id, other_velocity, other_heading); // Calculate minimum distance between vehicle to consider collision negotiation. float inter_vehicle_length = reference_vehicle_length + other_vehicle_length; float ego_detection_range = std::pow(ego_bounding_box_extension + inter_vehicle_length, 2.0f); @@ -226,14 +240,14 @@ namespace CollisionStageConstants { bool ego_stopped_by_light = reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green; bool ego_at_junction_entrance = !closest_point->CheckJunction() && junction_look_ahead->CheckJunction(); - const ActorId reference_vehicle_id = reference_vehicle->GetId(); // Conditions to consider collision negotiation. if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light) && ((ego_inside_junction && other_vehicles_in_cross_detection_range) || (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) { GeometryComparisonCache cache = GetGeometryBetweenActors(reference_vehicle, other_vehicle, - reference_location, other_location); + reference_location, other_location, + reference_velocity, other_velocity); // Conditions for collision negotiation. bool geodesic_path_bbox_touching = cache.inter_geodesic_distance < 0.1; @@ -256,7 +270,6 @@ namespace CollisionStageConstants { available_distance_margin = static_cast(MAX(cache.reference_vehicle_to_other_geodesic - specific_distance_margin, 0.0)); - ActorId other_vehicle_id = other_vehicle->GetId(); ///////////////////////////////////// Collision locking mechanism ///////////////////////////////// // The idea is, when encountering a possible collision, // we should ensure that the bounding box extension doesn't decrease too fast and loose collision tracking. @@ -306,23 +319,27 @@ namespace CollisionStageConstants { return boundary_polygon; } - LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location) { - if (geodesic_boundaries.find(actor->GetId()) != geodesic_boundaries.end()) { - return geodesic_boundaries.at(actor->GetId()); + LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location, + const cg::Vector3D velocity) { + + const ActorId actor_id = actor->GetId(); + + if (geodesic_boundaries.find(actor_id) != geodesic_boundaries.end()) { + return geodesic_boundaries.at(actor_id); } - const LocationList bbox = GetBoundary(actor, vehicle_location); + const LocationList bbox = GetBoundary(actor, vehicle_location, velocity); - if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) { + if (vehicle_id_to_index.find(actor_id) != vehicle_id_to_index.end()) { - float bbox_extension = GetBoundingBoxExtention(actor); + float bbox_extension = GetBoundingBoxExtention(actor_id, velocity, actor->GetTransform().GetForwardVector()); const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor); if (specific_distance_margin > 0.0f) { bbox_extension = MAX(specific_distance_margin, bbox_extension); } - const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer; + const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor_id)).buffer; LocationList left_boundary; LocationList right_boundary; @@ -393,17 +410,17 @@ namespace CollisionStageConstants { } - float CollisionStage::GetBoundingBoxExtention(const Actor &actor) { + float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id, + const cg::Vector3D velocity_vector, + const cg::Vector3D heading_vector) { - // Calculate velocity along vehicle's heading. This also avoids calculating square root. - const float velocity = cg::Math::Dot(actor->GetVelocity(), actor->GetTransform().GetForwardVector()); + const float velocity = cg::Math::Dot(velocity_vector, heading_vector); float bbox_extension; // Using a linear function to calculate boundary length. // min speed : 0kmph -> BOUNDARY_EXTENSION_MINIMUM // max speed : 100kmph -> BOUNDARY_EXTENSION_MAXIMUM float slope = (BOUNDARY_EXTENSION_MAXIMUM - BOUNDARY_EXTENSION_MINIMUM) / ARBITRARY_MAX_SPEED; bbox_extension = slope * velocity + BOUNDARY_EXTENSION_MINIMUM; - const ActorId actor_id = actor->GetId(); // If a valid collision lock present, change boundary length to maintain lock. if (collision_locks.find(actor_id) != collision_locks.end()) { @@ -421,7 +438,9 @@ namespace CollisionStageConstants { return bbox_extension; } - LocationList CollisionStage::GetBoundary(const Actor &actor, const cg::Location &location) { + LocationList CollisionStage::GetBoundary(const Actor &actor, + const cg::Location &location, + const cg::Vector3D velocity) { const auto actor_type = actor->GetTypeId(); cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); @@ -437,7 +456,7 @@ namespace CollisionStageConstants { const auto walker = boost::static_pointer_cast(actor); bbox = walker->GetBoundingBox(); // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions. - forward_extension = walker->GetVelocity().Length() * WALKER_TIME_EXTENSION; + forward_extension = velocity.Length() * WALKER_TIME_EXTENSION; } const cg::Vector3D extent = bbox.extent; @@ -461,11 +480,12 @@ namespace CollisionStageConstants { bool CollisionStage::IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &other_actor, const SimpleWaypointPtr safe_point, - const cg::Location &other_location) { + const cg::Location &other_location, + const cg::Vector3D other_velocity){ bool safe_junction = true; - if (other_actor->GetVelocity().Length() < EPSILON_VELOCITY){ + if (other_velocity.Length() < EPSILON_VELOCITY){ cg::Location safe_location = safe_point->GetLocation(); cg::Vector3D heading_vector = safe_point->GetForwardVector(); @@ -490,7 +510,7 @@ namespace CollisionStageConstants { }; const Polygon reference_polygon = GetPolygon(ego_actor_boundary); - const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location)); + const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location, other_velocity)); const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); if (inter_bbox_distance < INTER_BBOX_DISTANCE_THRESHOLD){ @@ -511,7 +531,9 @@ namespace CollisionStageConstants { } GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle, - const cg::Location &reference_location, const cg::Location &other_location) { + const cg::Location &reference_location, const cg::Location &other_location, + const cg::Vector3D reference_velocity, const cg::Vector3D other_velocity) { + std::string actor_id_key = (reference_vehicle->GetId() < other_vehicle->GetId()) ? std::to_string(reference_vehicle->GetId()) + "|" + std::to_string(other_vehicle->GetId()) : std::to_string(other_vehicle->GetId()) +"|"+ std::to_string(reference_vehicle->GetId()); @@ -525,10 +547,10 @@ GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &r return mCache; } - const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location)); - const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location)); - const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location)); - const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location)); + const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location, reference_velocity)); + const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location, other_velocity)); + const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location, reference_velocity)); + const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location, other_velocity)); const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon); const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon); diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index 8b41f3f1a..b0c92bf89 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -105,37 +106,43 @@ private: /// 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 cg::Location &location); + /// Returns the bounding box corners of the vehicle passed to the method. + LocationList GetBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity); - /// Returns the extrapolated bounding box of the vehicle along its - /// trajectory. - LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location); + /// Returns the extrapolated bounding box of the vehicle along its + /// trajectory. + LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity); - /// Method to construct a boost polygon object. - Polygon GetPolygon(const LocationList &boundary); + /// Method to construct a boost polygon object. + Polygon GetPolygon(const LocationList &boundary); - /// The method returns true if ego_vehicle should stop and wait for - /// other_vehicle to pass. + /// The method returns true if ego_vehicle should stop and wait for + /// other_vehicle to pass. std::pair NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle, const cg::Location &reference_location, const cg::Location &other_location, const SimpleWaypointPtr& closest_point, - const SimpleWaypointPtr& junction_look_ahead); + const SimpleWaypointPtr& junction_look_ahead, + const cg::Vector3D reference_velocity, + const cg::Vector3D other_velocity); - /// Method to calculate the speed dependent bounding box extention for a vehicle. - float GetBoundingBoxExtention(const Actor &ego_vehicle); + /// Method to calculate the speed dependent bounding box extention for a vehicle. + float GetBoundingBoxExtention(const ActorId actor_id, + const cg::Vector3D velocity_vector, + const cg::Vector3D heading_vector); - /// At intersections, used to see if there is space after the junction - bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor, - const SimpleWaypointPtr safe_point, const cg::Location &other_location); + /// At intersections, used to see if there is space after the junction + bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor, + const SimpleWaypointPtr safe_point, const cg::Location &other_location, + const cg::Vector3D other_velocity); /// A simple method used to draw bounding boxes around vehicles void DrawBoundary(const LocationList &boundary); /// Method to compute Geometry result between two vehicles GeometryComparisonCache GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle, - const cg::Location &reference_location, const cg::Location &other_location); + const cg::Location &reference_location, const cg::Location &other_location, + const cg::Vector3D reference_velocity,const cg::Vector3D other_velocity); public: diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index db36f31fa..8a5480c90 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -339,30 +339,14 @@ namespace MapConstants { const WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint(); const crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange(); - const auto change_right = crd::element::LaneMarking::LaneChange::Right; - const auto change_left = crd::element::LaneMarking::LaneChange::Left; - const auto change_both = crd::element::LaneMarking::LaneChange::Both; - try { - if (lane_change == change_right || lane_change == change_both) { - - const WaypointPtr right_waypoint = raw_waypoint->GetRight(); - if (right_waypoint != nullptr && - right_waypoint->GetType() == crd::Lane::LaneType::Driving && - (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { - - SimpleWaypointPtr closest_simple_waypoint = - GetWaypointInVicinity(right_waypoint->GetTransform().location); - if (closest_simple_waypoint == nullptr) { - closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location); - } - reference_waypoint->SetRightWaypoint(closest_simple_waypoint); - } - } - - if (lane_change == change_left || lane_change == change_both) { - - const WaypointPtr left_waypoint = raw_waypoint->GetLeft(); + /// Cheack for transits + switch(lane_change) + { + /// Left transit way point present only + case crd::element::LaneMarking::LaneChange::Left: + { + const WaypointPtr left_waypoint = raw_waypoint->GetLeft(); if (left_waypoint != nullptr && left_waypoint->GetType() == crd::Lane::LaneType::Driving && (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { @@ -375,13 +359,61 @@ namespace MapConstants { reference_waypoint->SetLeftWaypoint(closest_simple_waypoint); } } - } catch (const std::invalid_argument &e) { - cg::Location loc = reference_waypoint->GetLocation(); - carla::log_info( - "Unable to link lane change connection at: " + - std::to_string(loc.x) + " " + - std::to_string(loc.y) + " " + - std::to_string(loc.z)); + break; + + /// Right transit way point present only + case crd::element::LaneMarking::LaneChange::Right: + { + const WaypointPtr right_waypoint = raw_waypoint->GetRight(); + if(right_waypoint != nullptr && + right_waypoint->GetType() == crd::Lane::LaneType::Driving && + (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { + + SimpleWaypointPtr closest_simple_waypoint = + GetWaypointInVicinity(right_waypoint->GetTransform().location); + if (closest_simple_waypoint == nullptr) { + closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location); + } + reference_waypoint->SetRightWaypoint(closest_simple_waypoint); + } + } + break; + + /// Both left and right transit present + case crd::element::LaneMarking::LaneChange::Both: + { + /// Right transit way point + const WaypointPtr right_waypoint = raw_waypoint->GetRight(); + if (right_waypoint != nullptr && + right_waypoint->GetType() == crd::Lane::LaneType::Driving && + (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { + + SimpleWaypointPtr closest_simple_waypointR = + GetWaypointInVicinity(right_waypoint->GetTransform().location); + if (closest_simple_waypointR == nullptr) { + closest_simple_waypointR = GetWaypoint(right_waypoint->GetTransform().location); + } + reference_waypoint->SetRightWaypoint(closest_simple_waypointR); + } + + /// Left transit way point + const WaypointPtr left_waypoint = raw_waypoint->GetLeft(); + if (left_waypoint != nullptr && + left_waypoint->GetType() == crd::Lane::LaneType::Driving && + (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { + + SimpleWaypointPtr closest_simple_waypointL = + GetWaypointInVicinity(left_waypoint->GetTransform().location); + if (closest_simple_waypointL == nullptr) { + closest_simple_waypointL = GetWaypoint(left_waypoint->GetTransform().location); + } + reference_waypoint->SetLeftWaypoint(closest_simple_waypointL); + } + } + break; + + /// For no transit waypoint (left or right) + default: break; } } diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index 95c8296ef..64457fb62 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -27,7 +27,9 @@ namespace LocalizationConstants { static const float STOPPED_VELOCITY_THRESHOLD = 0.8f; // meters per second. static const float INTER_LANE_CHANGE_DISTANCE = 10.0f; static const float MAX_COLLISION_RADIUS = 100.0f; - + static const float PHYSICS_RADIUS = 50.0f; + static const float POSITION_WINDOW_SIZE = 2.1f; + static const float HYBRID_MODE_DT = 0.05f; } // namespace LocalizationConstants using namespace LocalizationConstants; @@ -66,7 +68,8 @@ namespace LocalizationConstants { maximum_idle_time = std::make_pair(nullptr, 0.0); // Initializing srand. srand(static_cast(time(NULL))); - + // Initializing kinematic update clock. + previous_update_instance = chr::system_clock::now(); } LocalizationStage::~LocalizationStage() {} @@ -75,6 +78,8 @@ namespace LocalizationConstants { ScanUnregisteredVehicles(); + UpdateSwarmVelocities(); + // 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; @@ -90,7 +95,8 @@ namespace LocalizationConstants { const Actor vehicle = actor_list.at(i); const ActorId actor_id = vehicle->GetId(); const cg::Location vehicle_location = vehicle->GetLocation(); - const float vehicle_velocity = vehicle->GetVelocity().Length(); + const cg::Vector3D vehicle_velocity_vector = GetVelocity(actor_id); + const float vehicle_velocity = vehicle_velocity_vector.Length(); // Initializing idle times. if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0) { @@ -303,18 +309,54 @@ namespace LocalizationConstants { }); // Constructing output vector. - std::vector> collision_candidates; + std::vector> collision_candidates; std::for_each(collision_candidate_ids.begin(), collision_candidate_ids.end(), - [&overlapping_actor_info, &collision_candidates] (const ActorId& a_id) { - collision_candidates.push_back({a_id, overlapping_actor_info.at(a_id).first}); + [&overlapping_actor_info, &collision_candidates, this] (const ActorId& a_id) { + collision_candidates.push_back({a_id, + overlapping_actor_info.at(a_id).first, + this->GetVelocity(a_id)}); }); + // Sampling waypoint window for teleportation in hybrid physics mode. + std::vector position_window; + if (hybrid_physics_mode) { + + cg::Vector3D heading = vehicle->GetTransform().GetForwardVector(); + bool window_begin = false; + SimpleWaypointPtr begining_wp; + bool window_complete = false; + + for (uint32_t j = 0u; j < waypoint_buffer.size() && !window_complete; ++j) { + + SimpleWaypointPtr &swp = waypoint_buffer.at(j); + cg::Vector3D relative_position = swp->GetLocation() - vehicle_location; + + // Sample waypoints in front of the vehicle; + if (!window_begin && cg::Math::Dot(relative_position, heading) > 0.0f) { + window_begin = true; + begining_wp = swp; + } + + if (window_begin && !window_complete) { + if (swp->DistanceSquared(begining_wp) > std::pow(POSITION_WINDOW_SIZE, 2)) { + // Stop when maximum size is reached. + window_complete = true; + } else { + position_window.push_back(swp); + } + } + } + } + // Editing output frames. LocalizationToPlannerData &planner_message = current_planner_frame->at(i); planner_message.actor = vehicle; planner_message.deviation = dot_product; planner_message.distance = distance; planner_message.approaching_true_junction = approaching_junction; + planner_message.velocity = vehicle_velocity_vector; + planner_message.position_window = std::move(position_window); + planner_message.physics_enabled = IsPhysicsEnabled(actor_id); LocalizationToCollisionData &collision_message = current_collision_frame->at(i); collision_message.actor = vehicle; @@ -323,6 +365,7 @@ namespace LocalizationConstants { collision_message.closest_waypoint = updated_front_waypoint; collision_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index); collision_message.safe_point_after_junction = final_safe_points[actor_id]; + collision_message.velocity = vehicle_velocity_vector; LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i); traffic_light_message.actor = vehicle; @@ -342,6 +385,9 @@ namespace LocalizationConstants { } void LocalizationStage::DataReceiver() { + + hybrid_physics_mode = parameters.GetHybridPhysicsMode(); + bool is_deleted_actors_present = false; std::set world_actor_id; std::vector actor_list_to_be_deleted; @@ -357,6 +403,7 @@ namespace LocalizationConstants { return filtered; }; + // TODO: Combine this actor scanning logic with ScanUnregisteredActors() // Get all the actors. auto world_actors_list = episode_proxy_ls.Lock()->GetAllTheActorsInTheEpisode(); @@ -366,14 +413,34 @@ namespace LocalizationConstants { // Building a set of vehicle ids in the world. for (const auto &actor : vehicles) { world_actor_id.insert(actor.GetId()); + + // Identify hero vehicle if currently not present + // and system is in hybrid physics mode. + if (hybrid_physics_mode && hero_actor == nullptr) { + Actor actor_ptr = actor.Get(episode_proxy_ls); + for (auto&& attribute: actor_ptr->GetAttributes()) { + if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") { + hero_actor = actor_ptr; + break; + } + } + } + } + + // Invalidate hero actor pointer if it is not alive anymore. + if (hybrid_physics_mode && hero_actor != nullptr + && world_actor_id.find(hero_actor->GetId()) == world_actor_id.end()) { + hero_actor = nullptr; } // Search for invalid/destroyed vehicles. for (auto &actor : actor_list) { - if (world_actor_id.find(actor->GetId()) == world_actor_id.end()) { + ActorId deletion_id = actor->GetId(); + if (world_actor_id.find(deletion_id) == world_actor_id.end()) { actor_list_to_be_deleted.emplace_back(actor); - track_traffic.DeleteActor(actor->GetId()); - last_lane_change_location.erase(actor->GetId()); + track_traffic.DeleteActor(deletion_id); + last_lane_change_location.erase(deletion_id); + kinematic_state_map.erase(deletion_id); } } @@ -531,7 +598,7 @@ namespace LocalizationConstants { { const ActorId actor_id = vehicle->GetId(); - const float vehicle_velocity = vehicle->GetVelocity().Length(); + const float vehicle_velocity = GetVelocity(actor_id).Length(); // Waypoint representing the new starting point for the waypoint buffer // due to lane change. Remains nullptr if lane change not viable. @@ -760,7 +827,8 @@ namespace LocalizationConstants { } const auto vehicle = boost::static_pointer_cast(actor); - if (actor->GetVelocity().Length() > STOPPED_VELOCITY_THRESHOLD || (vehicle->IsAtTrafficLight() && vehicle->GetTrafficLightState() != TLS::Green)) { + if (GetVelocity(actor->GetId()).Length() > STOPPED_VELOCITY_THRESHOLD + || (vehicle->IsAtTrafficLight() && vehicle->GetTrafficLightState() != TLS::Green)) { idle_time[actor->GetId()] = current_timestamp.elapsed_seconds; } @@ -814,5 +882,87 @@ namespace LocalizationConstants { } return false; } + + void LocalizationStage::UpdateSwarmVelocities() { + + // Location of hero vehicle if present. + cg::Location hero_location; + if (hybrid_physics_mode && hero_actor != nullptr) { + hero_location = hero_actor->GetLocation(); + } + + // Using (1/20)s time delta for computing velocity. + float dt = HYBRID_MODE_DT; + // Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode. + if (!parameters.GetSynchronousMode()) { + TimePoint current_instance = chr::system_clock::now(); + chr::duration elapsed_time = current_instance - previous_update_instance; + if (elapsed_time.count() > dt) { + previous_update_instance = current_instance; + } else if (hybrid_physics_mode) { + return; + } + } + + // Update velocity for all registered vehicles. + for (const Actor &actor: actor_list) { + + ActorId actor_id = actor->GetId(); + cg::Location vehicle_location = actor->GetLocation(); + + // Adding entry if not present. + if (kinematic_state_map.find(actor_id) == kinematic_state_map.end()) { + kinematic_state_map.insert({actor_id, KinematicState{true, vehicle_location, cg::Vector3D()}}); + } + + // Check if current actor is in range of hero actor and enable physics in hybrid mode. + bool in_range_of_hero_actor = false; + if (hybrid_physics_mode + && hero_actor != nullptr + && (cg::Math::DistanceSquared(vehicle_location, hero_location) < std::pow(PHYSICS_RADIUS, 2))) { + in_range_of_hero_actor = true; + } + bool enable_physics = hybrid_physics_mode? in_range_of_hero_actor: true; + kinematic_state_map.at(actor_id).physics_enabled = enable_physics; + actor->SetSimulatePhysics(enable_physics); + + // When we say velocity, we usually mean velocity for a vehicle along it's heading. + // Velocity component due to rotation can be removed by taking dot product with heading vector. + cg::Vector3D heading = actor->GetTransform().GetForwardVector(); + if (enable_physics) { + + kinematic_state_map.at(actor_id).velocity = cg::Math::Dot(actor->GetVelocity(), heading) * heading; + + } else { + + cg::Vector3D displacement = (vehicle_location - kinematic_state_map.at(actor_id).location); + cg::Vector3D displacement_along_heading = cg::Math::Dot(displacement, heading) * heading; + cg::Vector3D velocity = displacement_along_heading/dt; + kinematic_state_map.at(actor_id).velocity = velocity; + } + + // Updating location after velocity is computed. + kinematic_state_map.at(actor_id).location = vehicle_location; + } + } + + cg::Vector3D LocalizationStage::GetVelocity(ActorId actor_id) { + cg::Vector3D vel; + if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) { + vel = kinematic_state_map.at(actor_id).velocity; + } else if (unregistered_actors.find(actor_id) != unregistered_actors.end()) { + vel = unregistered_actors.at(actor_id)->GetVelocity(); + } + return vel; + } + + bool LocalizationStage::IsPhysicsEnabled(ActorId actor_id) { + bool enabled = true; + if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) { + enabled = kinematic_state_map.at(actor_id).physics_enabled; + } + return enabled; + } + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index 2c1f93c92..cc47e204a 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include "carla/StringUtil.h" @@ -42,14 +43,23 @@ namespace carla { namespace traffic_manager { - using namespace std::chrono; namespace cc = carla::client; + namespace cg = carla::geom; + namespace chr = std::chrono; + using namespace chr; using Actor = carla::SharedPtr; using Vehicle = carla::SharedPtr; using ActorId = carla::ActorId; using ActorIdSet = std::unordered_set; using TLS = carla::rpc::TrafficLightState; + /// Structure to hold kinematic state of actors. + struct KinematicState { + bool physics_enabled; + cg::Location location; + cg::Vector3D velocity; + }; + /// This class is responsible for maintaining a horizon of waypoints ahead /// of the vehicle for it to follow. /// The class is also responsible for managing lane change decisions and @@ -122,6 +132,14 @@ namespace traffic_manager { SnippetProfiler snippet_profiler; /// Map to keep track of last lane change location. std::unordered_map last_lane_change_location; + /// Reference of hero vehicle. + Actor hero_actor {nullptr}; + /// Switch indicating hybrid physics mode. + bool hybrid_physics_mode {false}; + /// Structure to hold previous state of physics-less vehicle. + std::unordered_map kinematic_state_map; + /// Time instance used to calculate dt in asynchronous mode. + TimePoint previous_update_instance; /// A simple method used to draw waypoint buffer ahead of a vehicle. void DrawBuffer(Buffer &buffer); @@ -146,6 +164,11 @@ namespace traffic_manager { void CleanActor(const ActorId actor_id); bool TryDestroyVehicle(const Actor& actor); + /// Methods for actor state management. + void UpdateSwarmVelocities(); + cg::Vector3D GetVelocity(ActorId actor_id); + bool IsPhysicsEnabled(ActorId actor_id); + public: LocalizationStage( diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp index 92d28236b..8fea56e8e 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp @@ -234,7 +234,7 @@ namespace LocalizationConstants { } - std::pair TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) { +std::pair TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) { SimpleWaypointPtr target_waypoint = waypoint_buffer.front(); const SimpleWaypointPtr& buffer_front = waypoint_buffer.front(); @@ -250,27 +250,28 @@ namespace LocalizationConstants { } if(mScanForward) { - for (index = startPosn ; - (index < waypoint_buffer.size()) && - (buffer_front->DistanceSquared(target_waypoint) - < target_point_dist_power); - ++index) { - target_waypoint = waypoint_buffer.at(index); - } + for (uint64_t i = startPosn; + (i < waypoint_buffer.size()) + && (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power); + ++i) { + target_waypoint = waypoint_buffer.at(i); + index = i; + } } else { - for (index = startPosn ; - (buffer_front->DistanceSquared(target_waypoint) - > target_point_dist_power); - --index) { - target_waypoint = waypoint_buffer.at(index); - } + for (uint64_t i = startPosn; + (buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power); + --i) { + target_waypoint = waypoint_buffer.at(i); + index = i; + } } } else { target_waypoint = waypoint_buffer.back(); + index = waypoint_buffer.size() - 1; } - return std::make_pair(target_waypoint,index); + return std::make_pair(target_waypoint, index); } } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h index 5cf1da474..55111a488 100644 --- a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h +++ b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h @@ -7,11 +7,13 @@ #pragma once #include +#include #include #include #include "carla/client/Actor.h" #include "carla/geom/Vector3D.h" +#include "carla/geom/Transform.h" #include "carla/Memory.h" #include "carla/rpc/ActorId.h" @@ -22,6 +24,7 @@ namespace carla { namespace traffic_manager { namespace cc = carla::client; + namespace cg = carla::geom; /// Convenience typing. @@ -41,6 +44,9 @@ namespace traffic_manager { float deviation; float distance; bool approaching_true_junction; + cg::Vector3D velocity; + bool physics_enabled; + std::vector> position_window; }; /// Type of data sent by the motion planner stage to the batch control stage. @@ -49,16 +55,19 @@ namespace traffic_manager { float throttle; float brake; float steer; + bool physics_enabled; + cg::Transform transform; }; /// Type of data sent by the localization stage to the collision stage. struct LocalizationToCollisionData { Actor actor; Buffer buffer; - std::vector> overlapping_actors; + std::vector> overlapping_actors; std::shared_ptr safe_point_after_junction; std::shared_ptr closest_waypoint; std::shared_ptr junction_look_ahead_waypoint; + cg::Vector3D velocity; }; /// Type of data sent by the collision stage to the motion planner stage. diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp index 44d15e3de..4e13ce180 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp @@ -18,6 +18,8 @@ namespace PlannerConstants { static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f; static const float FOLLOW_DISTANCE_RATE = (MAX_FOLLOW_LEAD_DISTANCE-MIN_FOLLOW_LEAD_DISTANCE)/ARBITRARY_MAX_SPEED; static const float CRITICAL_BRAKING_MARGIN = 0.25f; + static const float HYBRID_MODE_DT = 0.05f; + static const float EPSILON_VELOCITY = 0.001f; } // namespace PlannerConstants using namespace PlannerConstants; @@ -82,7 +84,7 @@ namespace PlannerConstants { const ActorId actor_id = actor->GetId(); const auto vehicle = boost::static_pointer_cast(actor); - const cg::Vector3D current_velocity_vector = vehicle->GetVelocity(); + const cg::Vector3D current_velocity_vector = localization_data.velocity; const float current_velocity = current_velocity_vector.Length(); const auto current_time = chr::system_clock::now(); @@ -120,7 +122,7 @@ namespace PlannerConstants { // Consider collision avoidance decisions only if there is positive relative velocity // of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle). - if (ego_relative_velocity > 0.0f) { + if (ego_relative_velocity > EPSILON_VELOCITY) { // If other vehicle is approaching lead vehicle and lead vehicle is further // than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m. float follow_lead_distance = ego_relative_velocity * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE; @@ -139,29 +141,120 @@ namespace PlannerConstants { collision_emergency_stop = true; } } + if (collision_data.distance_to_other_vehicle < CRITICAL_BRAKING_MARGIN) { + collision_emergency_stop = true; + } } /////////////////////////////////////////////////////////////////////////////////// // Clip dynamic target velocity to maximum allowed speed for the vehicle. dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity); - // State update for vehicle. - StateEntry current_state = controller.StateUpdate(previous_state, current_velocity, - dynamic_target_velocity, current_deviation, - current_distance, current_time); - - // Controller actuation. - ActuationSignal actuation_signal = controller.RunStep(current_state, previous_state, - longitudinal_parameters, lateral_parameters); - - // In case of traffic light hazard. + bool emergency_stop = false; + // In case of collision or traffic light hazard. if (traffic_light_frame->at(i).traffic_light_hazard || collision_emergency_stop) { + emergency_stop = true; + } + + // Message items to be sent to batch control stage. + ActuationSignal actuation_signal {0.0f, 0.0f, 0.0f}; + bool physics_enabled = true; + cg::Transform teleportation_transform; + + // If physics is enabled for the vehicle, use PID controller. + StateEntry current_state; + if (localization_data.physics_enabled) { + + // State update for vehicle. + current_state = controller.StateUpdate(previous_state, current_velocity, + dynamic_target_velocity, current_deviation, + current_distance, current_time); + + // Controller actuation. + actuation_signal = controller.RunStep(current_state, previous_state, + longitudinal_parameters, lateral_parameters); + + if (emergency_stop) { + + current_state.deviation_integral = 0.0f; + current_state.velocity_integral = 0.0f; + actuation_signal.throttle = 0.0f; + actuation_signal.brake = 1.0f; + } + + } + // For physics-less vehicles, determine position and orientation for teleportation. + else if (hybrid_physics_mode) { + + physics_enabled = false; + + // Flushing controller state for vehicle. + current_state = {0.0f, 0.0f, 0.0f, + chr::system_clock::now(), + 0.0f, 0.0f, 0.0f}; + + // Add entry to teleportation duration clock table if not present. + if (teleportation_instance.find(actor_id) == teleportation_instance.end()) { + teleportation_instance.insert({actor_id, chr::system_clock::now()}); + } + + // Measuring time elapsed since last teleportation for the vehicle. + chr::duration elapsed_time = current_time - teleportation_instance.at(actor_id); + + // Find a location ahead of the vehicle for teleportation to achieve intended velocity. + if (!emergency_stop && !(!parameters.GetSynchronousMode() && elapsed_time.count() < HYBRID_MODE_DT)) { + + // Target displacement magnitude to achieve target velocity. + float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT; + + SimpleWaypointPtr target_interval_begin = nullptr; + SimpleWaypointPtr target_interval_end = nullptr; + bool teleportation_interval_found = false; + cg::Location vehicle_location = actor->GetLocation(); + + // Find the interval containing position to achieve target displacement. + for (uint32_t j = 0u; + j+1 < localization_data.position_window.size() && !teleportation_interval_found; + ++j) { + + target_interval_begin = localization_data.position_window.at(j); + target_interval_end = localization_data.position_window.at(j+1); + + if (target_interval_begin->DistanceSquared(vehicle_location) > std::pow(target_displacement, 2) + || (target_interval_begin->DistanceSquared(vehicle_location) < std::pow(target_displacement, 2) + && target_interval_end->DistanceSquared(vehicle_location) > std::pow(target_displacement, 2)) + ) { + teleportation_interval_found = true; + } + } + + if (target_interval_begin != nullptr && target_interval_end != nullptr) { + + // Construct target transform to accurately achieve desired velocity. + float missing_displacement = target_displacement - (target_interval_begin->Distance(vehicle_location)); + cg::Transform target_base_transform = target_interval_begin->GetTransform(); + cg::Location target_base_location = target_base_transform.location; + cg::Vector3D target_heading = target_base_transform.GetForwardVector(); + cg::Location teleportation_location = target_base_location + + cg::Location(target_heading * missing_displacement); + teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation); + + } else { + + teleportation_transform = actor->GetTransform(); + + } + + } + // In case of an emergency stop, stay in the same location. + // Also, teleport only once every dt in asynchronous mode. + else { + + teleportation_transform = actor->GetTransform(); + + } - current_state.deviation_integral = 0.0f; - current_state.velocity_integral = 0.0f; - actuation_signal.throttle = 0.0f; - actuation_signal.brake = 1.0f; } // Updating PID state. @@ -174,6 +267,8 @@ namespace PlannerConstants { message.throttle = actuation_signal.throttle; message.brake = actuation_signal.brake; message.steer = actuation_signal.steer; + message.physics_enabled = physics_enabled; + message.transform = teleportation_transform; } } @@ -192,6 +287,8 @@ namespace PlannerConstants { control_frame_a = std::make_shared(number_of_vehicles); control_frame_b = std::make_shared(number_of_vehicles); } + + hybrid_physics_mode = parameters.GetHybridPhysicsMode(); } void MotionPlannerStage::DataSender() { diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h index 55c458a89..da165c4e1 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h @@ -7,6 +7,7 @@ #pragma once #include +#include #include #include @@ -18,6 +19,7 @@ #include "carla/trafficmanager/Parameters.h" #include "carla/trafficmanager/PIDController.h" #include "carla/trafficmanager/PipelineStage.h" +#include "carla/trafficmanager/SimpleWaypoint.h" namespace carla { namespace traffic_manager { @@ -27,6 +29,7 @@ namespace traffic_manager { using Actor = carla::SharedPtr; using ActorId = carla::rpc::ActorId; + using SimpleWaypointPtr = std::shared_ptr; /// The class is responsible for aggregating information from various stages /// like the localization stage, traffic light stage, collision detection @@ -68,7 +71,10 @@ namespace traffic_manager { uint64_t number_of_vehicles; /// Reference to Carla's debug helper object. cc::DebugHelper &debug_helper; - + /// Switch indicating hybrid physics mode. + bool hybrid_physics_mode {false}; + /// Teleportation duration clock; + std::unordered_map teleportation_instance; public: diff --git a/LibCarla/source/carla/trafficmanager/Parameters.cpp b/LibCarla/source/carla/trafficmanager/Parameters.cpp index 7b9dce9d1..85c1bfb90 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.cpp +++ b/LibCarla/source/carla/trafficmanager/Parameters.cpp @@ -19,6 +19,11 @@ Parameters::Parameters() { Parameters::~Parameters() {} +void Parameters::SetHybridPhysicsMode(const bool mode_switch) { + + hybrid_physics_mode.store(mode_switch); +} + void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { float new_percentage = std::min(100.0f, percentage); @@ -260,5 +265,10 @@ float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) { return percentage; } +bool Parameters::GetHybridPhysicsMode() { + + return hybrid_physics_mode.load(); +} + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/Parameters.h b/LibCarla/source/carla/trafficmanager/Parameters.h index beb0295fd..f8949028e 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.h +++ b/LibCarla/source/carla/trafficmanager/Parameters.h @@ -60,6 +60,8 @@ namespace traffic_manager { std::atomic synchronous_mode {false}; /// Distance Margin std::atomic distance_margin {2.0}; + /// Hybrid physics mode switch. + std::atomic hybrid_physics_mode {false}; public: Parameters(); @@ -150,6 +152,12 @@ namespace traffic_manager { /// Set Synchronous mode time out. void SetSynchronousModeTimeOutInMiliSecond(const double time); + /// Method to set hybrid physics mode. + void SetHybridPhysicsMode(const bool mode_switch); + + /// Method to retrieve hybrid physics mode. + bool GetHybridPhysicsMode(); + /// Synchronous mode time out variable. std::chrono::duration synchronous_time_out; diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp index 9162b38fc..e94dfd21f 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp @@ -64,25 +64,21 @@ namespace traffic_manager { return static_cast(waypoints.size()); } - void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr _waypoint) { + void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr &_waypoint) { const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f) { next_left_waypoint = _waypoint; - } else { - throw std::invalid_argument("Argument not on the left side!"); } } - void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr _waypoint) { + void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr &_waypoint) { const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0.0f) { next_right_waypoint = _waypoint; - } else { - throw std::invalid_argument("Argument not on the right side!"); } } @@ -132,5 +128,9 @@ namespace traffic_manager { return waypoint->GetJunctionId(); } + cg::Transform SimpleWaypoint::GetTransform() const { + return waypoint->GetTransform(); + } + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h index da1e22911..a71d112a5 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h @@ -12,6 +12,7 @@ #include "carla/client/Waypoint.h" #include "carla/geom/Location.h" #include "carla/geom/Math.h" +#include "carla/geom/Transform.h" #include "carla/geom/Vector3D.h" #include "carla/Memory.h" #include "carla/road/RoadTypes.h" @@ -77,10 +78,10 @@ namespace traffic_manager { 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); + void SetLeftWaypoint(SimpleWaypointPtr &waypoint); /// This method is used to set the closest right waypoint for a lane change. - void SetRightWaypoint(SimpleWaypointPtr waypoint); + void SetRightWaypoint(SimpleWaypointPtr &waypoint); /// This method is used to get the closest left waypoint for a lane change. SimpleWaypointPtr GetLeftWaypoint(); @@ -117,6 +118,8 @@ namespace traffic_manager { /// Returns true if the object's waypoint belongs to an intersection (Doesn't use OpenDrive). bool CheckIntersection() const; + /// Return transform object for the current waypoint. + cg::Transform GetTransform() const; }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.h b/LibCarla/source/carla/trafficmanager/TrafficManager.h index f40ee3f9a..c060aef0a 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.h @@ -71,6 +71,14 @@ public: static void Tick(); + /// This method sets the hybrid physics mode. + void SetHybridPhysicsMode(const bool mode_switch) { + TrafficManagerBase* tm_ptr = GetTM(_port); + if(tm_ptr != nullptr){ + tm_ptr->SetHybridPhysicsMode(mode_switch); + } + } + /// This method registers a vehicle with the traffic manager. void RegisterVehicles(const std::vector &actor_list) { TrafficManagerBase* tm_ptr = GetTM(_port); diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h b/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h index 5e2968ad6..c9bc2a45f 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h @@ -103,6 +103,9 @@ public: /// Method to set probabilistic preference to keep on the right lane. virtual void SetKeepRightPercentage(const ActorPtr &actor,const float percentage) = 0; + /// Method to set hybrid physics mode. + virtual void SetHybridPhysicsMode(const bool mode_switch) = 0; + protected: }; diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h b/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h index c10a1e69c..465190429 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h @@ -179,6 +179,12 @@ public: _client->call("set_percentage_keep_right_rule", actor, percentage); } + /// Method to set hybrid physics mode. + void SetHybridPhysicsMode(const bool mode_switch) { + DEBUG_ASSERT(_client != nullptr); + _client->call("set_hybrid_physics_mode", mode_switch); + } + private: /// RPC client. diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp index d27c784f9..7a914b3dd 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp @@ -201,6 +201,10 @@ void TrafficManagerLocal::SetKeepRightPercentage(const ActorPtr &actor, const fl parameters.SetKeepRightPercentage(actor, percentage); } +void TrafficManagerLocal::SetHybridPhysicsMode(const bool mode_switch) { + parameters.SetHybridPhysicsMode(mode_switch); +} + bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) { for (auto& elem : tl_to_freeze) { if (!elem->IsFrozen() || elem->GetState() != TLS::Red) { diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h index adc843b79..be30d311b 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h @@ -184,6 +184,9 @@ namespace traffic_manager { /// Method to set probabilistic preference to keep on the right lane. void SetKeepRightPercentage(const ActorPtr &actor, const float percentage); + + /// Method to set hybrid physics mode. + void SetHybridPhysicsMode(const bool mode_switch); }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp index 5c5702158..f0ea88323 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp @@ -176,6 +176,10 @@ void TrafficManagerRemote::SetKeepRightPercentage(const ActorPtr &_actor, const client.SetKeepRightPercentage(actor, percentage); } +void TrafficManagerRemote::SetHybridPhysicsMode(const bool mode_switch) { + client.SetHybridPhysicsMode(mode_switch); +} + void TrafficManagerRemote::ResetAllTrafficLights() { client.ResetAllTrafficLights(); } diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h index 0a943143a..a7790f5c0 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h @@ -100,6 +100,9 @@ public: /// Method to set probabilistic preference to keep on the right lane. void SetKeepRightPercentage(const ActorPtr &actor, const float percentage); + /// Method to set hybrid physics mode. + void SetHybridPhysicsMode(const bool mode_switch); + /// Method to provide synchronous tick bool SynchronousTick(); diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h b/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h index c1f23b1e7..3f3c2fde6 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h @@ -150,6 +150,11 @@ public: tm->SetKeepRightPercentage(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage); }); + /// Method to set hybrid physics mode. + server->bind("set_hybrid_physics_mode", [=](const bool mode_switch) { + tm->SetHybridPhysicsMode(mode_switch); + }); + /// Method to set synchronous mode. server->bind("set_synchronous_mode", [=](const bool mode) { tm->SetSynchronousMode(mode); diff --git a/PythonAPI/carla/source/libcarla/TrafficManager.cpp b/PythonAPI/carla/source/libcarla/TrafficManager.cpp index b7bbcf28b..a5e174328 100644 --- a/PythonAPI/carla/source/libcarla/TrafficManager.cpp +++ b/PythonAPI/carla/source/libcarla/TrafficManager.cpp @@ -30,5 +30,6 @@ void export_trafficmanager() { .def("ignore_signs_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningSign) .def("set_global_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetGlobalDistanceToLeadingVehicle) .def("set_percentage_keep_right_rule", &carla::traffic_manager::TrafficManager::SetKeepRightPercentage) - .def("set_synchronous_mode", &carla::traffic_manager::TrafficManager::SetSynchronousMode); + .def("set_synchronous_mode", &carla::traffic_manager::TrafficManager::SetSynchronousMode) + .def("set_hybrid_physics_mode", &carla::traffic_manager::TrafficManager::SetHybridPhysicsMode); } diff --git a/PythonAPI/examples/spawn_npc.py b/PythonAPI/examples/spawn_npc.py index e659e657a..8b2e2cb9e 100755 --- a/PythonAPI/examples/spawn_npc.py +++ b/PythonAPI/examples/spawn_npc.py @@ -78,6 +78,10 @@ def main(): '--sync', action='store_true', help='Synchronous mode execution') + argparser.add_argument( + '--hybrid', + action='store_true', + help='Enanble') args = argparser.parse_args() logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) @@ -90,9 +94,13 @@ def main(): try: + world = client.get_world() + traffic_manager = client.get_trafficmanager(args.tm_port) traffic_manager.set_global_distance_to_leading_vehicle(2.0) - world = client.get_world() + if args.hybrid: + traffic_manager.set_hybrid_physics_mode(True) + synchronous_master = False diff --git a/PythonAPI/util/performance_benchmark.py b/PythonAPI/util/performance_benchmark.py index 6492de25e..c407d3c34 100755 --- a/PythonAPI/util/performance_benchmark.py +++ b/PythonAPI/util/performance_benchmark.py @@ -325,7 +325,7 @@ def run_benchmark(world, sensors, n_vehicles, n_walkers, client, debug=False): sensor_list = sensors_ret ticks = 0 - while ticks < args.ticks: + while ticks < int(args.ticks): _ = world.wait_for_tick() if debug: print("== Samples {} / {}".format(ticks + 1, args.ticks))