p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added changes for cacheing Geometric Result

This commit is contained in:
Sekhar Barua 2020-03-13 19:45:02 +05:30 committed by Jacopo Bartiromo
parent e5450c2d05
commit f256a68fa6
2 changed files with 190 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
LibCarla/source/carla/trafficmanager/CollisionStage.cpp
View File

@ -49,12 +49,15 @@ namespace CollisionStageConstants {
number_of_vehicles = 0u;
// Initializing srand.
srand(static_cast<unsigned>(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() {
@ -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<IdPair>{}(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

43
LibCarla/source/carla/trafficmanager/CollisionStage.h
View File

@ -33,6 +33,23 @@
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PipelineStage.h"
using IdPair = std::pair<uint32_t,uint32_t>;
//custom specialization of std::hash can be injected in namespace std
namespace std
{
template <>
struct hash<IdPair>
{
std::size_t operator()(const IdPair& s) const noexcept
{
return std::hash<uint32_t>{}(s.first) ^ std::hash<uint32_t>{}(s.second);
}
};
}
namespace carla {
namespace traffic_manager {
@ -48,13 +65,31 @@ namespace traffic_manager {
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using TLS = carla::rpc::TrafficLightState;
/// Structure to hold the Geometry of refference vehicle to Other Vehicle
struct GeometryComparisonCache {
double reference_vehicle_to_other_geodesic;
double other_vehicle_to_reference_geodesic;
double inter_geodesic_distance;
double inter_bbox_distance;
};
struct LocationTuple { // WIP - This is added only for debugging
cg::Location ref_loc,other_loc;
};
/// 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 {
class CollisionStage : public PipelineStage
{
private:
/// Geometry data for the vehicle
std::unordered_map<std::size_t, GeometryComparisonCache> vehicle_cache;
std::unordered_map<std::size_t, LocationTuple> 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.
@ -108,6 +143,10 @@ namespace traffic_manager {
/// 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(