Skip to content

Commit

Permalink
Increased pedestrian frontal boundign box to avoid some collisions
Browse files Browse the repository at this point in the history
  • Loading branch information
glopezdiest committed Dec 18, 2019
1 parent 463e6bb commit 93d7f61
Show file tree
Hide file tree
Showing 3 changed files with 35 additions and 44 deletions.
71 changes: 31 additions & 40 deletions LibCarla/source/carla/trafficmanager/CollisionStage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@ namespace CollisionStageConstants {
static const float BOUNDARY_EDGE_LENGTH = 2.0f;
static const float MAX_COLLISION_RADIUS = 100.0f;
static const float MIN_COLLISION_RADIUS = 15.0f;
static const float MIN_DISTANCE_VEHICLE_PEDESTRIAN = 1.5f;

} // namespace CollisionStageConstants

Expand Down Expand Up @@ -50,7 +49,6 @@ namespace CollisionStageConstants {
CollisionStage::~CollisionStage() {}

void CollisionStage::Action() {

const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;

// Looping over registered actors.
Expand Down Expand Up @@ -78,10 +76,10 @@ namespace CollisionStageConstants {
try {
const cg::Location ego_location = ego_actor->GetLocation();
const cg::Location other_location = actor->GetLocation();

// debug_helper.DrawArrow(ego_location + cg::Location(0, 0, 2),
// other_location + cg::Location(0, 0, 2),
// 0.5f, 0.5f, {255u, 0u, 255u}, 0.1f);
// Collision checks increase with speed (Official formula used)
float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f);
collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS);

if (actor_id != ego_actor_id &&
(cg::Math::DistanceSquared(ego_location, other_location)
Expand Down Expand Up @@ -148,9 +146,6 @@ namespace CollisionStageConstants {
const cg::Location reference_location = reference_vehicle->GetLocation();
const cg::Location other_location = other_vehicle->GetLocation();

const auto reference_vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(reference_vehicle);
const auto other_vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(other_vehicle);

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();
Expand Down Expand Up @@ -188,7 +183,7 @@ namespace CollisionStageConstants {

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);

Expand All @@ -204,36 +199,13 @@ namespace CollisionStageConstants {
))
) {

debug_helper.DrawString(reference_location,"Stopping, pedestrian",false,{255u,0u,255u},0.03f);
hazard = true;
}
hazard = true;
}
} else {
std::cout << "Actors not v or w?\n";
}
return hazard;
}

/*} else {
//debug_helper.DrawLine(reference_location,other_location,0.2f,{0u,255u,255u},0.02f);
float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x;
float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x;
float vehicle_length_sum = reference_vehicle_length + other_vehicle_length;
float bbox_extension_length = GetBoundingBoxExtention(reference_vehicle);
// 2) Hazard if close enough
const auto actor_type = other_vehicle->GetTypeId();

if (cg::Math::Dot(reference_heading, reference_to_other) > 0 &&
cg::Math::Dot(reference_heading, other_heading) > 0 &&
cg::Math::DistanceSquared(reference_location, other_location) <
std::pow(bbox_extension_length + vehicle_length_sum, 2)) {
debug_helper.DrawString(reference_location,"Stopping, vehicle",false,{0u,255u,255u},0.03f);
hazard = true;
}
}*/
return hazard;
}

traffic_manager::Polygon CollisionStage::GetPolygon(const LocationList &boundary) {
Expand All @@ -260,7 +232,6 @@ namespace CollisionStageConstants {

float bbox_extension = GetBoundingBoxExtention(actor);


const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor);
if (specific_distance_margin > 0.0f) {
bbox_extension = std::max(specific_distance_margin, bbox_extension);
Expand Down Expand Up @@ -348,7 +319,6 @@ namespace CollisionStageConstants {
BOUNDARY_EXTENSION_MINIMUM;
}


return bbox_extension;
}

Expand Down Expand Up @@ -377,24 +347,45 @@ namespace CollisionStageConstants {
heading_vector.z = 0.0f;
const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);


// Four corners of the vehicle in top view clockwise order (left-handed
// system).

if (actor_type[0] == 'v'){
const cg::Vector3D x_boundary_vector = heading_vector * extent.x;
const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y;

return {
location + cg::Location(x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
location + cg::Location(x_boundary_vector + y_boundary_vector),
};

// Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.

} else {
float walker_extension = 0.0f;
walker_extension = 3.0f;

const cg::Vector3D x_boundary_vector = heading_vector * extent.x;
const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y;

return {
location + cg::Location(heading_vector * (extent.x + walker_extension) - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
location + cg::Location(heading_vector * (extent.x + walker_extension) + y_boundary_vector),
};
}
}

void CollisionStage::DrawBoundary(const LocationList &boundary) {
for (uint64_t i = 0u; i < boundary.size(); ++i) {
debug_helper.DrawLine(
boundary[i] + cg::Location(0.0f, 0.0f, 1.0f),
boundary[(i + 1) % boundary.size()] + cg::Location(0.0f, 0.0f, 1.0f),
0.1f, {255u, 255u, 0u}, 0.02f);
0.1f, {0u, 255u, 255u}, 0.04f);
}
}

Expand Down
6 changes: 3 additions & 3 deletions LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h
Original file line number Diff line number Diff line change
Expand Up @@ -77,9 +77,9 @@ namespace traffic_manager {

chr::duration<float> print_duration = current_time - print_clock;
if (print_duration.count() > 1.0f) {
std::cout << "Snippet name : " << snippet_name << ", "
<< "avg. duration : " << 1000 * snippet_duration.count()
<< " ms" << std::endl;
//std::cout << "Snippet name : " << snippet_name << ", "
// << "avg. duration : " << 1000 * snippet_duration.count()
// << " ms" << std::endl;
print_clock = current_time;
}
}
Expand Down
2 changes: 1 addition & 1 deletion LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ namespace traffic_manager {
ego_vehicle->IsAtTrafficLight() &&
traffic_light_state != TLS::Green) {

traffic_light_hazard = false;
traffic_light_hazard = true;
}
// Handle entry negotiation at non-signalised junction.
else if (!closest_waypoint->CheckJunction() &&
Expand Down

0 comments on commit 93d7f61

Please sign in to comment.