From b4eb10b2bb050fba5674a7573fc5154d8eba3f0f Mon Sep 17 00:00:00 2001 From: Vimal Date: Mon, 21 Oct 2019 22:08:03 +0530 Subject: [PATCH] WIP:Pedestrian avoidance --- .../carla/trafficmanager/CollisionStage.cpp | 53 ++++++++++++++++--- .../carla/trafficmanager/CollisionStage.h | 7 ++- 2 files changed, 51 insertions(+), 9 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 9590ac525..297230add 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -52,6 +52,7 @@ namespace CollisionStageConstants { // Periodically check for actors not spawned by TrafficManager. if (diff.count() > 0.5f) { auto world_actors = world.GetActors()->Filter("vehicle.*"); + auto world_walker = world.GetActors()->Filter("walker"); for (auto actor: *world_actors.get()) { auto unregistered_id = actor->GetId(); if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() && @@ -59,6 +60,14 @@ namespace CollisionStageConstants { unregistered_actors.insert({unregistered_id, actor}); } } + // + for (auto walker: *world_walker.get()) { + auto unregistered_id = walker->GetId(); + if (walkers_list.find(unregistered_id) == walkers_list.end()) { + walkers_list.insert({unregistered_id, walker}); + } + } + last_world_actors_pass_instance = current_time; } @@ -66,9 +75,9 @@ namespace CollisionStageConstants { std::vector actor_ids_to_erase; for (auto actor_info: unregistered_actors) { if (actor_info.second->IsAlive()) { - vicinity_grid.UpdateGrid(actor_info.second); + vicinity_grid1.UpdateGrid(actor_info.second); } else { - vicinity_grid.EraseActor(actor_info.first); + vicinity_grid1.EraseActor(actor_info.first); actor_ids_to_erase.push_back(actor_info.first); } } @@ -76,6 +85,20 @@ namespace CollisionStageConstants { unregistered_actors.erase(actor_id); } + // Regularly update walkers_list. + std::vector walker_ids_to_erase; + for (auto actor_info: walkers_list) { + if (actor_info.second->IsAlive()) { + vicinity_grid2.UpdateGrid(actor_info.second); + } else { + vicinity_grid2.EraseActor(actor_info.first); + walker_ids_to_erase.push_back(actor_info.first); + } + } + for (auto actor_id: walker_ids_to_erase) { + walkers_list.erase(actor_id); + } + // Looping over registered actors. for (uint i = 0u; i < number_of_vehicles; ++i) { @@ -84,7 +107,9 @@ namespace CollisionStageConstants { ActorId ego_actor_id = ego_actor->GetId(); // Retrieve actors around ego actor. - std::unordered_set actor_id_list = vicinity_grid.GetActors(ego_actor); + std::unordered_set actor_id_list = vicinity_grid1.GetActors(ego_actor); + std::unordered_set walker_id_list = vicinity_grid2.GetActors(ego_actor); + actor_id_list.insert(walker_id_list.begin(), walker_id_list.end()); bool collision_hazard = false; // Check every actor in the vicinity if it poses a collision hazard. @@ -101,8 +126,9 @@ namespace CollisionStageConstants { 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); + } else if (walkers_list.find(actor_id) != walkers_list.end()) { + actor = walkers_list.at(actor_id); } - float squared_distance = cg::Math::DistanceSquared(ego_actor->GetLocation(), actor->GetLocation()); if (squared_distance <= SEARCH_RADIUS * SEARCH_RADIUS) { @@ -294,12 +320,23 @@ namespace CollisionStageConstants { } LocationList CollisionStage::GetBoundary(const Actor &actor) const { + auto actor_type = actor->GetTypeId(); + + cg::BoundingBox bbox; + cg::Location location; + cg::Vector3D heading_vector; + + if (actor_type[0] =='v') { + auto vehicle = boost::static_pointer_cast(actor); + bbox = vehicle->GetBoundingBox(); + heading_vector = vehicle->GetTransform().GetForwardVector(); + } else if (actor_type[0] =='w') { + auto walker = boost::static_pointer_cast(actor); + bbox = walker->GetBoundingBox(); + heading_vector = walker->GetTransform().GetForwardVector(); + } - auto vehicle = boost::static_pointer_cast(actor); - cg::BoundingBox bbox = vehicle->GetBoundingBox(); cg::Vector3D extent = bbox.extent; - cg::Location location = vehicle->GetLocation(); - cg::Vector3D heading_vector = vehicle->GetTransform().GetForwardVector(); heading_vector.z = 0; cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0); diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index 29e66164d..63d35825e 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -13,6 +13,7 @@ #include "boost/pointer_cast.hpp" #include "carla/client/ActorList.h" #include "carla/client/Vehicle.h" +#include "carla/client/Walker.h" #include "carla/client/World.h" #include "carla/geom/Location.h" #include "carla/geom/Math.h" @@ -66,7 +67,8 @@ namespace bg = boost::geometry; /// Reference to Carla's debug helper object. cc::DebugHelper &debug_helper; /// An object used for grid binning vehicles for faster proximity detection. - VicinityGrid vicinity_grid; + VicinityGrid vicinity_grid1; + VicinityGrid vicinity_grid2; /// The map used to connect actor ids to the array index of data frames. std::unordered_map vehicle_id_to_index; /// A structure used to keep track of actors spawned outside of traffic @@ -74,6 +76,9 @@ namespace bg = boost::geometry; std::unordered_map unregistered_actors; /// An object used to keep track of time between checking for all world /// actors. + + std::unordered_map walkers_list; + /// chr::time_point last_world_actors_pass_instance; /// Number of vehicles registered with the traffic manager. uint number_of_vehicles;