diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index 64457fb62..133f98f56 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -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 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); } diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index cc47e204a..95d3c65a9 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -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(); diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp index 4e13ce180..1a7fef66f 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp @@ -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(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(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 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);