diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 29790a81c..8903808e2 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -49,12 +49,15 @@ namespace CollisionStageConstants { number_of_vehicles = 0u; // Initializing srand. srand(static_cast(time(NULL))); + // Clearing the Map + //vehicle_cache.clear(); } CollisionStage::~CollisionStage() {} void CollisionStage::Action() { + //vehicle_cache.clear(); const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; // Looping over registered actors. @@ -152,6 +155,9 @@ namespace CollisionStageConstants { // Cleaning geodesic boundaries from the last iteration. geodesic_boundaries.clear(); + + // Clearing the old chache. + //vehicle_cache.clear(); } void CollisionStage::DataSender() { @@ -168,7 +174,7 @@ namespace CollisionStageConstants { const SimpleWaypointPtr& junction_look_ahead) { bool hazard = false; - + const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector(); cg::Vector3D reference_to_other = other_location - reference_location; reference_to_other = reference_to_other.MakeUnitVector(); @@ -206,24 +212,17 @@ namespace CollisionStageConstants { std::pow(parameters.GetDistanceToLeadingVehicle(reference_vehicle) + inter_vehicle_length, 2.0f)))) { - 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 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); - - const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon); - const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); + + GeometryComparisonCache cache = GetGeometryBetweenActors(reference_vehicle, other_vehicle, + reference_location, other_location); // Whichever vehicle's path is farthest away from the other vehicle gets // priority to move. - if (inter_geodesic_distance < 0.1 && - ((inter_bbox_distance > 0.1 && - reference_vehicle_to_other_geodesic > other_vehicle_to_reference_geodesic + if (cache.inter_geodesic_distance < 0.1 && + ((cache.inter_bbox_distance > 0.1 && + cache.reference_vehicle_to_other_geodesic > cache.other_vehicle_to_reference_geodesic ) || ( - inter_bbox_distance < 0.1 && + cache.inter_bbox_distance < 0.1 && (cg::Math::Dot(reference_heading, reference_to_other) > cg::Math::Dot(other_heading, other_to_reference)) )) @@ -441,5 +440,86 @@ namespace CollisionStageConstants { } } +GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle, + const cg::Location &reference_location, const cg::Location &other_location) +{ + std::size_t actor_id_key = std::hash{}(std::make_pair(reference_vehicle->GetId(),other_vehicle->GetId())); + GeometryComparisonCache mRetC{-1,-1,-1,-1}; + bool mReceived = false; + + if (vehicle_cache.find(actor_id_key) != vehicle_cache.end()) { + mRetC = vehicle_cache.at(actor_id_key); + mReceived = true; + } + + 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 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); + + const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon); + const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); + + GeometryComparisonCache mRetCache = {reference_vehicle_to_other_geodesic, + other_vehicle_to_reference_geodesic, + inter_geodesic_distance, + inter_bbox_distance}; + + vehicle_cache.insert({actor_id_key, mRetCache}); + + ///Added for Debugging purpose ////// + + // Storing refference location and other location in vehicle_loc_cache by hasing the refference vehicle ID( say A ) and other vehicle ID ( say B) as key_A_B" + LocationTuple mLocTuple = {reference_location,other_location}; + vehicle_loc_cache.insert({actor_id_key, mLocTuple}); + + debug_helper.DrawLine(reference_vehicle->GetLocation()+ cg::Location(0.0f,0.0f,2.0f),other_vehicle->GetLocation()+cg::Location(0.0f,0.0f,2.0f),0.2f,{255u,0u,0u},0.1f); + if(mReceived) + { + std::string tempString = "**CACHE***" + std::to_string(mRetC.inter_bbox_distance) + " " + std::to_string(mRetC.inter_geodesic_distance) + " " + + std::to_string(mRetC.other_vehicle_to_reference_geodesic)+ " " + std::to_string(mRetC.reference_vehicle_to_other_geodesic); + + std::string tempStringComputed = "###COMPUTED###" + std::to_string(mRetCache.inter_bbox_distance) + " " + std::to_string(mRetCache.inter_geodesic_distance) + " " + + std::to_string(mRetCache.other_vehicle_to_reference_geodesic)+ " " + std::to_string(mRetCache.reference_vehicle_to_other_geodesic); + + debug_helper.DrawString(reference_vehicle->GetLocation() + cg::Location(0.0f,0.0f,4.0f),tempString ,false, {0u, 255u, 0u}, 0.1f); + + debug_helper.DrawString(reference_vehicle->GetLocation() + cg::Location(0.0f,0.0f,2.0f),tempStringComputed, false, {255u, 0u, 0u}, 0.1f); + + /// If the refference vehicle ( say B) and other vehicle ( say A), we expect reffernce_location is other_loc from the cache [ say E] by the same hash key_B_A + /// other location is refference location ( say D) + if (vehicle_loc_cache.find(actor_id_key) != vehicle_loc_cache.end()){ + cg::Location ref_loc = vehicle_loc_cache.at(actor_id_key).ref_loc; // - say D + cg::Location other_loc = vehicle_loc_cache.at(actor_id_key).other_loc; // Say E + + std::string ref_loc_str = std::to_string(ref_loc.x) + " | " +std::to_string(ref_loc.y) + " | " + std::to_string(ref_loc.z); + std::string other_loc_str = std::to_string(other_loc.x) + " | " +std::to_string(other_loc.y) + " | " + std::to_string(other_loc.z); + + std::string reference_location_str = std::to_string(reference_location.x) + " | " +std::to_string(reference_location.y) + " | " + std::to_string(reference_location.z); + std::string other_location_str = std::to_string(other_location.x) + " | " +std::to_string(other_location.y) + " | " + std::to_string(other_location.z); + +/// It seems that reference_location_str is not same as other_loc_str +/// And other_loc_str is not same as reference_location_str. Hence the issue is . + + + std::cout<< "----------------------------------------------\n" << std::endl; + std::cout<< "ref_loc " << ref_loc_str << " other_loc " << other_loc_str << std::endl; + std::cout<< "reference_location_str " << reference_location_str << " " << "other_location_str " << other_location_str << std::endl; + std::cout<< "----------------------------------------------\n" << std::endl; + debug_helper.DrawString(reference_vehicle->GetLocation() + cg::Location(0.0f,0.0f,4.0f),"ref_loc_str"+ref_loc_str ,false, {0u, 255u, 0u}, 0.1f); + debug_helper.DrawString(reference_vehicle->GetLocation() + cg::Location(0.0f,0.0f,8.0f),"other_loc_str"+other_loc_str ,false, {0u, 255u, 0u}, 0.1f); + debug_helper.DrawString(reference_vehicle->GetLocation() + cg::Location(0.0f,0.0f,12.0f),"reference_location_str" + reference_location_str ,false, {0u, 255u, 0u}, 0.1f); + debug_helper.DrawString(reference_vehicle->GetLocation() + cg::Location(0.0f,0.0f,16.0f),"other_location_str"+other_location_str ,false, {0u, 255u, 0u}, 0.1f); + } + + } + ///Added for Debugging purpose ////// + + return mRetCache; + } + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index 923cceb2b..631220d25 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -33,6 +33,23 @@ #include "carla/trafficmanager/Parameters.h" #include "carla/trafficmanager/PipelineStage.h" +using IdPair = std::pair; + +//custom specialization of std::hash can be injected in namespace std + namespace std + { + template <> + struct hash + { + std::size_t operator()(const IdPair& s) const noexcept + { + return std::hash{}(s.first) ^ std::hash{}(s.second); + } + + }; +} + + namespace carla { namespace traffic_manager { @@ -47,83 +64,105 @@ namespace traffic_manager { using LocationList = std::vector; using SimpleWaypointPtr = std::shared_ptr; using TLS = carla::rpc::TrafficLightState; + - /// This class is the thread executable for the collision detection stage - /// and is responsible for checking possible collisions with other - /// cars along the vehicle's trajectory. - class CollisionStage : public PipelineStage { + /// Structure to hold the Geometry of refference vehicle to Other Vehicle + struct GeometryComparisonCache { - private: + double reference_vehicle_to_other_geodesic; + double other_vehicle_to_reference_geodesic; + double inter_geodesic_distance; + double inter_bbox_distance; +}; - /// Selection key for switching between output frames. - bool frame_selector; - /// Pointer to data received from localization stage. - std::shared_ptr localization_frame; - /// Pointers to output frames to be shared with motion planner stage. - std::shared_ptr planner_frame_a; - std::shared_ptr planner_frame_b; - /// Pointers to messenger objects. - std::shared_ptr localization_messenger; - std::shared_ptr planner_messenger; - /// Runtime parameterization object. - Parameters ¶meters; - /// Reference to Carla's debug helper object. - cc::DebugHelper &debug_helper; - /// The map used to connect actor ids to the array index of data frames. - 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; - /// Structure to hold the geodesic boundaries during one iteration. - std::unordered_map geodesic_boundaries; - /// Snippet profiler for measuring execution time. - SnippetProfiler snippet_profiler; +struct LocationTuple { // WIP - This is added only for debugging - /// Returns the bounding box corners of the vehicle passed to the method. - LocationList GetBoundary(const Actor &actor, const cg::Location &location); + cg::Location ref_loc,other_loc; +}; - /// Returns the extrapolated bounding box of the vehicle along its - /// trajectory. - LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location); +/// This class is the thread executable for the collision detection stage +/// and is responsible for checking possible collisions with other +/// cars along the vehicle's trajectory. +class CollisionStage : public PipelineStage +{ - /// Method to construct a boost polygon object. - Polygon GetPolygon(const LocationList &boundary); +private: + /// Geometry data for the vehicle + std::unordered_map vehicle_cache; + std::unordered_map vehicle_loc_cache; // WIP - This is added only for debugging + /// Selection key for switching between output frames. + bool frame_selector; + /// Pointer to data received from localization stage. + std::shared_ptr localization_frame; + /// Pointers to output frames to be shared with motion planner stage. + std::shared_ptr planner_frame_a; + std::shared_ptr planner_frame_b; + /// Pointers to messenger objects. + std::shared_ptr localization_messenger; + std::shared_ptr planner_messenger; + /// Runtime parameterization object. + Parameters ¶meters; + /// Reference to Carla's debug helper object. + cc::DebugHelper &debug_helper; + /// The map used to connect actor ids to the array index of data frames. + 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; + /// Structure to hold the geodesic boundaries during one iteration. + std::unordered_map geodesic_boundaries; + /// Snippet profiler for measuring execution time. + SnippetProfiler snippet_profiler; - /// 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 cg::Location &reference_location, const cg::Location &other_location, - const SimpleWaypointPtr& closest_point, - const SimpleWaypointPtr& junction_look_ahead); + /// Returns the bounding box corners of the vehicle passed to the method. + LocationList GetBoundary(const Actor &actor, const cg::Location &location); - /// Method to calculate the speed dependent bounding box extention for a vehicle. - float GetBoundingBoxExtention(const Actor &ego_vehicle); + /// Returns the extrapolated bounding box of the vehicle along its + /// trajectory. + LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &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); + /// Method to construct a boost polygon object. + Polygon GetPolygon(const LocationList &boundary); - /// A simple method used to draw bounding boxes around vehicles - void DrawBoundary(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 cg::Location &reference_location, const cg::Location &other_location, + const SimpleWaypointPtr& closest_point, + const SimpleWaypointPtr& junction_look_ahead); - public: + /// Method to calculate the speed dependent bounding box extention for a vehicle. + float GetBoundingBoxExtention(const Actor &ego_vehicle); - CollisionStage( + /// 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); + + /// A simple method used to draw bounding boxes around vehicles + void DrawBoundary(const LocationList &boundary); + + /// A method used compute Geometry result between two vehicle + GeometryComparisonCache GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle, + const cg::Location &reference_location, const cg::Location &other_location); + +public: + +CollisionStage( std::string stage_name, std::shared_ptr localization_messenger, std::shared_ptr planner_messenger, Parameters ¶meters, cc::DebugHelper &debug_helper); - ~CollisionStage(); + ~CollisionStage(); - void DataReceiver() override; + void DataReceiver() override; - void Action() override; + void Action() override; - void DataSender() override; + void DataSender() override; };