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

110
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() {
@ -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<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

151
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 {
@ -47,83 +64,105 @@ namespace traffic_manager {
using LocationList = std::vector<cg::Location>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
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<LocalizationToCollisionFrame> localization_frame;
/// Pointers to output frames to be shared with motion planner stage.
std::shared_ptr<CollisionToPlannerFrame> planner_frame_a;
std::shared_ptr<CollisionToPlannerFrame> planner_frame_b;
/// Pointers to messenger objects.
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger;
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger;
/// Runtime parameterization object.
Parameters &parameters;
/// 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<ActorId, uint64_t> vehicle_id_to_index;
/// An object used to keep track of time between checking for all world
/// actors.
chr::time_point<chr::system_clock, chr::nanoseconds> 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<ActorId, LocationList> 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<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.
std::shared_ptr<LocalizationToCollisionFrame> localization_frame;
/// Pointers to output frames to be shared with motion planner stage.
std::shared_ptr<CollisionToPlannerFrame> planner_frame_a;
std::shared_ptr<CollisionToPlannerFrame> planner_frame_b;
/// Pointers to messenger objects.
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger;
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger;
/// Runtime parameterization object.
Parameters &parameters;
/// 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<ActorId, uint64_t> vehicle_id_to_index;
/// An object used to keep track of time between checking for all world
/// actors.
chr::time_point<chr::system_clock, chr::nanoseconds> 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<ActorId, LocationList> 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<LocalizationToCollisionMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
Parameters &parameters,
cc::DebugHelper &debug_helper);
~CollisionStage();
~CollisionStage();
void DataReceiver() override;
void DataReceiver() override;
void Action() override;
void Action() override;
void DataSender() override;
void DataSender() override;
};