Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Release candiadate #2310

Merged
merged 74 commits into from
Dec 21, 2019
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
74 commits
Select commit Hold shift + click to select a range
076e4e2
Windows compatibility changes
jackbart94 Dec 12, 2019
6f9cfbe
Windows compatibility changes pt2
jackbart94 Dec 12, 2019
fd9a4c1
Updated docs regarding Traffic Manager
marcgpuig Dec 12, 2019
a578ef0
Fixed Import folder in gitignore
marcgpuig Dec 12, 2019
2f16886
Namespaces fix
jackbart94 Dec 13, 2019
053f92e
WIP:TM modified to run synchronously with carla
pravinblaze Dec 13, 2019
40a4e66
Synchronised all stages of TrafficManager
pravinblaze Dec 14, 2019
b4dde3f
Fix for LibCarla build error
pravinblaze Dec 14, 2019
feb4ec9
Fixed collision handling with unregistered actors
pravinblaze Dec 14, 2019
3b5041c
WIP:Geodesic map gridding
pravinblaze Dec 15, 2019
e2c445b
WIP:Integrating geodesic griding with localization and collision stage
pravinblaze Dec 16, 2019
da631b3
Moved unregistered actor handling logic
pravinblaze Dec 16, 2019
552254d
Modified PerformanceDiagnostics class
pravinblaze Dec 17, 2019
b0b56b1
Fixed incorrect overlap vehicle accumulation
pravinblaze Dec 17, 2019
a873468
WIP:Improvements to collision stage throughput
pravinblaze Dec 17, 2019
3be07f0
Fixed incorrect memory handoff between stages
pravinblaze Dec 18, 2019
1c84331
Seg fault fixes for lane changes
jackbart94 Dec 18, 2019
9a19095
Removed logic for altering world settings from traffic manager
pravinblaze Dec 18, 2019
4cdea73
Fix type int to int64_t for windows compilation
bernatx Dec 18, 2019
c81871c
Fixing command batch autopilot
bernatx Dec 18, 2019
978c7ac
Set timeout for RPC calls to 5 seconds instead of 1 second, to get ti…
bernatx Dec 17, 2019
a7db151
Disable autopilot in server
bernatx Dec 18, 2019
bc91ed7
Fix uint to uint64_t for windows
bernatx Dec 18, 2019
0b030ff
Fix variable type and add parenthesis
bernatx Dec 18, 2019
eaf9be4
Merge branch 'bernatx/praveen' into praveen/traffic_manager
bernatx Dec 18, 2019
a29c182
Pipeline changes + cleaning syncr changes
jackbart94 Dec 18, 2019
264c0a4
Velocity dependent pedestrian bbox extension
pravinblaze Dec 19, 2019
2048bbf
Fixing segmentation fault in AssignLaneChange
pravinblaze Dec 19, 2019
ebcff0a
Fixed S.F when forcing lane changes
glopezdiest Dec 19, 2019
1c80603
Added UnNotifyWheeledVehicle()
marcgpuig Dec 19, 2019
53e4f93
Set some limits to the parameters.
glopezdiest Dec 19, 2019
54897ed
Merge branch 'main_traffic_manager' of https://github.com/carla-simul…
glopezdiest Dec 19, 2019
5fc0209
Fixed TM delete process
doterop Dec 19, 2019
f1a6958
Fixed merge error
doterop Dec 19, 2019
d7c0d3a
WIP PID Changes
jackbart94 Dec 19, 2019
84eeaf1
WIP:Improved Geodesic Grid Implementation
pravinblaze Dec 20, 2019
046fd59
Deleting redundant class files
pravinblaze Dec 20, 2019
e369b1a
WIP: Fixing no control flow
pravinblaze Dec 20, 2019
ba14c78
WIP:TM modified to run synchronously with carla
pravinblaze Dec 13, 2019
7634955
Synchronised all stages of TrafficManager
pravinblaze Dec 14, 2019
e5393ed
Fix for LibCarla build error
pravinblaze Dec 14, 2019
ae2b364
Fixed collision handling with unregistered actors
pravinblaze Dec 14, 2019
472ff61
WIP:Geodesic map gridding
pravinblaze Dec 15, 2019
ccb6f21
WIP:Integrating geodesic griding with localization and collision stage
pravinblaze Dec 16, 2019
cab20d7
Moved unregistered actor handling logic
pravinblaze Dec 16, 2019
77af96e
Modified PerformanceDiagnostics class
pravinblaze Dec 17, 2019
3c31c08
Fixed incorrect overlap vehicle accumulation
pravinblaze Dec 17, 2019
a1ab802
WIP:Improvements to collision stage throughput
pravinblaze Dec 17, 2019
77c738e
Fixed incorrect memory handoff between stages
pravinblaze Dec 18, 2019
ecb1fb8
Seg fault fixes for lane changes
jackbart94 Dec 18, 2019
a21b2e5
Removed logic for altering world settings from traffic manager
pravinblaze Dec 18, 2019
837576c
Fix type int to int64_t for windows compilation
bernatx Dec 18, 2019
67482f0
Fixing command batch autopilot
bernatx Dec 18, 2019
d5c52fd
Set timeout for RPC calls to 5 seconds instead of 1 second, to get ti…
bernatx Dec 17, 2019
f51c64a
Disable autopilot in server
bernatx Dec 18, 2019
d5e3cd5
Fix uint to uint64_t for windows
bernatx Dec 18, 2019
b2bb32b
Fix variable type and add parenthesis
bernatx Dec 18, 2019
835643a
Pipeline changes + cleaning syncr changes
jackbart94 Dec 18, 2019
afbffad
Velocity dependent pedestrian bbox extension
pravinblaze Dec 19, 2019
d613f1e
Fixing segmentation fault in AssignLaneChange
pravinblaze Dec 19, 2019
1d0bd98
Fixed S.F when forcing lane changes
glopezdiest Dec 19, 2019
0fe22c8
Added UnNotifyWheeledVehicle()
marcgpuig Dec 19, 2019
4726b21
Set some limits to the parameters.
glopezdiest Dec 19, 2019
efcc8a5
Fixed TM delete process
doterop Dec 19, 2019
efa31f1
Fixed merge error
doterop Dec 19, 2019
9caf585
WIP PID Changes
jackbart94 Dec 19, 2019
4b8f1a9
Modified InMemoryMap to use cc::Map::GenerateWaypoints
pravinblaze Dec 20, 2019
e074452
Turning off redundant collision avoidance algorithm
pravinblaze Dec 20, 2019
0464442
Fixing for windows compilation
bernatx Dec 20, 2019
1f8d017
Commit for testing
jackbart94 Dec 20, 2019
5c6281b
Final bug fixes
jackbart94 Dec 21, 2019
c2ea71b
Merge branch 'praveen/traffic_manager' into release_candiadate
marcgpuig Dec 21, 2019
40c00c4
Updated content version
marcgpuig Dec 21, 2019
279d74c
Adjust bounding box extension for walkers
jackbart94 Dec 21, 2019
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
WIP:Integrating geodesic griding with localization and collision stage
  • Loading branch information
pravinblaze committed Dec 16, 2019
commit e2c445b21d9727c2a8cecbc4167223b7ed294eeb
61 changes: 3 additions & 58 deletions LibCarla/source/carla/trafficmanager/CollisionStage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,11 @@ namespace CollisionStageConstants {
std::string stage_name,
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
cc::World &world,
Parameters &parameters,
cc::DebugHelper &debug_helper)
: PipelineStage(stage_name),
localization_messenger(localization_messenger),
planner_messenger(planner_messenger),
world(world),
parameters(parameters),
debug_helper(debug_helper){

Expand All @@ -57,51 +55,6 @@ namespace CollisionStageConstants {
void CollisionStage::Action() {
const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;

// ------------------------------- Move to localization utils ------------------------- //

// Handle vehicles not spawned by TrafficManager.
const auto current_time = chr::system_clock::now();
const chr::duration<double> diff = current_time - last_world_actors_pass_instance;

// Periodically check for actors not spawned by TrafficManager.
if (diff.count() > 0.5f) {

const auto world_actors = world.GetActors()->Filter("vehicle.*");
const auto world_walker = world.GetActors()->Filter("walker.*");
// Scanning for vehicles.
for (auto actor: *world_actors.get()) {
const auto unregistered_id = actor->GetId();
if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() &&
unregistered_actors.find(unregistered_id) == unregistered_actors.end()) {
unregistered_actors.insert({unregistered_id, actor});
}
}
// Scanning for pedestrians.
for (auto walker: *world_walker.get()) {
const auto unregistered_id = walker->GetId();
if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) {
unregistered_actors.insert({unregistered_id, walker});
}
}

last_world_actors_pass_instance = current_time;
}

// Regularly update unregistered actors.
std::vector<ActorId> actor_ids_to_erase;
for (auto actor_info: unregistered_actors) {
if (actor_info.second->IsAlive()) {
vicinity_grid.UpdateGrid(actor_info.second);
} else {
vicinity_grid.EraseActor(actor_info.first);
actor_ids_to_erase.push_back(actor_info.first);
}
}
for (auto actor_id: actor_ids_to_erase) {
unregistered_actors.erase(actor_id);
}
// ----------------------------------------------------------------------------------------------- //

// Looping over registered actors.
for (uint i = 0u; i < number_of_vehicles; ++i) {

Expand All @@ -112,7 +65,6 @@ namespace CollisionStageConstants {
DrawBoundary(GetGeodesicBoundary(ego_actor));

// Retrieve actors around the path of the ego vehicle.
std::unordered_set<ActorId>& actor_id_list = data.overlapping_actors;
bool collision_hazard = false;

// Generate number between 0 and 100
Expand All @@ -121,17 +73,10 @@ namespace CollisionStageConstants {
// Continue only if random number is lower than our %, default is 0.
if (parameters.GetPercentageIgnoreActors(boost::shared_ptr<cc::Actor>(ego_actor)) <= r) {
// Check every actor in the vicinity if it poses a collision hazard.
for (auto j = actor_id_list.begin(); (j != actor_id_list.end()) && !collision_hazard; ++j) {
const ActorId actor_id = *j;
for (auto j = data.overlapping_actors.begin(); (j != data.overlapping_actors.end()) && !collision_hazard; ++j) {
const Actor actor = j->second;
const ActorId actor_id = j->first;
try {

Actor actor = nullptr;
if (vehicle_id_to_index.find(actor_id) != vehicle_id_to_index.end()) {
actor = localization_frame->at(vehicle_id_to_index.at(actor_id)).actor;
} else if (unregistered_actors.find(actor_id) != unregistered_actors.end()) {
actor = unregistered_actors.at(actor_id);
}

const cg::Location ego_location = ego_actor->GetLocation();
const cg::Location other_location = actor->GetLocation();

Expand Down
6 changes: 0 additions & 6 deletions LibCarla/source/carla/trafficmanager/CollisionStage.h
Original file line number Diff line number Diff line change
Expand Up @@ -68,17 +68,12 @@ namespace traffic_manager {
/// Pointers to messenger objects.
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger;
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger;
/// Reference to Carla's world object.
cc::World &world;
/// 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, uint> vehicle_id_to_index;
/// A structure used to keep track of actors spawned outside of traffic
/// manager.
std::unordered_map<ActorId, Actor> unregistered_actors;
/// 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;
Expand Down Expand Up @@ -111,7 +106,6 @@ namespace traffic_manager {
std::string stage_name,
std::shared_ptr<LocalizationToCollisionMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
cc::World &world,
Parameters &parameters,
cc::DebugHelper &debug_helper);

Expand Down
78 changes: 74 additions & 4 deletions LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ namespace LocalizationConstants {
static const float HIGHWAY_SPEED = 50 / 3.6f;
static const float MINIMUM_LANE_CHANGE_DISTANCE = 20.0f;
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.93969f;
static const uint UNREGISTERED_ACTORS_SCAN_INTERVAL = 10;

} // namespace LocalizationConstants

Expand All @@ -32,15 +33,17 @@ namespace LocalizationConstants {
AtomicActorSet &registered_actors,
InMemoryMap &local_map,
Parameters &parameters,
cc::DebugHelper &debug_helper)
cc::DebugHelper &debug_helper,
cc::World& world)
: PipelineStage(stage_name),
planner_messenger(planner_messenger),
collision_messenger(collision_messenger),
traffic_light_messenger(traffic_light_messenger),
registered_actors(registered_actors),
local_map(local_map),
parameters(parameters),
debug_helper(debug_helper) {
debug_helper(debug_helper),
world(world) {

// Initializing various output frame selectors.
planner_frame_selector = true;
Expand All @@ -64,6 +67,8 @@ namespace LocalizationConstants {

void LocalizationStage::Action() {

ScanUnregisteredVehicles();

// Selecting output frames based on selector keys.
const auto current_planner_frame = planner_frame_selector ? planner_frame_a : planner_frame_b;
const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b;
Expand Down Expand Up @@ -216,7 +221,16 @@ namespace LocalizationConstants {
LocalizationToCollisionData &collision_message = current_collision_frame->at(i);
collision_message.actor = vehicle;
collision_message.buffer = waypoint_buffer;
collision_message.overlapping_actors = track_traffic.GetOverlappingVehicles(actor_id);
ActorIdSet overlapping_actor_set = track_traffic.GetOverlappingVehicles(actor_id);
for (ActorId overlapping_actor_id: overlapping_actor_set) {
Actor actor_ptr = nullptr;
if (vehicle_id_to_index.find(overlapping_actor_id) != vehicle_id_to_index.end()) {
actor_ptr = actor_list.at(vehicle_id_to_index.at(overlapping_actor_id));
} else if (unregistered_actors.find(overlapping_actor_id) != unregistered_actors.end()) {
actor_ptr = unregistered_actors.at(overlapping_actor_id);
}
collision_message.overlapping_actors.insert({overlapping_actor_id, actor_ptr});
}
}

LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i);
Expand Down Expand Up @@ -329,14 +343,70 @@ namespace LocalizationConstants {
buffer.push_back(waypoint);
track_traffic.UpdatePassingVehicle(waypoint_id, actor_id);

track_traffic.UpdateGridPosition(actor_id, waypoint);
}

void LocalizationStage::PopWaypoint(Buffer& buffer, ActorId actor_id) {

const uint64_t removed_waypoint_id = buffer.front()->GetId();
SimpleWaypointPtr removed_waypoint = buffer.front();
SimpleWaypointPtr remaining_waypoint = nullptr;
const uint64_t removed_waypoint_id = removed_waypoint->GetId();
buffer.pop_front();
track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id);

if (!buffer.empty()) {
remaining_waypoint = buffer.front();
}
track_traffic.RemoveGridPosition(actor_id, removed_waypoint, remaining_waypoint);
}

void LocalizationStage::ScanUnregisteredVehicles() {
++unregistered_scan_duration;
// Periodically check for actors not spawned by TrafficManager.
if (unregistered_scan_duration == UNREGISTERED_ACTORS_SCAN_INTERVAL) {
unregistered_scan_duration = 0;

const auto world_actors = world.GetActors()->Filter("vehicle.*");
const auto world_walker = world.GetActors()->Filter("walker.*");
// Scanning for vehicles.
for (auto actor: *world_actors.get()) {
const auto unregistered_id = actor->GetId();
if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() &&
unregistered_actors.find(unregistered_id) == unregistered_actors.end()) {
unregistered_actors.insert({unregistered_id, actor});
}
}
// Scanning for pedestrians.
for (auto walker: *world_walker.get()) {
const auto unregistered_id = walker->GetId();
if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) {
unregistered_actors.insert({unregistered_id, walker});
}
}
}

// Regularly update unregistered actors.
std::vector<ActorId> actor_ids_to_erase;
for (auto& actor_info: unregistered_actors) {
if (actor_info.second->IsAlive()) {
SimpleWaypointPtr nearest_waypoint = local_map.GetWaypointInVicinity(actor_info.second->GetLocation());
SimpleWaypointPtr previous_waypoint = nullptr;
if (unregistered_waypoints.find(actor_info.first) != unregistered_waypoints.end()) {
previous_waypoint = unregistered_waypoints.at(actor_info.first);
unregistered_waypoints.at(actor_info.first) = nearest_waypoint;
} else {
unregistered_waypoints.insert({actor_info.first, nearest_waypoint});
}
track_traffic.UpdateGridPosition(actor_info.first, nearest_waypoint);
track_traffic.RemoveGridPosition(actor_info.first, previous_waypoint, nearest_waypoint);
} else {
track_traffic.DeleteActor(actor_info.first);
actor_ids_to_erase.push_back(actor_info.first);
}
}
for (auto actor_id: actor_ids_to_erase) {
unregistered_actors.erase(actor_id);
}
}

SimpleWaypointPtr LocalizationStage::AssignLaneChange(Actor vehicle, bool force, bool direction) {
Expand Down
14 changes: 13 additions & 1 deletion LibCarla/source/carla/trafficmanager/LocalizationStage.h
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,8 @@ namespace traffic_manager {
Parameters &parameters;
/// Reference to Carla's debug helper object.
cc::DebugHelper &debug_helper;
/// Carla world object;
cc::World& world;
/// Structures to hold waypoint buffers for all vehicles.
/// These are shared with the collisions stage.
std::shared_ptr<BufferList> buffer_list;
Expand All @@ -101,6 +103,13 @@ namespace traffic_manager {
TrackTraffic track_traffic;
/// Map of all vehicles' idle time
std::unordered_map<ActorId, chr::time_point<chr::system_clock, chr::nanoseconds>> idle_time;
/// Counter to track unregistered actors' scan interval.
uint unregistered_scan_duration = 0;
/// A structure used to keep track of actors spawned outside of traffic
/// manager.
std::unordered_map<ActorId, Actor> unregistered_actors;
/// Structure to keep track of the closest waypoint for unregistered actors.
std::unordered_map<ActorId, SimpleWaypointPtr> unregistered_waypoints;

/// A simple method used to draw waypoint buffer ahead of a vehicle.
void DrawBuffer(Buffer &buffer);
Expand All @@ -110,6 +119,8 @@ 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);
/// Method to scan for unregistered actors and update their grid positioning.
void ScanUnregisteredVehicles();

public:

Expand All @@ -121,7 +132,8 @@ namespace traffic_manager {
AtomicActorSet &registered_actors,
InMemoryMap &local_map,
Parameters &parameters,
cc::DebugHelper &debug_helper);
cc::DebugHelper &debug_helper,
cc::World& world);

~LocalizationStage();

Expand Down
52 changes: 38 additions & 14 deletions LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ namespace traffic_manager {

if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
auto& grid_ids = actor_to_grids.at(actor_id);
if (grid_id.find(new_geo_grid_id) == grid_ids.end()) {
if (grid_ids.find(new_geo_grid_id) == grid_ids.end()) {
grid_ids.insert(new_geo_grid_id);
}
} else {
Expand All @@ -78,36 +78,60 @@ namespace traffic_manager {

void TrackTraffic::RemoveGridPosition(ActorId actor_id, SimpleWaypointPtr removed_waypoint,
SimpleWaypointPtr remaining_waypoint) {

GeoGridId removed_grid_id = removed_waypoint->GetGeodesicGridId();
GeoGridId remaining_grid_id = remaining_waypoint->GetGeodesicGridId();
GeoGridId remaining_grid_id;
if (remaining_waypoint != nullptr) {
remaining_grid_id = remaining_waypoint->GetGeodesicGridId();
}

if (removed_grid_id != remaining_grid_id) {
if (actor_to_grids.find(actor_id) != actor_to_grids.end() {
if (remaining_waypoint == nullptr || removed_grid_id != remaining_grid_id) {
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
auto& grid_ids = actor_to_grids.at(actor_id);
if (grid_ids.find(removed_grid_id) != grid_ids.end()) {
grid_ids.erase(removed_grid_id);
}
}
}

if (grid_to_actors.find(removed_grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_set = grid_to_actors.at(new_geo_grid_id);
if (actor_set.find(actor_id) != actor_set.end()) {
actor_set.erase(actor_id);
if (grid_to_actors.find(removed_grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_set = grid_to_actors.at(removed_grid_id);
if (actor_set.find(actor_id) != actor_set.end()) {
actor_set.erase(actor_id);
}
}
}
}

ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) {

if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) {
return overlapping_vehicles.at(actor_id);
} else {
return ActorIdSet();
ActorIdSet actor_id_set;
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
std::unordered_set<GeoGridId>& grid_ids = actor_to_grids.at(actor_id);
for (auto& grid_id: grid_ids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_ids = grid_to_actors.at(grid_id);
actor_id_set.insert(actor_ids.begin(), actor_ids.end());
}
}
}

return actor_id_set;
}

void TrackTraffic::DeleteActor(ActorId actor_id) {
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
std::unordered_set<GeoGridId>& grid_ids = actor_to_grids.at(actor_id);
for (auto& grid_id: grid_ids) {
if (grid_to_actors.find(grid_id) != grid_to_actors.end()) {
ActorIdSet& actor_ids = grid_to_actors.at(grid_id);
if (actor_ids.find(actor_id) != actor_ids.end()) {
actor_ids.erase(actor_id);
}
}
}
actor_to_grids.erase(actor_id);
}
}

void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {

Expand Down
Loading