Landmark and Curvature Anticipation for Traffic Manager and other updates (#4428)
* Added LargeMapManager * Added DVector Vector of doubles * Declaration of interface * Added AUncenteredPivotPointMesh * Added map generation from assets folder * Updates on FDVector * Added tile load on demand * Fixed load tile issues after rebasing origin * Updated FDVector * Added some auxiliary functions * Added conversion and operator * Added methods to check and configure the state * Removed const and updated printed info * Added macro to avoid logs * Added info dump functions * The asset spawn check was added to the tick * LargeMapManager added to CarlaGM * Moving to level load autocontrol * Updated with new function name, AddNewClientToConsider * Fixing forced tile load * Added operator*(float) * Disabled Localplayer * Now the streaming is handled by us * Fixed remove of considered actors * Fixed typo * Added MapOrigin to the Episode * Added Vector3DInt to LibCarla * Removed floats in Vector3DInt * Added offset to client actor's location * Fixed old cast warning * Removed float * Added explicit cast to avoid warning * Fixed client actor transform * Removed unnecessary code * Added origin offset to spawn_actor * Adding spawn points generation * Added spawn points folder in editor * Changed Spawnpoint for only Transforms * World rebase is triggered directly * Added offset when attaching actors * Detecting hero vehicle * Added LargeMapManager * Added DVector Vector of doubles * Declaration of interface * Added AUncenteredPivotPointMesh * Added map generation from assets folder * Updates on FDVector * Added tile load on demand * Fixed load tile issues after rebasing origin * Updated FDVector * Added some auxiliary functions * Added conversion and operator * Added methods to check and configure the state * Removed const and updated printed info * Added macro to avoid logs * Added info dump functions * The asset spawn check was added to the tick * LargeMapManager added to CarlaGM * Moving to level load autocontrol * Updated with new function name, AddNewClientToConsider * Fixing forced tile load * Added operator*(float) * Disabled Localplayer * Now the streaming is handled by us * Fixed remove of considered actors * Fixed typo * Added MapOrigin to the Episode * Added Vector3DInt to LibCarla * Removed floats in Vector3DInt * Added offset to client actor's location * Fixed old cast warning * Removed float * Added explicit cast to avoid warning * Fixed client actor transform * Removed unnecessary code * Added origin offset to spawn_actor * Adding spawn points generation * Added spawn points folder in editor * Changed Spawnpoint for only Transforms * World rebase is triggered directly * Added offset when attaching actors * Detecting hero vehicle * Only hero vehicles are considered to stream tiles * Little update on log macro * Adding support for ghost actors * Refactored LargeMap classes * Refactored LargeMap classes * Revert refactor * Remove files * Added dormant state to actors * Fixed spawn+rebase on new OnActorSpawn * Removed circular dependency warning * Converted FActorView into an USTRUCT * Added actor state conversion to dormant * Removed ConvertActorDormant * Added spawn and transition of dormants * Ghost to Dormant working * Replicated dormant actors * Add substraction operators * Added dormant to ghost conversion * Fixed some issues in dormant to ghost conversion * WIP! Attached actors not working properly * WIP! Attached actors not working properly * Changing several constant variables Removing sorting in LocalizationStage.cpp * Fixing variable name * Fixing variable name * WIP: Fixing hybrid mode * Added tiles loaded from umaps. * Fixed Missing includes * Debug settings are still on. Added extra points in tight curves for PID improvement. Fixed Hybrid Mode, now moves in the direction of the next waypoint. Reduced calculations in MotionPlanningStage.cpp * Removed all debug helper functions * Added FActorData classes to save and restore actor state for dormant actors. Edited all client - server calls to recognize the dormant state. FActorView now can hold null AActor* when is dormant. Large Maps now load tiled umaps instead of assets. * Fixed dormant check for attached actors * Expanded FActorData classes. Added world observer aware of dormant actors. * Extended FActorData classes with walker data. * Fixed missing includes * Removed some with editor macros. * Added virtual destructor to FActorData. * Adding includes * Fixed warnings. Added UNoWorldOffsetSceneComponent to have unreal actors unnafected by rebase. * Fixed tile alingment. * Added sensor data to restore the comunication. * Added parent hierarchy consideration to sleep/wakeup of ghost/dormant actors. * Fixed crash for sensor destruction. * Changed InMemoryMap to generate more points if two waypoints are even further than a multiple of the limit. Removed SetSimulatePhysics call when not needed. Changed PID functions to inline. * Fixed child-parent when waking up dormant actors. Added profiling traces. Fixed transformation of debug shapes in large maps. * WIP: Relocating TM-controlled vehicles near the hero vehicle * WIP: Vehicle teleportation near hero vehicle. Missing a way to place them in a better order around the map. Fixes non-deterministic seed setup. * Fixes bug where the hero vehicle was also set to not have physics. * Introduces parameter SetRespawnDormantVehicles WIP Respawning dormant vehicles * Fixed child-parent when waking up dormant actors. Added profiling traces. Fixed transformation of debug shapes in large maps. * WIP: renaming FActorView to FCarlaActor. Changing calls related to FCarlaActor to acomodate new typing. * Renaming FActorView to FCarlaActor. Changing calls related to FCarlaActor to acomodate new typing. * Now traffic lights can be spawned without controllers inside junctions. * Added functions to interface the FCarlaActor object with recorder. Started adapting recorder to new FCarlaActor object. * Fixed missing includes. * Updated CarlaRecorder and CarlaReplayer to use new FCarlaActor interface for vehicles, walkers and sensors. * Fixed missing includes. * Streaming level list now is persistant and can be saved for unreal to detect the dependencies. * Enabled packages for large maps. * Added Tiles to the MapPath.txt file. Removed debug warnings. * WIP: Fix seed when setting deterministic mode. WIP: Respawn actors when dormant Added --hero and --respawn to spawn_npc.py * Fixed traffic light trigger box placement in large maps. * Fixed traffic light warning. Set speed limit for dormant actors to 30. Fixed semantic segmentation on streaming levels. * added server bind for set_respawn_dormant_vehicles parameter * TM doesn't destroy hero vehicle if idle. ALSM stage will now update hero vehicles first. MP stage will treat dormant vehicles like physics-less in hybrid mode. Added fixed seed increment in deterministic mode. * Fixed Traffic Light issue with recorder and dormant state. Removed old ActorView files. * handles scenario where the hero vehicle does not exist * Fixing line markings semantics * Fixed recorder issues with traffic lights. * Filter out the base large map as available map * Added tile_stream_distance and actor_stream_distance to world settings. * removed unused comments. * Added IsActive functions. Dormant actors that cannot be respawned now remain dormant. * Fixed missing includes. * Fixed missing includes * Review fixes. Missing includes. * Fixed link errors. * Fixes issue with Hybrid mode in async mode. Introduces new set_boundaries_respawn_dormant_vehicles parameter. Fixes issue when reloading map. * WIP: New algorithm to move vehicles from dormant to active * Finished new policy for teleporting vehicles. Created new generate_traffic.py script, which works in sync mode. Removed spawn_npc.py script. General cleanup in Traffic Manager. * Cleanup. * Set FXAA antialiasing method * Added automatic generation of speed limit signals. * Adding some includes * Fixed speed limit component crash when waking up. * Fixed speed limit component wrong check. * Fixed speed limit not reset when waking up actors. * Fixed rare crash when traffic lights become dormant. * Added speed limit to FActorData. * Fixed generating waypoints when angle is tight * Added warning when using asynchronous mode in generate traffic. Lane changes now require more space. Vehicle switching from no physics to physics in hybrid mode will have an initial speed. Changed lower bound to 20 meters for respawn in Large Maps. * WIP: Landmark anticipation using waypoints * WIP: anticipation landmark * Added lane check to landmark search * Fixed excluded lanes * WIP: landmark anticipation * WIP: landmark anticipation * Changes to constants for path buffer and max speed Bug fix for landmarks that return nullptr Included prints for debugging * Updating constants Changed collision radius check value New max_distance for GetAllLandmakrsInDistance * Incoming turns now make vehicles break * Added variable radius when finding possible collisions Added GetMap in InMemoryMap Added TLMap in MPStage to cache TL Finished GetLandmarkTargetVelocity function General cleanup * Fix merge error Change constant for collision detection * Improved lateral PID * More PID improvements * Improved longitudinal PID + minor fixes * Tuned PID parameters * Changed algorithm for respawn using GeoGrid IDs. * Fix merge error * Fixes intersection anticipation algorithm * Removed print and debug helper init Co-authored-by: doterop <daniel.oterop@gmail.com> Co-authored-by: Axel <axellopez92@outlook.com> Co-authored-by: bernat <bernatx@gmail.com> Co-authored-by: Joel Moriana <joel.moriana@gmail.com> Co-authored-by: Axel1092 <lopez1092@hotmail.com> Co-authored-by: Axel1092 <35765780+Axel1092@users.noreply.github.com> Co-authored-by: Guillermo <glopez@cvc.uab.es>
This commit is contained in:
Jacopo Bartiromo
GitHub
parent
5912d8dc98
commit
13c08e8e9c
|
|
@ -83,9 +83,6 @@ void ALSM::Update() {
|
|||
}
|
||||
}
|
||||
|
||||
// Remove taken waypoints from last frame.
|
||||
track_traffic.ClearTakenWaypoints();
|
||||
|
||||
// Scan for new unregistered actors.
|
||||
IdentifyNewActors(world_actors);
|
||||
|
||||
|
|
|
|||
|
|
@ -41,12 +41,17 @@ void CollisionStage::Update(const unsigned long index) {
|
|||
const cg::Location ego_location = simulation_state.GetLocation(ego_actor_id);
|
||||
const Buffer &ego_buffer = buffer_map.at(ego_actor_id);
|
||||
const unsigned long look_ahead_index = GetTargetWaypoint(ego_buffer, JUNCTION_LOOK_AHEAD).second;
|
||||
const float velocity = simulation_state.GetVelocity(ego_actor_id).Length();
|
||||
|
||||
ActorIdSet overlapping_actors = track_traffic.GetOverlappingVehicles(ego_actor_id);
|
||||
std::vector<ActorId> collision_candidate_ids;
|
||||
|
||||
// Run through vehicles with overlapping paths and filter them;
|
||||
float collision_radius_square = SQUARE(MAX_COLLISION_RADIUS);
|
||||
float value = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN) + parameters.GetDistanceToLeadingVehicle(ego_actor_id);
|
||||
float collision_radius_square = value;
|
||||
if (velocity < 1.0f) {
|
||||
collision_radius_square = SQUARE(COLLISION_RADIUS_STOP) + parameters.GetDistanceToLeadingVehicle(ego_actor_id);
|
||||
}
|
||||
for (ActorId overlapping_actor_id : overlapping_actors) {
|
||||
// If actor is within maximum collision avoidance and vertical overlap range.
|
||||
const cg::Location &overlapping_actor_location = simulation_state.GetLocation(overlapping_actor_id);
|
||||
|
|
|
|||
|
|
@ -39,7 +39,6 @@ static const float PHYSICS_RADIUS = 50.0f;
|
|||
|
||||
namespace SpeedThreshold {
|
||||
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
|
||||
static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f;
|
||||
static const float AFTER_JUNCTION_MIN_SPEED = 5.0f / 3.6f;
|
||||
static const float INITIAL_PERCENTAGE_SPEED_DIFFERENCE = 30.0f;
|
||||
} // namespace SpeedThreshold
|
||||
|
|
@ -47,38 +46,34 @@ static const float INITIAL_PERCENTAGE_SPEED_DIFFERENCE = 30.0f;
|
|||
namespace PathBufferUpdate {
|
||||
static const float MAX_START_DISTANCE = 20.0f;
|
||||
static const float MINIMUM_HORIZON_LENGTH = 20.0f;
|
||||
static const float MAXIMUM_HORIZON_LENGTH = 40.0f;
|
||||
static const float HORIZON_RATE = RATE(MAXIMUM_HORIZON_LENGTH,
|
||||
MINIMUM_HORIZON_LENGTH,
|
||||
SpeedThreshold::ARBITRARY_MAX_SPEED);
|
||||
static const float HORIZON_RATE = 1.45f;
|
||||
} // namespace PathBufferUpdate
|
||||
|
||||
namespace WaypointSelection {
|
||||
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.1f;
|
||||
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 0.6f;
|
||||
static const float JUNCTION_LOOK_AHEAD = 6.0f;
|
||||
static const float SAFE_DISTANCE_AFTER_JUNCTION = 6.0f;
|
||||
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.4f;
|
||||
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 1.0f;
|
||||
static const float JUNCTION_LOOK_AHEAD = 5.0f;
|
||||
static const float SAFE_DISTANCE_AFTER_JUNCTION = 4.0f;
|
||||
static const float MIN_JUNCTION_LENGTH = 8.0f;
|
||||
static const float MIN_SAFE_INTERVAL_LENGTH = 1.5f * SAFE_DISTANCE_AFTER_JUNCTION;
|
||||
static const float MIN_SAFE_INTERVAL_LENGTH = 0.5f * SAFE_DISTANCE_AFTER_JUNCTION;
|
||||
} // namespace WaypointSelection
|
||||
|
||||
namespace LaneChange {
|
||||
static const float MINIMUM_LANE_CHANGE_DISTANCE = 18.0f;
|
||||
static const float MINIMUM_LANE_CHANGE_DISTANCE = 15.0f;
|
||||
static const float MAXIMUM_LANE_OBSTACLE_DISTANCE = 50.0f;
|
||||
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f;
|
||||
static const float INTER_LANE_CHANGE_DISTANCE = 10.0f;
|
||||
} // namespace LaneChange
|
||||
|
||||
namespace Collision {
|
||||
static const float BOUNDARY_EXTENSION_MAXIMUM = 40.0f;
|
||||
static const float BOUNDARY_EXTENSION_MINIMUM = 2.5f;
|
||||
static const float BOUNDARY_EXTENSION_RATE = RATE(BOUNDARY_EXTENSION_MAXIMUM,
|
||||
BOUNDARY_EXTENSION_MINIMUM,
|
||||
SpeedThreshold::ARBITRARY_MAX_SPEED);
|
||||
static const float BOUNDARY_EXTENSION_RATE = 1.35f;
|
||||
static const float COS_10_DEGREES = 0.9848f;
|
||||
static const float OVERLAP_THRESHOLD = 0.1f;
|
||||
static const float LOCKING_DISTANCE_PADDING = 4.0f;
|
||||
static const float MAX_COLLISION_RADIUS = 30.0f;
|
||||
static const float COLLISION_RADIUS_STOP = 8.0f;
|
||||
static const float COLLISION_RADIUS_MIN = 20.0f;
|
||||
static const float COLLISION_RADIUS_RATE = 0.5f;
|
||||
static const float MAX_LOCKING_EXTENSION = 10.0f;
|
||||
static const float WALKER_TIME_EXTENSION = 1.5f;
|
||||
static const float SQUARE_ROOT_OF_TWO = 1.414f;
|
||||
|
|
@ -96,10 +91,10 @@ static const float INV_GROWTH_STEP_SIZE = 1.0f / static_cast<float>(GROWTH_STEP_
|
|||
namespace Map {
|
||||
static const float INFINITE_DISTANCE = std::numeric_limits<float>::max();
|
||||
static const float MAX_GEODESIC_GRID_LENGTH = 20.0f;
|
||||
static const double MAP_RESOLUTION = 5.0;
|
||||
static const float MAP_RESOLUTION = 5.0f;
|
||||
static const float INV_MAP_RESOLUTION = 1.0f / MAP_RESOLUTION;
|
||||
static const double MAX_WPT_DISTANCE = MAP_RESOLUTION/2.0 + SQUARE(MAP_RESOLUTION);
|
||||
static const float INV_MAP_RESOLUTION = 0.2f;
|
||||
static const float TWENTY_DEG_TO_RAD = 0.35f;
|
||||
static const float MAX_WPT_RADIANS = 0.1745f; // 10º
|
||||
static float const DELTA = 25.0f;
|
||||
static float const Z_DELTA = 500.0f;
|
||||
} // namespace Map
|
||||
|
|
@ -113,33 +108,41 @@ namespace MotionPlan {
|
|||
static const float RELATIVE_APPROACH_SPEED = 10.0f / 3.6f;
|
||||
static const float MIN_FOLLOW_LEAD_DISTANCE = 5.0f;
|
||||
static const float MAX_FOLLOW_LEAD_DISTANCE = 10.0f;
|
||||
static const float FOLLOW_DISTANCE_RATE = RATE(MAX_FOLLOW_LEAD_DISTANCE,
|
||||
MIN_FOLLOW_LEAD_DISTANCE,
|
||||
SpeedThreshold::ARBITRARY_MAX_SPEED);
|
||||
static const float FOLLOW_DISTANCE_RATE = 0.1f;
|
||||
static const float CRITICAL_BRAKING_MARGIN = 0.25f;
|
||||
static const float EPSILON_RELATIVE_SPEED = 0.001f;
|
||||
static const float MAX_JUNCTION_BLOCK_DISTANCE = 0.5f * WaypointSelection::SAFE_DISTANCE_AFTER_JUNCTION;
|
||||
static const float MAX_JUNCTION_BLOCK_DISTANCE = 1.0f * WaypointSelection::SAFE_DISTANCE_AFTER_JUNCTION;
|
||||
static const float TWO_KM = 2000.0f;
|
||||
static const uint16_t ATTEMPTS_TO_TELEPORT = 5u;
|
||||
static const float LANDMARK_DETECTION_TIME = 2.5f;
|
||||
static const float TL_GREEN_TARGET_VELOCITY = 20.0f / 3.6f;
|
||||
static const float TL_RED_TARGET_VELOCITY = 15.0f / 3.6f;
|
||||
static const float TL_UNKNOWN_TARGET_VELOCITY = TL_RED_TARGET_VELOCITY;
|
||||
static const float STOP_TARGET_VELOCITY = 15.0f / 3.6f;
|
||||
static const float YIELD_TARGET_VELOCITY = 15.0f / 3.6f;
|
||||
static const float FRICTION = 0.6f;
|
||||
static const float GRAVITY = 9.81f;
|
||||
static const float PI = 3.1415927f;
|
||||
} // namespace MotionPlan
|
||||
|
||||
namespace PID {
|
||||
static const float MAX_THROTTLE = 0.7f;
|
||||
static const float MAX_THROTTLE = 0.85f;
|
||||
static const float MAX_BRAKE = 1.0f;
|
||||
static const float STEERING_LIMIT = 0.8f;
|
||||
static const float MAX_STEERING = 0.8f;
|
||||
static const float MAX_STEERING_DIFF = 0.15f;
|
||||
static const float VELOCITY_INTEGRAL_MAX = 5.0f;
|
||||
static const float VELOCITY_INTEGRAL_MIN = -5.0f;
|
||||
static const float DT = 0.05f;
|
||||
static const float INV_DT = 1.0f / DT;
|
||||
static const std::vector<float> LONGITUDIAL_PARAM = {2.0f, 0.01f, 0.4f};
|
||||
static const std::vector<float> LONGITUDIAL_HIGHWAY_PARAM = {4.0f, 0.02f, 0.2f};
|
||||
static const std::vector<float> LATERAL_PARAM = {9.0f, 0.02f, 1.0f};
|
||||
static const std::vector<float> LATERAL_HIGHWAY_PARAM = {7.0f, 0.02f, 1.0f};
|
||||
static const std::vector<float> LONGITUDIAL_PARAM = {3.0f, 0.01f, 0.02f};
|
||||
static const std::vector<float> LONGITUDIAL_HIGHWAY_PARAM = {3.6f, 0.01f, 0.05f};
|
||||
static const std::vector<float> LATERAL_PARAM = {5.0f, 0.02f, 1.0f};
|
||||
static const std::vector<float> LATERAL_HIGHWAY_PARAM = {3.0f, 0.02f, 0.5f};
|
||||
} // namespace PID
|
||||
|
||||
namespace TrackTraffic {
|
||||
static const uint64_t BUFFER_STEP_THROUGH = 5;
|
||||
static const float INV_BUFFER_STEP_THROUGH = 0.2f;
|
||||
static const float INV_BUFFER_STEP_THROUGH = 1.0f / static_cast<float>(BUFFER_STEP_THROUGH);
|
||||
} // namespace TrackTraffic
|
||||
|
||||
} // namespace constants
|
||||
|
|
|
|||
|
|
@ -65,6 +65,7 @@ struct StateEntry {
|
|||
float velocity;
|
||||
float deviation_integral;
|
||||
float velocity_integral;
|
||||
float steer;
|
||||
};
|
||||
|
||||
} // namespace traffic_manager
|
||||
|
|
|
|||
|
|
@ -166,7 +166,7 @@ namespace traffic_manager {
|
|||
for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
|
||||
float distance = distance_squared(segment_waypoints.at(i)->GetLocation(), segment_waypoints.at(i+1)->GetLocation());
|
||||
double angle = wpt_angle(segment_waypoints.at(i)->GetTransform().rotation.GetForwardVector(), segment_waypoints.at(i+1)->GetTransform().rotation.GetForwardVector());
|
||||
int16_t angle_splits = static_cast<int16_t>(angle/TWENTY_DEG_TO_RAD);
|
||||
int16_t angle_splits = static_cast<int16_t>(angle/MAX_WPT_RADIANS);
|
||||
int16_t distance_splits = static_cast<int16_t>(distance/MAX_WPT_DISTANCE);
|
||||
auto max_splits = max(angle_splits, distance_splits);
|
||||
if (max_splits >= 1) {
|
||||
|
|
@ -204,9 +204,10 @@ namespace traffic_manager {
|
|||
|
||||
// Adding simple waypoints to processed dense topology.
|
||||
for (auto swp: segment_waypoints) {
|
||||
// Checking whether the waypoint is a real junction.
|
||||
auto road_id = swp->GetWaypoint()->GetRoadId();
|
||||
if (swp->GetWaypoint()->IsJunction() && !is_real_junction.count(road_id)) {
|
||||
// Checking whether the waypoint is in a real junction.
|
||||
auto wpt = swp->GetWaypoint();
|
||||
auto road_id = wpt->GetRoadId();
|
||||
if (wpt->IsJunction() && !is_real_junction.count(road_id)) {
|
||||
swp->SetIsJunction(false);
|
||||
} else {
|
||||
swp->SetIsJunction(swp->GetWaypoint()->IsJunction());
|
||||
|
|
@ -378,5 +379,10 @@ namespace traffic_manager {
|
|||
return _world_map->GetName();
|
||||
}
|
||||
|
||||
cc::Map& InMemoryMap::GetMap() const {
|
||||
return *_world_map;
|
||||
}
|
||||
|
||||
|
||||
} // namespace traffic_manager
|
||||
} // namespace carla
|
||||
|
|
|
|||
|
|
@ -85,6 +85,8 @@ namespace bgi = boost::geometry::index;
|
|||
|
||||
std::string GetMapName();
|
||||
|
||||
cc::Map& GetMap() const;
|
||||
|
||||
private:
|
||||
|
||||
/// This method is used to find and place lane change links.
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ LocalizationStage::LocalizationStage(
|
|||
std::vector<ActorId>& marked_for_removal,
|
||||
LocalizationFrame &output_array,
|
||||
RandomGeneratorMap &random_devices)
|
||||
: vehicle_id_list(vehicle_id_list),
|
||||
: vehicle_id_list(vehicle_id_list),
|
||||
buffer_map(buffer_map),
|
||||
simulation_state(simulation_state),
|
||||
track_traffic(track_traffic),
|
||||
|
|
@ -39,7 +39,7 @@ void LocalizationStage::Update(const unsigned long index) {
|
|||
const float vehicle_speed = vehicle_velocity_vector.Length();
|
||||
|
||||
// Speed dependent waypoint horizon length.
|
||||
float horizon_length = std::min(vehicle_speed * HORIZON_RATE + MINIMUM_HORIZON_LENGTH, MAXIMUM_HORIZON_LENGTH);
|
||||
float horizon_length = vehicle_speed * HORIZON_RATE + MINIMUM_HORIZON_LENGTH;
|
||||
const float horizon_square = SQUARE(horizon_length);
|
||||
|
||||
if (buffer_map.find(actor_id) == buffer_map.end()) {
|
||||
|
|
@ -59,12 +59,10 @@ void LocalizationStage::Update(const unsigned long index) {
|
|||
}
|
||||
|
||||
bool is_at_junction_entrance = false;
|
||||
|
||||
if (!waypoint_buffer.empty()) {
|
||||
// Purge passed waypoints.
|
||||
float dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation());
|
||||
while (dot_product <= 0.0f && !waypoint_buffer.empty()) {
|
||||
|
||||
PopWaypoint(actor_id, track_traffic, waypoint_buffer);
|
||||
if (!waypoint_buffer.empty()) {
|
||||
dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation());
|
||||
|
|
@ -74,10 +72,18 @@ void LocalizationStage::Update(const unsigned long index) {
|
|||
if (!waypoint_buffer.empty()) {
|
||||
// Determine if the vehicle is at the entrance of a junction.
|
||||
SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
|
||||
is_at_junction_entrance = !waypoint_buffer.front()->CheckJunction() && look_ahead_point->CheckJunction();
|
||||
SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
|
||||
bool front_waypoint_junction = front_waypoint->CheckJunction();
|
||||
is_at_junction_entrance = !front_waypoint_junction && look_ahead_point->CheckJunction();
|
||||
if (!is_at_junction_entrance) {
|
||||
std::vector<SimpleWaypointPtr> last_passed_waypoints = front_waypoint->GetPreviousWaypoint();
|
||||
if (last_passed_waypoints.size() == 1) {
|
||||
is_at_junction_entrance = !last_passed_waypoints.front()->CheckJunction() && front_waypoint_junction;
|
||||
}
|
||||
}
|
||||
if (is_at_junction_entrance
|
||||
// Exception for roundabout in Town03.
|
||||
&& local_map->GetMapName() == "Town03"
|
||||
&& local_map->GetMapName() == "Carla/Maps/Town03"
|
||||
&& vehicle_location.SquaredLength() < SQUARE(30)) {
|
||||
is_at_junction_entrance = false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,7 +30,8 @@ float DeviationDotProduct(const cg::Location &reference_location,
|
|||
next_vector = next_vector.MakeSafeUnitVector(EPSILON);
|
||||
cg::Vector3D heading_vector_flat(heading_vector.x, heading_vector.y, 0);
|
||||
heading_vector_flat = heading_vector_flat.MakeSafeUnitVector(EPSILON);
|
||||
const float dot_product = cg::Math::Dot(next_vector, heading_vector_flat);
|
||||
float dot_product = cg::Math::Dot(next_vector, heading_vector_flat);
|
||||
dot_product = std::max(0.0f, std::min(dot_product, 1.0f));
|
||||
return dot_product;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,14 @@
|
|||
// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
|
||||
// de Barcelona (UAB).
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#include <limits>
|
||||
|
||||
#include "carla/client/TrafficSign.h"
|
||||
#include "carla/client/TrafficLight.h"
|
||||
#include "carla/rpc/TrafficLightState.h"
|
||||
|
||||
#include "carla/trafficmanager/Constants.h"
|
||||
#include "carla/trafficmanager/PIDController.h"
|
||||
|
|
@ -47,12 +58,22 @@ MotionPlanStage::MotionPlanStage(
|
|||
world(world),
|
||||
output_array(output_array),
|
||||
random_devices(random_devices),
|
||||
local_map(local_map) {}
|
||||
local_map(local_map) {
|
||||
|
||||
// Adding structure to avoid retrieving traffic lights when checking for landmarks.
|
||||
std::vector<SharedPtr<cc::Landmark>> traffic_lights = world.GetMap()->GetAllLandmarksOfType("1000001");
|
||||
for (auto &tl : traffic_lights) {
|
||||
std::string landmark_id = tl->GetId();
|
||||
SharedPtr<cc::Actor> actor = world.GetTrafficLight(*tl);
|
||||
tl_map.insert({landmark_id, actor});
|
||||
}
|
||||
}
|
||||
|
||||
void MotionPlanStage::Update(const unsigned long index) {
|
||||
const ActorId actor_id = vehicle_id_list.at(index);
|
||||
const cg::Location vehicle_location = simulation_state.GetLocation(actor_id);
|
||||
const cg::Vector3D vehicle_velocity = simulation_state.GetVelocity(actor_id);
|
||||
const cg::Rotation vehicle_rotation = simulation_state.GetRotation(actor_id);
|
||||
const float vehicle_speed = vehicle_velocity.Length();
|
||||
const cg::Vector3D vehicle_heading = simulation_state.GetHeading(actor_id);
|
||||
const bool vehicle_physics_enabled = simulation_state.IsPhysicsEnabled(actor_id);
|
||||
|
|
@ -63,8 +84,8 @@ void MotionPlanStage::Update(const unsigned long index) {
|
|||
current_timestamp = world.GetSnapshot().GetTimestamp();
|
||||
StateEntry current_state;
|
||||
|
||||
// Instanciating teleportation transform.
|
||||
cg::Transform teleportation_transform;
|
||||
// Instanciating teleportation transform as current vehicle transform.
|
||||
cg::Transform teleportation_transform = cg::Transform(vehicle_location, vehicle_rotation);
|
||||
|
||||
// Get information about the hero location from the actor_id state.
|
||||
cg::Location hero_location = track_traffic.GetHeroLocation();
|
||||
|
|
@ -74,7 +95,8 @@ void MotionPlanStage::Update(const unsigned long index) {
|
|||
// Flushing controller state for vehicle.
|
||||
current_state = {current_timestamp,
|
||||
0.0f, 0.0f,
|
||||
0.0f, 0.0f};
|
||||
0.0f, 0.0f,
|
||||
0.0f};
|
||||
|
||||
// Add entry to teleportation duration clock table if not present.
|
||||
if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
|
||||
|
|
@ -93,22 +115,16 @@ void MotionPlanStage::Update(const unsigned long index) {
|
|||
float random_sample = (static_cast<float>(random_devices.at(actor_id).next())*dilate_factor) + lower_bound;
|
||||
NodeList teleport_waypoint_list = local_map->GetWaypointsInDelta(hero_location, ATTEMPTS_TO_TELEPORT, random_sample);
|
||||
if (!teleport_waypoint_list.empty()) {
|
||||
teleportation_transform = teleport_waypoint_list.at(0)->GetTransform();
|
||||
for (auto &teleport_waypoint : teleport_waypoint_list) {
|
||||
uint64_t waypoint_id = teleport_waypoint->GetId();
|
||||
if (track_traffic.IsWaypointFree(waypoint_id)) {
|
||||
GeoGridId geogrid_id = teleport_waypoint->GetGeodesicGridId();
|
||||
if (track_traffic.IsGeoGridFree(geogrid_id)) {
|
||||
teleportation_transform = teleport_waypoint->GetTransform();
|
||||
track_traffic.AddTakenWaypoint(waypoint_id);
|
||||
teleportation_transform.location.z += 0.5f;
|
||||
track_traffic.AddTakenGrid(geogrid_id, actor_id);
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// No possible points were found, stay dormant.
|
||||
teleportation_transform = cg::Transform(vehicle_location, simulation_state.GetRotation(actor_id));
|
||||
}
|
||||
} else {
|
||||
// Teleport only once every dt in asynchronous mode.
|
||||
teleportation_transform = cg::Transform(vehicle_location, simulation_state.GetRotation(actor_id));
|
||||
}
|
||||
}
|
||||
output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
|
||||
}
|
||||
|
|
@ -119,6 +135,13 @@ void MotionPlanStage::Update(const unsigned long index) {
|
|||
const float vehicle_speed_limit = simulation_state.GetSpeedLimit(actor_id);
|
||||
float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, vehicle_speed_limit) / 3.6f;
|
||||
|
||||
// Algorithm to reduce speed near landmarks
|
||||
float max_landmark_target_velocity = GetLandmarkTargetVelocity(*(waypoint_buffer.at(0)), vehicle_location, actor_id, max_target_velocity);
|
||||
|
||||
// Algorithm to reduce speed near turns
|
||||
float max_turn_target_velocity = GetTurnTargetVelocity(waypoint_buffer, max_target_velocity);
|
||||
max_target_velocity = std::min(std::min(max_target_velocity, max_landmark_target_velocity), max_turn_target_velocity);
|
||||
|
||||
// Collision handling and target velocity correction.
|
||||
std::pair<bool, float> collision_response = CollisionHandling(collision_hazard, tl_hazard, vehicle_velocity,
|
||||
vehicle_heading, max_target_velocity);
|
||||
|
|
@ -140,14 +163,14 @@ void MotionPlanStage::Update(const unsigned long index) {
|
|||
const cg::Location target_location = target_waypoint->GetLocation();
|
||||
float dot_product = DeviationDotProduct(vehicle_location, vehicle_heading, target_location);
|
||||
float cross_product = DeviationCrossProduct(vehicle_location, vehicle_heading, target_location);
|
||||
dot_product = 1.0f - dot_product;
|
||||
dot_product = acos(dot_product) / PI;
|
||||
if (cross_product < 0.0f) {
|
||||
dot_product *= -1.0f;
|
||||
}
|
||||
const float current_deviation = dot_product;
|
||||
// If previous state for vehicle not found, initialize state entry.
|
||||
if (pid_state_map.find(actor_id) == pid_state_map.end()) {
|
||||
const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
pid_state_map.insert({actor_id, initial_state});
|
||||
}
|
||||
|
||||
|
|
@ -193,6 +216,7 @@ void MotionPlanStage::Update(const unsigned long index) {
|
|||
output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
|
||||
|
||||
// Updating PID state.
|
||||
current_state.steer = actuation_signal.steer;
|
||||
StateEntry &state = pid_state_map.at(actor_id);
|
||||
state = current_state;
|
||||
|
||||
|
|
@ -202,7 +226,8 @@ void MotionPlanStage::Update(const unsigned long index) {
|
|||
// Flushing controller state for vehicle.
|
||||
current_state = {current_timestamp,
|
||||
0.0f, 0.0f,
|
||||
0.0f, 0.0f};
|
||||
0.0f, 0.0f,
|
||||
0.0f};
|
||||
|
||||
// Add entry to teleportation duration clock table if not present.
|
||||
if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
|
||||
|
|
@ -250,34 +275,29 @@ bool MotionPlanStage::SafeAfterJunction(const LocalizationData &localization,
|
|||
SimpleWaypointPtr safe_point = localization.safe_point;
|
||||
|
||||
bool safe_after_junction = true;
|
||||
|
||||
if (!tl_hazard && !collision_emergency_stop
|
||||
&& localization.is_at_junction_entrance
|
||||
&& junction_end_point != nullptr && safe_point != nullptr
|
||||
&& junction_end_point->DistanceSquared(safe_point) > SQUARE(MIN_SAFE_INTERVAL_LENGTH)) {
|
||||
|
||||
ActorIdSet initial_set = track_traffic.GetPassingVehicles(junction_end_point->GetId());
|
||||
ActorIdSet passing_safe_point = track_traffic.GetPassingVehicles(safe_point->GetId());
|
||||
ActorIdSet passing_junction_end_point = track_traffic.GetPassingVehicles(junction_end_point->GetId());
|
||||
float safe_interval_length_squared = junction_end_point->DistanceSquared(safe_point);
|
||||
cg::Location mid_point = (junction_end_point->GetLocation() + safe_point->GetLocation())/2.0f;
|
||||
|
||||
// Scan through the safe interval and find if any vehicles are present in it
|
||||
// by finding their occupied waypoints.
|
||||
for (SimpleWaypointPtr current_waypoint = junction_end_point;
|
||||
current_waypoint->DistanceSquared(junction_end_point) < safe_interval_length_squared && safe_after_junction;
|
||||
current_waypoint = current_waypoint->GetNextWaypoint().front()) {
|
||||
|
||||
ActorIdSet current_set = track_traffic.GetPassingVehicles(current_waypoint->GetId());
|
||||
ActorIdSet difference;
|
||||
std::set_difference(current_set.begin(), current_set.end(),
|
||||
initial_set.begin(), initial_set.end(),
|
||||
std::inserter(difference, difference.begin()));
|
||||
if (difference.size() > 0) {
|
||||
for (const ActorId &blocking_id: difference) {
|
||||
cg::Location blocking_actor_location = simulation_state.GetLocation(blocking_id);
|
||||
if (cg::Math::DistanceSquared(blocking_actor_location, mid_point) < SQUARE(MAX_JUNCTION_BLOCK_DISTANCE)
|
||||
&& simulation_state.GetVelocity(blocking_id).SquaredLength() < SQUARE(AFTER_JUNCTION_MIN_SPEED)) {
|
||||
safe_after_junction = false;
|
||||
}
|
||||
// Only check for vehicles that have the safe point in their passing waypoint, but not
|
||||
// the junction end point.
|
||||
ActorIdSet difference;
|
||||
std::set_difference(passing_safe_point.begin(), passing_safe_point.end(),
|
||||
passing_junction_end_point.begin(), passing_junction_end_point.end(),
|
||||
std::inserter(difference, difference.begin()));
|
||||
if (difference.size() > 0) {
|
||||
for (const ActorId &blocking_id: difference) {
|
||||
cg::Location blocking_actor_location = simulation_state.GetLocation(blocking_id);
|
||||
if (cg::Math::DistanceSquared(blocking_actor_location, mid_point) < SQUARE(MAX_JUNCTION_BLOCK_DISTANCE)
|
||||
&& simulation_state.GetVelocity(blocking_id).SquaredLength() < SQUARE(AFTER_JUNCTION_MIN_SPEED)) {
|
||||
safe_after_junction = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -307,8 +327,8 @@ std::pair<bool, float> MotionPlanStage::CollisionHandling(const CollisionHazardD
|
|||
if (vehicle_relative_speed > EPSILON_RELATIVE_SPEED) {
|
||||
// If other vehicle is approaching lead vehicle and lead vehicle is further
|
||||
// than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
|
||||
float follow_lead_distance = vehicle_relative_speed * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE;
|
||||
if (available_distance_margin > follow_lead_distance) {
|
||||
float follow_lead_distance = std::min(vehicle_relative_speed * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE, MAX_FOLLOW_LEAD_DISTANCE);
|
||||
if (available_distance_margin > follow_lead_distance && other_velocity.Length() < 1.0f) {
|
||||
// Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE
|
||||
// by maintaining a relative speed of RELATIVE_APPROACH_SPEED
|
||||
dynamic_target_velocity = other_speed_along_heading + RELATIVE_APPROACH_SPEED;
|
||||
|
|
@ -333,6 +353,138 @@ std::pair<bool, float> MotionPlanStage::CollisionHandling(const CollisionHazardD
|
|||
return {collision_emergency_stop, dynamic_target_velocity};
|
||||
}
|
||||
|
||||
float MotionPlanStage::GetLandmarkTargetVelocity(const SimpleWaypoint& waypoint,
|
||||
const cg::Location vehicle_location,
|
||||
const ActorId actor_id,
|
||||
float max_target_velocity) {
|
||||
|
||||
auto const max_distance = LANDMARK_DETECTION_TIME * max_target_velocity;
|
||||
|
||||
float landmark_target_velocity = std::numeric_limits<float>::max();
|
||||
|
||||
auto all_landmarks = waypoint.GetWaypoint()->GetAllLandmarksInDistance(max_distance, false);
|
||||
|
||||
for (auto &landmark: all_landmarks) {
|
||||
|
||||
auto landmark_location = landmark->GetWaypoint()->GetTransform().location;
|
||||
auto landmark_type = landmark->GetType();
|
||||
auto distance = landmark_location.Distance(vehicle_location);
|
||||
|
||||
if (distance > max_distance) {
|
||||
continue;
|
||||
}
|
||||
|
||||
float minimum_velocity = max_target_velocity;
|
||||
if (landmark_type == "1000001") { // Traffic light
|
||||
auto landmark_id = landmark->GetId();
|
||||
if (tl_map.find(landmark_id) != tl_map.end()) {
|
||||
auto actor = tl_map.at(landmark_id);
|
||||
if (actor != nullptr) {
|
||||
|
||||
cc::TrafficLight* tl = static_cast<cc::TrafficLight*>(actor.get());
|
||||
auto state = tl->GetState();
|
||||
|
||||
if (state == carla::rpc::TrafficLightState::Green) {
|
||||
minimum_velocity = TL_GREEN_TARGET_VELOCITY;
|
||||
} else if (state == carla::rpc::TrafficLightState::Yellow || state == carla::rpc::TrafficLightState::Red){
|
||||
minimum_velocity = TL_RED_TARGET_VELOCITY;
|
||||
} else if (state == carla::rpc::TrafficLightState::Unknown){
|
||||
minimum_velocity = TL_UNKNOWN_TARGET_VELOCITY;
|
||||
} else {
|
||||
// Traffic light is off
|
||||
continue;
|
||||
}
|
||||
} else {
|
||||
// It is a traffic light, but it's not present in our structure
|
||||
minimum_velocity = TL_UNKNOWN_TARGET_VELOCITY;
|
||||
}
|
||||
} else {
|
||||
// It is a traffic light, but it's not present in our structure
|
||||
minimum_velocity = TL_UNKNOWN_TARGET_VELOCITY;
|
||||
}
|
||||
} else if (landmark_type == "206") { // Stop
|
||||
minimum_velocity = STOP_TARGET_VELOCITY;
|
||||
} else if (landmark_type == "205") { // Yield
|
||||
minimum_velocity = YIELD_TARGET_VELOCITY;
|
||||
} else if (landmark_type == "274") { // Speed limit
|
||||
float value = static_cast<float>(landmark->GetValue()) / 3.6f;
|
||||
value = parameters.GetVehicleTargetVelocity(actor_id, value);
|
||||
minimum_velocity = (value < max_target_velocity) ? value : max_target_velocity;
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
|
||||
float v = std::max(((max_target_velocity - minimum_velocity) / max_distance) * distance + minimum_velocity, minimum_velocity);
|
||||
landmark_target_velocity = std::min(landmark_target_velocity, v);
|
||||
}
|
||||
|
||||
return landmark_target_velocity;
|
||||
}
|
||||
|
||||
float MotionPlanStage::GetTurnTargetVelocity(const Buffer &waypoint_buffer,
|
||||
float max_target_velocity) {
|
||||
|
||||
if (waypoint_buffer.size() < 3) {
|
||||
return max_target_velocity;
|
||||
}
|
||||
else {
|
||||
const SimpleWaypointPtr first_waypoint = waypoint_buffer.front();
|
||||
const SimpleWaypointPtr last_waypoint = waypoint_buffer.back();
|
||||
const SimpleWaypointPtr middle_waypoint = waypoint_buffer.at(static_cast<uint16_t>(waypoint_buffer.size() / 2));
|
||||
|
||||
float radius = GetThreePointCircleRadius(first_waypoint->GetLocation(),
|
||||
middle_waypoint->GetLocation(),
|
||||
last_waypoint->GetLocation());
|
||||
|
||||
// Return the max velocity at the turn
|
||||
return std::sqrt(radius * FRICTION * GRAVITY);
|
||||
}
|
||||
}
|
||||
|
||||
float MotionPlanStage::GetThreePointCircleRadius(cg::Location first_location,
|
||||
cg::Location middle_location,
|
||||
cg::Location last_location) {
|
||||
|
||||
float x1 = first_location.x;
|
||||
float y1 = first_location.y;
|
||||
float x2 = middle_location.x;
|
||||
float y2 = middle_location.y;
|
||||
float x3 = last_location.x;
|
||||
float y3 = last_location.y;
|
||||
|
||||
float x12 = x1 - x2;
|
||||
float x13 = x1 - x3;
|
||||
float y12 = y1 - y2;
|
||||
float y13 = y1 - y3;
|
||||
float y31 = y3 - y1;
|
||||
float y21 = y2 - y1;
|
||||
float x31 = x3 - x1;
|
||||
float x21 = x2 - x1;
|
||||
|
||||
float sx13 = x1 * x1 - x3 * x3;
|
||||
float sy13 = y1 * y1 - y3 * y3;
|
||||
float sx21 = x2 * x2 - x1 * x1;
|
||||
float sy21 = y2 * y2 - y1 * y1;
|
||||
|
||||
float f_denom = 2 * (y31 * x12 - y21 * x13);
|
||||
if (f_denom == 0) {
|
||||
return std::numeric_limits<float>::max();
|
||||
}
|
||||
float f = (sx13 * x12 + sy13 * x12 + sx21 * x13 + sy21 * x13) / f_denom;
|
||||
|
||||
float g_denom = 2 * (x31 * y12 - x21 * y13);
|
||||
if (g_denom == 0) {
|
||||
return std::numeric_limits<float>::max();
|
||||
}
|
||||
float g = (sx13 * y12 + sy13 * y12 + sx21 * y13 + sy21 * y13) / g_denom;
|
||||
|
||||
float c = - (x1 * x1 + y1 * y1) - 2 * g * x1 - 2 * f * y1;
|
||||
float h = -g;
|
||||
float k = -f;
|
||||
|
||||
return std::sqrt(h * h + k * k - c);
|
||||
}
|
||||
|
||||
void MotionPlanStage::RemoveActor(const ActorId actor_id) {
|
||||
pid_state_map.erase(actor_id);
|
||||
teleportation_instance.erase(actor_id);
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@ namespace carla {
|
|||
namespace traffic_manager {
|
||||
|
||||
using LocalMapPtr = std::shared_ptr<InMemoryMap>;
|
||||
using TLMap = std::unordered_map<std::string, SharedPtr<client::Actor>>;
|
||||
|
||||
class MotionPlanStage: Stage {
|
||||
private:
|
||||
|
|
@ -43,6 +44,7 @@ private:
|
|||
cc::Timestamp current_timestamp;
|
||||
RandomGeneratorMap &random_devices;
|
||||
const LocalMapPtr &local_map;
|
||||
TLMap tl_map;
|
||||
|
||||
std::pair<bool, float> CollisionHandling(const CollisionHazardData &collision_hazard,
|
||||
const bool tl_hazard,
|
||||
|
|
@ -50,9 +52,21 @@ private:
|
|||
const cg::Vector3D ego_heading,
|
||||
const float max_target_velocity);
|
||||
|
||||
bool SafeAfterJunction(const LocalizationData &localization,
|
||||
const bool tl_hazard,
|
||||
const bool collision_emergency_stop);
|
||||
bool SafeAfterJunction(const LocalizationData &localization,
|
||||
const bool tl_hazard,
|
||||
const bool collision_emergency_stop);
|
||||
|
||||
float GetLandmarkTargetVelocity(const SimpleWaypoint& waypoint,
|
||||
const cg::Location vehicle_location,
|
||||
const ActorId actor_id,
|
||||
float max_target_velocity);
|
||||
|
||||
float GetTurnTargetVelocity(const Buffer &waypoint_buffer,
|
||||
float max_target_velocity);
|
||||
|
||||
float GetThreePointCircleRadius(cg::Location first_location,
|
||||
cg::Location middle_location,
|
||||
cg::Location last_location);
|
||||
|
||||
public:
|
||||
MotionPlanStage(const std::vector<ActorId> &vehicle_id_list,
|
||||
|
|
|
|||
|
|
@ -32,13 +32,14 @@ inline StateEntry StateUpdate(StateEntry previous_state,
|
|||
StateEntry current_state = {
|
||||
current_time,
|
||||
angular_deviation,
|
||||
(current_velocity - target_velocity) / target_velocity,
|
||||
target_velocity - current_velocity,
|
||||
0.0f,
|
||||
0.0f,
|
||||
0.0f};
|
||||
|
||||
// Calculating integrals.
|
||||
current_state.deviation_integral = angular_deviation * DT + previous_state.deviation_integral;
|
||||
current_state.velocity_integral = DT * current_state.velocity + previous_state.velocity_integral;
|
||||
current_state.velocity_integral = current_state.velocity * DT + previous_state.velocity_integral;
|
||||
|
||||
// Clamp velocity integral to avoid accumulating over-compensation
|
||||
// with time for vehicles that take a long time to reach target velocity.
|
||||
|
|
@ -63,12 +64,12 @@ inline ActuationSignal RunStep(StateEntry present_state,
|
|||
float throttle;
|
||||
float brake;
|
||||
|
||||
if (expr_v < 0.0f) {
|
||||
throttle = std::min(std::abs(expr_v), MAX_THROTTLE);
|
||||
if (expr_v > 0.0f) {
|
||||
throttle = std::min(expr_v, MAX_THROTTLE);
|
||||
brake = 0.0f;
|
||||
} else {
|
||||
throttle = 0.0f;
|
||||
brake = std::min(expr_v, MAX_BRAKE);
|
||||
brake = std::min(std::abs(expr_v), MAX_BRAKE);
|
||||
}
|
||||
|
||||
// Lateral PID calculation.
|
||||
|
|
@ -77,7 +78,8 @@ inline ActuationSignal RunStep(StateEntry present_state,
|
|||
lateral_parameters[1] * present_state.deviation_integral +
|
||||
lateral_parameters[2] * (present_state.deviation - previous_state.deviation) * INV_DT;
|
||||
|
||||
steer = std::max(-STEERING_LIMIT, std::min(steer, STEERING_LIMIT));
|
||||
steer = std::max(previous_state.steer - MAX_STEERING_DIFF, std::min(steer, previous_state.steer + MAX_STEERING_DIFF));
|
||||
steer = std::max(-MAX_STEERING, std::min(steer, MAX_STEERING));
|
||||
|
||||
return ActuationSignal{throttle, brake, steer};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -61,9 +61,6 @@ void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buff
|
|||
auto waypoint = buffer.at(std::min(i * step_size, buffer_size - 1u));
|
||||
GeoGridId ggid = waypoint->GetGeodesicGridId();
|
||||
current_grids.insert(ggid);
|
||||
if (i == 0u && taken_waypoints.find(waypoint->GetId()) == taken_waypoints.end()) {
|
||||
taken_waypoints.insert(waypoint->GetId());
|
||||
}
|
||||
// Add grid entry if not present.
|
||||
if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
|
||||
grid_to_actors.insert({ggid, {}});
|
||||
|
|
@ -79,19 +76,20 @@ void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buff
|
|||
}
|
||||
}
|
||||
|
||||
bool TrackTraffic::IsWaypointFree(const uint64_t waypoint_id) const {
|
||||
return taken_waypoints.find(waypoint_id) == taken_waypoints.end();
|
||||
|
||||
bool TrackTraffic::IsGeoGridFree(const GeoGridId geogrid_id) const {
|
||||
if (grid_to_actors.find(geogrid_id) != grid_to_actors.end()) {
|
||||
return grid_to_actors.at(geogrid_id).empty();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void TrackTraffic::AddTakenWaypoint(const uint64_t waypoint_id) {
|
||||
if (taken_waypoints.find(waypoint_id) == taken_waypoints.end()) {
|
||||
taken_waypoints.insert(waypoint_id);
|
||||
void TrackTraffic::AddTakenGrid(const GeoGridId geogrid_id, const ActorId actor_id) {
|
||||
if (grid_to_actors.find(geogrid_id) == grid_to_actors.end()) {
|
||||
grid_to_actors.insert({geogrid_id, {actor_id}});
|
||||
}
|
||||
}
|
||||
|
||||
void TrackTraffic::ClearTakenWaypoints() {
|
||||
taken_waypoints.clear();
|
||||
}
|
||||
|
||||
void TrackTraffic::SetHeroLocation(const cg::Location _location) {
|
||||
hero_location = _location;
|
||||
|
|
|
|||
|
|
@ -32,8 +32,6 @@ private:
|
|||
std::unordered_map<ActorId, std::unordered_set<GeoGridId>> actor_to_grids;
|
||||
/// Actors currently passing through grids.
|
||||
std::unordered_map<GeoGridId, ActorIdSet> grid_to_actors;
|
||||
/// Waypoints currently taken.
|
||||
WaypointIdSet taken_waypoints;
|
||||
/// Current hero location.
|
||||
cg::Location hero_location = cg::Location(0,0,0);
|
||||
|
||||
|
|
@ -51,9 +49,8 @@ public:
|
|||
const std::vector<SimpleWaypointPtr> waypoints);
|
||||
|
||||
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const;
|
||||
bool IsWaypointFree(const uint64_t waypoint_id) const;
|
||||
void AddTakenWaypoint(const uint64_t waypoint_id);
|
||||
void ClearTakenWaypoints();
|
||||
bool IsGeoGridFree(const GeoGridId geogrid_id) const;
|
||||
void AddTakenGrid(const GeoGridId geogrid_id, const ActorId actor_id);
|
||||
|
||||
void SetHeroLocation(const cg::Location location);
|
||||
cg::Location GetHeroLocation() const;
|
||||
|
|
|
|||
|
|
@ -30,33 +30,35 @@ void TrafficLightStage::Update(const unsigned long index) {
|
|||
bool traffic_light_hazard = false;
|
||||
|
||||
const ActorId ego_actor_id = vehicle_id_list.at(index);
|
||||
const Buffer &waypoint_buffer = buffer_map.at(ego_actor_id);
|
||||
const SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
|
||||
if (!simulation_state.IsDormant(ego_actor_id)) {
|
||||
const Buffer &waypoint_buffer = buffer_map.at(ego_actor_id);
|
||||
const SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
|
||||
|
||||
const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId();
|
||||
current_timestamp = world.GetSnapshot().GetTimestamp();
|
||||
const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId();
|
||||
current_timestamp = world.GetSnapshot().GetTimestamp();
|
||||
|
||||
const TrafficLightState tl_state = simulation_state.GetTLS(ego_actor_id);
|
||||
const TLS traffic_light_state = tl_state.tl_state;
|
||||
const bool is_at_traffic_light = tl_state.at_traffic_light;
|
||||
const TrafficLightState tl_state = simulation_state.GetTLS(ego_actor_id);
|
||||
const TLS traffic_light_state = tl_state.tl_state;
|
||||
const bool is_at_traffic_light = tl_state.at_traffic_light;
|
||||
|
||||
// We determine to stop if the current position of the vehicle is not a
|
||||
// junction and there is a red or yellow light.
|
||||
if (is_at_traffic_light &&
|
||||
traffic_light_state != TLS::Green &&
|
||||
traffic_light_state != TLS::Off &&
|
||||
parameters.GetPercentageRunningLight(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
|
||||
// We determine to stop if the current position of the vehicle is not a
|
||||
// junction and there is a red or yellow light.
|
||||
if (is_at_traffic_light &&
|
||||
traffic_light_state != TLS::Green &&
|
||||
traffic_light_state != TLS::Off &&
|
||||
parameters.GetPercentageRunningLight(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
|
||||
|
||||
traffic_light_hazard = true;
|
||||
}
|
||||
// Handle entry negotiation at non-signalised junction.
|
||||
else if (look_ahead_point->CheckJunction() &&
|
||||
!is_at_traffic_light &&
|
||||
traffic_light_state != TLS::Green &&
|
||||
traffic_light_state != TLS::Off &&
|
||||
parameters.GetPercentageRunningSign(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
|
||||
traffic_light_hazard = true;
|
||||
}
|
||||
// Handle entry negotiation at non-signalised junction.
|
||||
else if (look_ahead_point->CheckJunction() &&
|
||||
!is_at_traffic_light &&
|
||||
traffic_light_state != TLS::Green &&
|
||||
traffic_light_state != TLS::Off &&
|
||||
parameters.GetPercentageRunningSign(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
|
||||
|
||||
traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_timestamp);
|
||||
traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_timestamp);
|
||||
}
|
||||
}
|
||||
output_array.at(index) = traffic_light_hazard;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -162,6 +162,7 @@ void TrafficManagerLocal::Run() {
|
|||
// Updating simulation state, actor life cycle and performing necessary cleanup.
|
||||
alsm.Update();
|
||||
|
||||
|
||||
// Re-allocating inter-stage communication frames based on changed number of registered vehicles.
|
||||
int current_registered_vehicles_state = registered_vehicles.GetState();
|
||||
unsigned long number_of_vehicles = vehicle_id_list.size();
|
||||
|
|
|
|||
|
|
@ -1125,10 +1125,25 @@ def game_loop(args):
|
|||
pygame.init()
|
||||
pygame.font.init()
|
||||
world = None
|
||||
original_settings = None
|
||||
|
||||
if args.seed is not None:
|
||||
random.seed(args.seed)
|
||||
try:
|
||||
client = carla.Client(args.host, args.port)
|
||||
client.set_timeout(2.0)
|
||||
client.set_timeout(20.0)
|
||||
|
||||
sim_world = client.get_world()
|
||||
if args.sync:
|
||||
original_settings = sim_world.get_settings()
|
||||
settings = sim_world.get_settings()
|
||||
if not settings.synchronous_mode:
|
||||
settings.synchronous_mode = True
|
||||
settings.fixed_delta_seconds = 0.05
|
||||
sim_world.apply_settings(settings)
|
||||
|
||||
traffic_manager = client.get_trafficmanager()
|
||||
traffic_manager.set_synchronous_mode(True)
|
||||
|
||||
display = pygame.display.set_mode(
|
||||
(args.width, args.height),
|
||||
|
|
@ -1137,11 +1152,18 @@ def game_loop(args):
|
|||
pygame.display.flip()
|
||||
|
||||
hud = HUD(args.width, args.height)
|
||||
world = World(client.get_world(), hud, args)
|
||||
world = World(sim_world, hud, args)
|
||||
controller = KeyboardControl(world, args.autopilot)
|
||||
|
||||
if args.sync:
|
||||
sim_world.tick()
|
||||
else:
|
||||
sim_world.wait_for_tick()
|
||||
|
||||
clock = pygame.time.Clock()
|
||||
while True:
|
||||
if args.sync:
|
||||
sim_world.tick()
|
||||
clock.tick_busy_loop(60)
|
||||
if controller.parse_events(client, world, clock):
|
||||
return
|
||||
|
|
@ -1151,6 +1173,9 @@ def game_loop(args):
|
|||
|
||||
finally:
|
||||
|
||||
if original_settings:
|
||||
sim_world.apply_settings(original_settings)
|
||||
|
||||
if (world and world.recording_enabled):
|
||||
client.stop_recorder()
|
||||
|
||||
|
|
@ -1208,6 +1233,15 @@ def main():
|
|||
default=2.2,
|
||||
type=float,
|
||||
help='Gamma correction of the camera (default: 2.2)')
|
||||
argparser.add_argument(
|
||||
'-s', '--seed',
|
||||
help='Set seed for repeating executions (default: None)',
|
||||
default=None,
|
||||
type=int)
|
||||
argparser.add_argument(
|
||||
'--sync',
|
||||
action='store_true',
|
||||
help='Activate synchronous mode execution')
|
||||
args = argparser.parse_args()
|
||||
|
||||
args.width, args.height = [int(x) for x in args.res.split('x')]
|
||||
|
|
|
|||
Loading…
Reference in New Issue