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

Hot Fixes For Hybrid Physics Mode (#2680) 

* 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

* WIP: Hybrid mode oscillation bug

* Added vertical offset parameter for physics-less mode.
Restricting longitudinal waypoint offset to forward only.

* remove comment

Co-authored-by: Jacopo Bartiromo <jackbart94@gmail.com>
Co-authored-by: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com>
This commit is contained in:
Praveen Kumar 2020-03-31 18:26:18 +05:30 committed by GitHub
parent 1e06309953
commit 858a58556f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 52 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
View File

@ -12,12 +12,14 @@ namespace traffic_manager {
namespace LocalizationConstants {
static const float WAYPOINT_TIME_HORIZON = 5.0f;
static const float MINIMUM_HORIZON_LENGTH = 30.0f;
static const float MAXIMUM_HORIZON_LENGTH = 60.0f;
static const float TARGET_WAYPOINT_TIME_HORIZON = 1.0f;
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 5.0f;
static const float MINIMUM_JUNCTION_LOOK_AHEAD = 10.0f;
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f;
static const float HORIZON_RATE = (MAXIMUM_HORIZON_LENGTH - MINIMUM_HORIZON_LENGTH) / ARBITRARY_MAX_SPEED;
static const float MINIMUM_LANE_CHANGE_DISTANCE = 10.0f;
static const float MAXIMUM_LANE_OBSTACLE_DISTANCE = 50.0f;
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f;
@ -103,9 +105,9 @@ namespace LocalizationConstants {
idle_time[actor_id] = current_timestamp.elapsed_seconds;
}
const float horizon_size = std::max(
WAYPOINT_TIME_HORIZON * std::sqrt(vehicle_velocity * 10.0f),
MINIMUM_HORIZON_LENGTH);
// Speed dependent waypoint horizon length.
const float horizon_square = std::min(std::pow(vehicle_velocity * HORIZON_RATE + MINIMUM_HORIZON_LENGTH, 2.0f),
std::pow(MAXIMUM_HORIZON_LENGTH, 2.0f));
if (buffer_list->find(actor_id) == buffer_list->end()) {
buffer_list->insert({actor_id, Buffer()});
@ -123,17 +125,22 @@ namespace LocalizationConstants {
}
}
// Purge passed waypoints.
if (!waypoint_buffer.empty()) {
float dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation(), true);
// Purge passed waypoints.
float dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation());
while (dot_product <= 0.0f && !waypoint_buffer.empty()) {
PopWaypoint(waypoint_buffer, actor_id);
if (!waypoint_buffer.empty()) {
dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation(), true);
dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation());
}
}
// Purge waypoints too far from the front of the buffer.
while (!waypoint_buffer.empty()
&& waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square) {
PopWaypoint(waypoint_buffer, actor_id, false);
}
}
// Initializing buffer if it is empty.
@ -183,9 +190,9 @@ namespace LocalizationConstants {
PushWaypoint(waypoint_buffer, actor_id, change_over_point);
}
}
// Populating the buffer.
while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front())
<= std::pow(horizon_size, 2)) {
while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
std::vector<SimpleWaypointPtr> next_waypoints = waypoint_buffer.back()->GetNextWaypoint();
uint64_t selection_index = 0u;
@ -497,13 +504,12 @@ namespace LocalizationConstants {
}
void LocalizationStage::DrawBuffer(Buffer &buffer) {
for (uint64_t i = 0u; i < buffer.size(); ++i) {
if(buffer.at(i)->GetWaypoint()->IsJunction()){
debug_helper.DrawPoint(buffer.at(i)->GetLocation() + cg::Location(0.0f,0.0f,2.0f), 0.3f, {0u, 0u, 255u}, 0.05f);
} else {
debug_helper.DrawPoint(buffer.at(i)->GetLocation() + cg::Location(0.0f,0.0f,2.0f), 0.3f, {0u, 255u, 255u}, 0.05f);
}
uint64_t buffer_size = buffer.size();
uint64_t step_size = buffer_size/10u;
for (uint64_t i = 0u; i + step_size < buffer_size; i += step_size) {
debug_helper.DrawLine(buffer.at(i)->GetLocation() + cg::Location(0.0, 0.0, 2.0),
buffer.at(i + step_size)->GetLocation() + cg::Location(0.0, 0.0, 2.0),
0.2f, {0u, 255u, 0u}, 0.05f);
}
}
@ -514,12 +520,15 @@ namespace LocalizationConstants {
track_traffic.UpdatePassingVehicle(waypoint_id, actor_id);
}
void LocalizationStage::PopWaypoint(Buffer& buffer, ActorId actor_id) {
void LocalizationStage::PopWaypoint(Buffer& buffer, ActorId actor_id, bool front_or_back) {
SimpleWaypointPtr removed_waypoint = buffer.front();
SimpleWaypointPtr remaining_waypoint = nullptr;
SimpleWaypointPtr removed_waypoint = front_or_back? buffer.front(): buffer.back();
const uint64_t removed_waypoint_id = removed_waypoint->GetId();
buffer.pop_front();
if (front_or_back) {
buffer.pop_front();
} else {
buffer.pop_back();
}
track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id);
}

2
LibCarla/source/carla/trafficmanager/LocalizationStage.h
View File

@ -153,7 +153,7 @@ namespace traffic_manager {
/// Methods to modify waypoint buffer and track traffic.
void PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint);
void PopWaypoint(Buffer& buffer, ActorId actor_id);
void PopWaypoint(Buffer& buffer, ActorId actor_id, bool front_or_back = true);
/// Method to scan for unregistered actors and update their grid positioning.
void ScanUnregisteredVehicles();

35
LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp
View File

@ -20,6 +20,7 @@ namespace PlannerConstants {
static const float CRITICAL_BRAKING_MARGIN = 0.25f;
static const float HYBRID_MODE_DT = 0.05f;
static const float EPSILON_VELOCITY = 0.001f;
static const float VERTICAL_OFFSET = 0.2f;
} // namespace PlannerConstants
using namespace PlannerConstants;
@ -80,13 +81,13 @@ namespace PlannerConstants {
const Actor actor = localization_data.actor;
const float current_deviation = localization_data.deviation;
const float current_distance = localization_data.distance;
const ActorId actor_id = actor->GetId();
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const cg::Vector3D current_velocity_vector = localization_data.velocity;
const float current_velocity = current_velocity_vector.Length();
const ActorId actor_id = actor->GetId();
const cg::Vector3D ego_heading = actor->GetTransform().GetForwardVector();
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const auto current_time = chr::system_clock::now();
// If previous state for vehicle not found, initialize state entry.
@ -116,8 +117,6 @@ namespace PlannerConstants {
if (collision_data.hazard) {
cg::Vector3D other_vehicle_velocity = collision_data.other_vehicle_velocity;
float ego_relative_velocity = (current_velocity_vector - other_vehicle_velocity).Length();
cg::Vector3D ego_heading = actor->GetTransform().GetForwardVector();
float other_velocity_along_heading = cg::Math::Dot(other_vehicle_velocity, ego_heading);
// Consider collision avoidance decisions only if there is positive relative velocity
@ -203,10 +202,11 @@ namespace PlannerConstants {
chr::duration<float> 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)) {
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;
const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT;
const float target_displacement_square = std::pow(target_displacement, 2.0f);
SimpleWaypointPtr target_interval_begin = nullptr;
SimpleWaypointPtr target_interval_end = nullptr;
@ -214,6 +214,8 @@ namespace PlannerConstants {
cg::Location vehicle_location = actor->GetLocation();
// Find the interval containing position to achieve target displacement.
const cg::Location vertical_offset(0, 0, VERTICAL_OFFSET);
const cg::Location vehicle_offset_location = vehicle_location - vertical_offset;
for (uint32_t j = 0u;
j+1 < localization_data.position_window.size() && !teleportation_interval_found;
++j) {
@ -221,10 +223,11 @@ namespace PlannerConstants {
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))
) {
cg::Vector3D relative_position = target_interval_begin->GetLocation() - vehicle_offset_location;
if (cg::Math::Dot(relative_position, ego_heading) > 0.0f
&& ((target_interval_begin->DistanceSquared(vehicle_offset_location) > target_displacement_square)
|| (target_interval_begin->DistanceSquared(vehicle_offset_location) < target_displacement_square
&& target_interval_end->DistanceSquared(vehicle_offset_location) > target_displacement_square))) {
teleportation_interval_found = true;
}
}
@ -232,9 +235,13 @@ namespace PlannerConstants {
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));
float missing_displacement = 0.0f;
const float base_displacement = target_interval_begin->Distance(vehicle_offset_location);
if (base_displacement < target_displacement) {
missing_displacement = target_displacement - base_displacement;
}
cg::Transform target_base_transform = target_interval_begin->GetTransform();
cg::Location target_base_location = target_base_transform.location;
cg::Location target_base_location = target_base_transform.location + vertical_offset;
cg::Vector3D target_heading = target_base_transform.GetForwardVector();
cg::Location teleportation_location = target_base_location
+ cg::Location(target_heading * missing_displacement);