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

Release candiadate (#2310)

This commit is contained in:
bernat 2019-12-21 04:59:22 +01:00 committed by Marc Garcia Puig
parent ab97355040
commit b9fbbf7fd0
41 changed files with 939 additions and 1339 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
LibCarla/source/carla/client/Vehicle.cpp
View File

@ -34,9 +34,11 @@ namespace client {
_is_control_sticky(GetControlIsSticky(GetAttributes())) {}
void Vehicle::SetAutopilot(bool enabled) {
TM &tm = TM::GetInstance(TM::GetUniqueLocalClient());
if (enabled) {
TM &tm = TM::GetInstance(TM::GetUniqueLocalClient());
tm.RegisterVehicles({shared_from_this()});
} else {
tm.UnregisterVehicles({shared_from_this()});
}
}

2
LibCarla/source/carla/client/detail/Client.cpp
View File

@ -47,7 +47,7 @@ namespace detail {
: endpoint(host + ":" + std::to_string(port)),
rpc_client(host, port),
streaming_client(host) {
rpc_client.set_timeout(1000u);
rpc_client.set_timeout(5000u);
streaming_client.AsyncRun(
worker_threads > 0u ? worker_threads : std::thread::hardware_concurrency());
}

14
LibCarla/source/carla/trafficmanager/BatchControlStage.cpp
View File

@ -17,8 +17,6 @@ namespace traffic_manager {
messenger(messenger),
carla_client(carla_client) {
// Initializing messenger state.
messenger_state = messenger->GetState();
// Initializing number of vehicles to zero in the beginning.
number_of_vehicles = 0u;
}
@ -28,7 +26,7 @@ namespace traffic_manager {
void BatchControlStage::Action() {
// Looping over registered actors.
for (uint64_t i = 0u; i < number_of_vehicles; ++i) {
for (uint64_t i = 0u; i < number_of_vehicles && data_frame != nullptr; ++i) {
cr::VehicleControl vehicle_control;
@ -44,15 +42,13 @@ namespace traffic_manager {
void BatchControlStage::DataReceiver() {
auto packet = messenger->ReceiveData(messenger_state);
data_frame = packet.data;
messenger_state = packet.id;
data_frame = messenger->Peek();
// Allocating new containers for the changed number of registered vehicles.
if (data_frame != nullptr &&
number_of_vehicles != (*data_frame.get()).size()) {
number_of_vehicles = static_cast<uint>((*data_frame.get()).size());
number_of_vehicles = static_cast<uint64_t>((*data_frame.get()).size());
// Allocating array for command batching.
commands = std::make_shared<std::vector<cr::Command>>(number_of_vehicles);
}
@ -61,12 +57,16 @@ namespace traffic_manager {
void BatchControlStage::DataSender() {
messenger->Pop();
if (commands != nullptr) {
carla_client.ApplyBatch(*commands.get());
}
// Limiting updates to 100 frames per second.
std::this_thread::sleep_for(10ms);
}
} // namespace traffic_manager

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

@ -30,8 +30,6 @@ namespace traffic_manager {
private:
/// Variable to remember messenger state.
int messenger_state;
/// Pointer to frame received from MotionPlanner.
std::shared_ptr<PlannerToControlFrame> data_frame;
/// Pointer to a messenger from MotionPlanner.

21
LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.cpp
View File

@ -1,21 +0,0 @@
// Copyright (c) 2019 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 "CarlaDataAccessLayer.h"
namespace carla {
namespace traffic_manager {
CarlaDataAccessLayer::CarlaDataAccessLayer(carla::SharedPtr<cc::Map> _world_map) {
world_map = _world_map;
}
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
std::vector<std::pair<WaypointPtr, WaypointPtr>> CarlaDataAccessLayer::GetTopology() const {
return world_map->GetTopology();
}
} // namespace traffic_manager
} // namespace carla

39
LibCarla/source/carla/trafficmanager/CarlaDataAccessLayer.h
View File

@ -1,39 +0,0 @@
// Copyright (c) 2019 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>.
#pragma once
#include <vector>
#include "carla/client/Map.h"
#include "carla/client/Waypoint.h"
#include "carla/Memory.h"
namespace carla {
namespace traffic_manager {
namespace cc = carla::client;
/// This class is responsible for retrieving data from the server.
class CarlaDataAccessLayer {
private:
/// Pointer to Carla's map object.
carla::SharedPtr<cc::Map> world_map;
public:
CarlaDataAccessLayer(carla::SharedPtr<cc::Map> world_map);
/// This method retrieves a list of topology segments from the simulator.
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
std::vector<std::pair<WaypointPtr, WaypointPtr>> GetTopology() const;
};
} // namespace traffic_manager
} // namespace carla

254
LibCarla/source/carla/trafficmanager/CollisionStage.cpp
View File

@ -12,7 +12,7 @@ namespace traffic_manager {
namespace CollisionStageConstants {
static const float VERTICAL_OVERLAP_THRESHOLD = 2.0f;
static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f;
static const float BOUNDARY_EXTENSION_MINIMUM = 1.0f;
static const float EXTENSION_SQUARE_POINT = 7.5f;
static const float TIME_HORIZON = 0.5f;
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
@ -20,7 +20,8 @@ namespace CollisionStageConstants {
static const float CRAWL_SPEED = 10.0f / 3.6f;
static const float BOUNDARY_EDGE_LENGTH = 2.0f;
static const float MAX_COLLISION_RADIUS = 100.0f;
static const float MIN_COLLISION_RADIUS = 15.0f;
static const float WALKER_TIME_EXTENSION = 1.5f;
} // namespace CollisionStageConstants
using namespace CollisionStageConstants;
@ -29,13 +30,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){
@ -43,11 +42,6 @@ namespace CollisionStageConstants {
last_world_actors_pass_instance = chr::system_clock::now();
// Initializing output array selector.
frame_selector = true;
// Initializing messenger states.
localization_messenger_state = localization_messenger->GetState();
// Initializing this messenger to preemptively write since it precedes
// motion planner stage.
planner_messenger_state = planner_messenger->GetState() - 1;
// Initializing the number of vehicles to zero in the beginning.
number_of_vehicles = 0u;
}
@ -57,57 +51,18 @@ namespace CollisionStageConstants {
void CollisionStage::Action() {
const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;
// 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() > 1.0f) {
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()) {
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);
}
last_world_actors_pass_instance = current_time;
}
// Looping over registered actors.
for (uint64_t i = 0u; i < number_of_vehicles; ++i) {
for (uint64_t i = 0u; i < number_of_vehicles && localization_frame != nullptr; ++i) {
const LocalizationToCollisionData &data = localization_frame->at(i);
LocalizationToCollisionData &data = localization_frame->at(i);
const Actor ego_actor = data.actor;
const ActorId ego_actor_id = ego_actor->GetId();
const std::unordered_map<ActorId, Actor> overlapping_actors = data.overlapping_actors;
const cg::Location ego_location = ego_actor->GetLocation();
const SimpleWaypointPtr& closest_point = data.closest_waypoint;
const SimpleWaypointPtr& junction_look_ahead = data.junction_look_ahead_waypoint;
// Retrieve actors around the path of the ego vehicle.
std::unordered_set<ActorId> actor_id_list = GetPotentialVehicleObstacles(ego_actor);
bool collision_hazard = false;
// Generate number between 0 and 100
@ -115,35 +70,31 @@ 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;
// Check every actor in the vicinity if it poses a collision hazard.
for (auto j = overlapping_actors.begin(); (j != overlapping_actors.end()) && !collision_hazard; ++j) {
const Actor actor = j->second;
const ActorId actor_id = j->first;
const cg::Location other_location = actor->GetLocation();
try {
// Collision checks increase with speed (Official formula used)
float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f);
collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS);
// Temporary fix to (0,0,0) bug
if (other_location.x != 0 && other_location.y != 0 && other_location.z != 0){
if (actor_id != ego_actor_id &&
(cg::Math::DistanceSquared(ego_location, other_location)
< std::pow(MAX_COLLISION_RADIUS, 2)) &&
(std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) {
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();
if (actor_id != ego_actor_id &&
(cg::Math::DistanceSquared(ego_location, other_location)
< std::pow(MAX_COLLISION_RADIUS, 2)) &&
(std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) {
if (parameters.GetCollisionDetection(ego_actor, actor) &&
NegotiateCollision(ego_actor, actor)) {
collision_hazard = true;
if (parameters.GetCollisionDetection(ego_actor, actor) &&
NegotiateCollision(ego_actor, actor, closest_point, junction_look_ahead)) {
collision_hazard = true;
}
}
}
}
} catch (const std::exception &e) {
carla::log_warning("Encountered problem while determining collision \n");
carla::log_info("Actor might not be alive \n");
}
@ -152,14 +103,11 @@ namespace CollisionStageConstants {
CollisionToPlannerData &message = current_planner_frame->at(i);
message.hazard = collision_hazard;
}
}
void CollisionStage::DataReceiver() {
const auto packet = localization_messenger->ReceiveData(localization_messenger_state);
localization_frame = packet.data;
localization_messenger_state = packet.id;
localization_frame = localization_messenger->Peek();
if (localization_frame != nullptr) {
// Connecting actor ids to their position indices on data arrays.
@ -175,7 +123,7 @@ namespace CollisionStageConstants {
// vehicles.
if (number_of_vehicles != (*localization_frame.get()).size()) {
number_of_vehicles = static_cast<uint>((*localization_frame.get()).size());
number_of_vehicles = static_cast<uint64_t>((*localization_frame.get()).size());
// Allocating output arrays to be shared with motion planner stage.
planner_frame_a = std::make_shared<CollisionToPlannerFrame>(number_of_vehicles);
planner_frame_b = std::make_shared<CollisionToPlannerFrame>(number_of_vehicles);
@ -185,24 +133,18 @@ namespace CollisionStageConstants {
void CollisionStage::DataSender() {
const DataPacket<std::shared_ptr<CollisionToPlannerFrame>> packet{
planner_messenger_state,
frame_selector ? planner_frame_a : planner_frame_b
};
localization_messenger->Pop();
planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b);
frame_selector = !frame_selector;
planner_messenger_state = planner_messenger->SendData(packet);
}
bool CollisionStage::NegotiateCollision(const Actor &reference_vehicle, const Actor &other_vehicle) const {
bool CollisionStage::NegotiateCollision(const Actor &reference_vehicle, const Actor &other_vehicle,
const SimpleWaypointPtr& closest_point,
const SimpleWaypointPtr& junction_look_ahead) {
bool hazard = false;
auto& data_packet = localization_frame->at(vehicle_id_to_index.at(reference_vehicle->GetId()));
Buffer& waypoint_buffer = data_packet.buffer;
auto& other_packet = localization_frame->at(vehicle_id_to_index.at(other_vehicle->GetId()));
Buffer& other_buffer = other_packet.buffer;
const cg::Location reference_location = reference_vehicle->GetLocation();
const cg::Location other_location = other_vehicle->GetLocation();
@ -210,11 +152,32 @@ namespace CollisionStageConstants {
cg::Vector3D reference_to_other = other_location - reference_location;
reference_to_other = reference_to_other.MakeUnitVector();
const cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector();
cg::Vector3D other_to_reference = reference_vehicle->GetLocation() - other_vehicle->GetLocation();
other_to_reference = other_to_reference.MakeUnitVector();
const auto &waypoint_buffer = localization_frame->at(
vehicle_id_to_index.at(reference_vehicle->GetId())).buffer;
const SimpleWaypointPtr& reference_front_wp = waypoint_buffer.front();
const auto reference_vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(reference_vehicle);
const auto other_vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(other_vehicle);
if (waypoint_buffer.front()->CheckJunction() &&
other_buffer.front()->CheckJunction()) {
float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x * 1.414f;
float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * 1.414f;
float inter_vehicle_length = reference_vehicle_length + other_vehicle_length;
if (!(!reference_front_wp->CheckJunction() &&
cg::Math::Dot(reference_heading, reference_to_other) < 0) &&
!(!closest_point->CheckJunction() && junction_look_ahead->CheckJunction() &&
reference_vehicle_ptr->GetVelocity().SquaredLength() < 0.1 &&
reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green) &&
!(!reference_front_wp->CheckJunction() &&
cg::Math::Dot(reference_heading, reference_to_other) > 0 &&
(cg::Math::DistanceSquared(reference_location, other_location) >
std::pow(GetBoundingBoxExtention(reference_vehicle) + inter_vehicle_length, 2)))) {
const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle));
const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle));
@ -227,44 +190,25 @@ namespace CollisionStageConstants {
const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
const cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector();
cg::Vector3D other_to_reference = reference_vehicle->GetLocation() - other_vehicle->GetLocation();
other_to_reference = other_to_reference.MakeUnitVector();
// Whichever vehicle's path is farthest away from the other vehicle gets
// priority to move.
if (inter_geodesic_distance < 0.1 &&
((
inter_bbox_distance > 0.1 &&
((inter_bbox_distance > 0.1 &&
reference_vehicle_to_other_geodesic > other_vehicle_to_reference_geodesic
) || (
) || (
inter_bbox_distance < 0.1 &&
cg::Math::Dot(reference_heading, reference_to_other) > cg::Math::Dot(other_heading, other_to_reference)
)) ) {
hazard = true;
}
} else if (!waypoint_buffer.front()->CheckJunction()) {
const float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x;
const float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x;
const float vehicle_length_sum = reference_vehicle_length + other_vehicle_length;
const float bbox_extension_length = GetBoundingBoxExtention(reference_vehicle);
if ((cg::Math::Dot(reference_heading, reference_to_other) > 0.0f) &&
(cg::Math::DistanceSquared(reference_location, other_location) <
std::pow(bbox_extension_length+vehicle_length_sum, 2))) {
(cg::Math::Dot(reference_heading, reference_to_other) >
cg::Math::Dot(other_heading, other_to_reference))
))
) {
hazard = true;
}
}
return hazard;
}
traffic_manager::Polygon CollisionStage::GetPolygon(const LocationList &boundary) const {
traffic_manager::Polygon CollisionStage::GetPolygon(const LocationList &boundary) {
std::string boundary_polygon_wkt;
for (const cg::Location &location: boundary) {
@ -278,7 +222,7 @@ namespace CollisionStageConstants {
return boundary_polygon;
}
LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor) const {
LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor) {
const LocationList bbox = GetBoundary(actor);
@ -299,7 +243,7 @@ namespace CollisionStageConstants {
LocationList right_boundary;
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const float width = vehicle->GetBoundingBox().extent.y;
const float length = vehicle->GetBoundingBox().extent.x;
const float length = vehicle->GetBoundingBox().extent.x*2;
SimpleWaypointPtr boundary_start = waypoint_buffer.front();
uint64_t boundary_start_index = 0u;
@ -357,9 +301,10 @@ namespace CollisionStageConstants {
return bbox;
}
}
float CollisionStage::GetBoundingBoxExtention(const Actor &actor) const {
float CollisionStage::GetBoundingBoxExtention(const Actor &actor) {
const float velocity = actor->GetVelocity().Length();
float bbox_extension = BOUNDARY_EXTENSION_MINIMUM;
@ -377,62 +322,45 @@ namespace CollisionStageConstants {
return bbox_extension;
}
std::unordered_set<ActorId> CollisionStage::GetPotentialVehicleObstacles(const Actor &ego_vehicle) {
LocationList CollisionStage::GetBoundary(const Actor &actor) {
vicinity_grid.UpdateGrid(ego_vehicle);
const auto& data_packet = localization_frame->at(vehicle_id_to_index.at(ego_vehicle->GetId()));
const Buffer &waypoint_buffer = data_packet.buffer;
const float velocity = ego_vehicle->GetVelocity().Length();
std::unordered_set<ActorId> actor_id_list = data_packet.overlapping_actors;
if (waypoint_buffer.front()->CheckJunction() && velocity < HIGHWAY_SPEED) {
actor_id_list = vicinity_grid.GetActors(ego_vehicle);
} else {
actor_id_list = data_packet.overlapping_actors;
}
return actor_id_list;
}
LocationList CollisionStage::GetBoundary(const Actor &actor) const {
const auto actor_type = actor->GetTypeId();
cg::Location location = actor->GetLocation();
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
heading_vector.z = 0.0f;
heading_vector = heading_vector.MakeUnitVector();
cg::BoundingBox bbox;
cg::Location location;
cg::Vector3D heading_vector;
float forward_extension = 0.0f;
if (actor_type[0] == 'v') {
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
bbox = vehicle->GetBoundingBox();
location = vehicle->GetLocation();
heading_vector = vehicle->GetTransform().GetForwardVector();
} else if (actor_type[0] == 'w') {
const auto walker = boost::static_pointer_cast<cc::Walker>(actor);
bbox = walker->GetBoundingBox();
location = walker->GetLocation();
heading_vector = walker->GetTransform().GetForwardVector();
// Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
forward_extension = walker->GetVelocity().Length() * WALKER_TIME_EXTENSION;
}
const cg::Vector3D extent = bbox.extent;
heading_vector.z = 0.0f;
const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
const cg::Vector3D x_boundary_vector = heading_vector * (extent.x + forward_extension);
const cg::Vector3D y_boundary_vector = perpendicular_vector * (extent.y + forward_extension);
// Four corners of the vehicle in top view clockwise order (left-handed
// system).
const cg::Vector3D x_boundary_vector = heading_vector * extent.x;
const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y;
return {
location + cg::Location(x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
location + cg::Location(x_boundary_vector + y_boundary_vector),
LocationList bbox_boundary = {
location + cg::Location(x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
location + cg::Location(x_boundary_vector + y_boundary_vector),
};
return bbox_boundary;
}
void CollisionStage::DrawBoundary(const LocationList &boundary) const {
void CollisionStage::DrawBoundary(const LocationList &boundary) {
for (uint64_t i = 0u; i < boundary.size(); ++i) {
debug_helper.DrawLine(
boundary[i] + cg::Location(0.0f, 0.0f, 1.0f),

34
LibCarla/source/carla/trafficmanager/CollisionStage.h
View File

@ -31,8 +31,8 @@
#include "carla/trafficmanager/MessengerAndDataTypes.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PerformanceDiagnostics.h"
#include "carla/trafficmanager/PipelineStage.h"
#include "carla/trafficmanager/VicinityGrid.h"
namespace carla {
namespace traffic_manager {
@ -56,9 +56,6 @@ namespace traffic_manager {
private:
/// Variables to remember messenger states.
int localization_messenger_state;
int planner_messenger_state;
/// Selection key for switching between output frames.
bool frame_selector;
/// Pointer to data received from localization stage.
@ -69,47 +66,41 @@ 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;
/// An object used for grid binning vehicles for faster proximity detection.
VicinityGrid vicinity_grid;
/// 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;
std::unordered_map<ActorId, uint64_t> vehicle_id_to_index;
/// 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;
/// Number of vehicles registered with the traffic manager.
uint64_t number_of_vehicles;
/// Snippet profiler for measuring execution time.
SnippetProfiler snippet_profiler;
/// Returns the bounding box corners of the vehicle passed to the method.
LocationList GetBoundary(const Actor &actor) const;
LocationList GetBoundary(const Actor &actor);
/// Returns the extrapolated bounding box of the vehicle along its
/// trajectory.
LocationList GetGeodesicBoundary(const Actor &actor) const;
LocationList GetGeodesicBoundary(const Actor &actor);
/// Method to construct a boost polygon object.
Polygon GetPolygon(const LocationList &boundary) const;
Polygon GetPolygon(const LocationList &boundary);
/// The method returns true if ego_vehicle should stop and wait for
/// other_vehicle to pass.
bool NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle) const;
bool NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle,
const SimpleWaypointPtr& closest_point,
const SimpleWaypointPtr& junction_look_ahead);
/// Method to calculate the speed dependent bounding box extention for a vehicle.
float GetBoundingBoxExtention(const Actor &ego_vehicle) const;
/// Method to retreive the set of vehicles around the path of the given vehicle.
std::unordered_set<ActorId> GetPotentialVehicleObstacles(const Actor &ego_vehicle);
float GetBoundingBoxExtention(const Actor &ego_vehicle);
/// A simple method used to draw bounding boxes around vehicles
void DrawBoundary(const LocationList &boundary) const;
void DrawBoundary(const LocationList &boundary);
public:
@ -117,7 +108,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);

240
LibCarla/source/carla/trafficmanager/InMemoryMap.cpp
View File

@ -11,24 +11,21 @@ namespace traffic_manager {
namespace MapConstants {
// Very important that this is less than 10^-4.
static const float ZERO_LENGTH = 0.0001f;
static const float INFINITE_DISTANCE = std::numeric_limits<float>::max();
static const uint64_t LANE_CHANGE_LOOK_AHEAD = 5u;
// Cosine of the angle.
static const float LANE_CHANGE_ANGULAR_THRESHOLD = 0.5f;
static const float GRID_SIZE = 4.0f;
static const float PED_GRID_SIZE = 10.0f;
static const float MAX_GEODESIC_GRID_LENGTH = 20.0f;
} // namespace MapConstants
namespace cg = carla::geom;
using namespace MapConstants;
InMemoryMap::InMemoryMap(TopologyList topology) {
_topology = topology;
InMemoryMap::InMemoryMap(RawNodeList _raw_dense_topology) {
raw_dense_topology = _raw_dense_topology;
}
InMemoryMap::~InMemoryMap() {}
void InMemoryMap::SetUp(float sampling_resolution) {
void InMemoryMap::SetUp() {
NodeList entry_node_list;
NodeList exit_node_list;
@ -39,110 +36,131 @@ namespace MapConstants {
};
auto square = [](float input) {return std::pow(input, 2);};
// Creating dense topology.
for (auto &pair : _topology) {
// Consuming the raw dense topology from cc::Map into SimpleWaypoints.
std::map<std::pair<crd::RoadId, crd::LaneId>, std::vector<SimpleWaypointPtr>> segment_map;
for (auto& waypoint_ptr: raw_dense_topology) {
auto road_id = waypoint_ptr->GetRoadId();
auto lane_id = waypoint_ptr->GetLaneId();
if (segment_map.find({road_id, lane_id}) != segment_map.end()) {
segment_map.at({road_id, lane_id}).push_back(std::make_shared<SimpleWaypoint>(waypoint_ptr));
} else {
segment_map.insert({{road_id, lane_id}, {std::make_shared<SimpleWaypoint>(waypoint_ptr)}});
}
}
// Looping through every topology segment.
const WaypointPtr begin_waypoint = pair.first;
const WaypointPtr end_waypoint = pair.second;
const cg::Location begin_location = begin_waypoint->GetTransform().location;
const cg::Location end_location = end_waypoint->GetTransform().location;
auto compare_s = [] (const SimpleWaypointPtr& swp1, const SimpleWaypointPtr& swp2) {
return (swp1->GetWaypoint()->GetDistance() < swp2->GetWaypoint()->GetDistance());
};
if (distance_squared(begin_location, end_location) > square(ZERO_LENGTH)) {
GeoGridId geodesic_grid_id_counter = -1;
for (auto& segment: segment_map) {
// Adding entry waypoint.
WaypointPtr current_waypoint = begin_waypoint;
dense_topology.push_back(std::make_shared<SimpleWaypoint>(current_waypoint));
// Generating geodesic grid ids.
++geodesic_grid_id_counter;
entry_node_list.push_back(dense_topology.back());
// Ordering waypoints to be consecutive.
auto& segment_waypoints = segment.second;
std::sort(segment_waypoints.begin(), segment_waypoints.end(), compare_s);
// Populating waypoints from begin_waypoint to end_waypoint.
while (distance_squared(current_waypoint->GetTransform().location,
end_location) > square(sampling_resolution)) {
if (segment_waypoints.front()->DistanceSquared(segment_waypoints.back()) > square(0.1f)) {
current_waypoint = current_waypoint->GetNext(sampling_resolution)[0];
SimpleWaypointPtr previous_wp = dense_topology.back();
dense_topology.push_back(std::make_shared<SimpleWaypoint>(current_waypoint));
SimpleWaypointPtr first_point = segment_waypoints.at(0);
SimpleWaypointPtr second_point = segment_waypoints.at(1);
cg::Vector3D first_to_second = second_point->GetLocation() - first_point->GetLocation();
first_to_second = first_to_second.MakeUnitVector();
cg::Vector3D first_heading = first_point->GetForwardVector();
previous_wp->SetNextWaypoint({dense_topology.back()});
if (cg::Math::Dot(first_heading, first_to_second) < 0.0f) {
std::reverse(segment_waypoints.begin(), segment_waypoints.end());
}
// Adding exit waypoint.
SimpleWaypointPtr previous_wp = dense_topology.back();
dense_topology.push_back(std::make_shared<SimpleWaypoint>(end_waypoint));
// Registering segment end points.
entry_node_list.push_back(segment_waypoints.front());
exit_node_list.push_back(segment_waypoints.back());
previous_wp->SetNextWaypoint({dense_topology.back()});
exit_node_list.push_back(dense_topology.back());
// Placing intra-segment connections.
cg::Location grid_edge_location = segment_waypoints.front()->GetLocation();
for (uint64_t i=0; i< segment_waypoints.size() -1; ++i) {
// Assigning grid id.
if (distance_squared(grid_edge_location, segment_waypoints.at(i)->GetLocation()) >
square(MAX_GEODESIC_GRID_LENGTH)) {
++geodesic_grid_id_counter;
grid_edge_location = segment_waypoints.at(i)->GetLocation();
}
segment_waypoints.at(i)->SetGeodesicGridId(geodesic_grid_id_counter);
segment_waypoints.at(i)->SetNextWaypoint({segment_waypoints.at(i+1)});
segment_waypoints.at(i+1)->SetPreviousWaypoint({segment_waypoints.at(i)});
}
segment_waypoints.back()->SetGeodesicGridId(geodesic_grid_id_counter);
// Adding to processed dense topology.
for (auto swp: segment_waypoints) {
dense_topology.push_back(swp);
}
}
}
// Localizing waypoints into grids.
for (auto &simple_waypoint: dense_topology) {
if (simple_waypoint != nullptr) {
const cg::Location loc = simple_waypoint->GetLocation();
const std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, true));
if (waypoint_grid.find(grid_key) == waypoint_grid.end()) {
waypoint_grid.insert({grid_key, {simple_waypoint}});
} else {
waypoint_grid.at(grid_key).insert(simple_waypoint);
}
const std::string ped_grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, false));
if (ped_waypoint_grid.find(ped_grid_key) == ped_waypoint_grid.end()) {
ped_waypoint_grid.insert({ped_grid_key, {simple_waypoint}});
} else {
ped_waypoint_grid.at(ped_grid_key).insert(simple_waypoint);
}
}
}
// Linking segments.
uint64_t i = 0u, j = 0u;
for (SimpleWaypointPtr end_point : exit_node_list) {
for (SimpleWaypointPtr begin_point : entry_node_list) {
if (end_point->DistanceSquared(begin_point) < square(ZERO_LENGTH) && i != j) {
j = 0u;
for (SimpleWaypointPtr begin_point: entry_node_list) {
if ((end_point->DistanceSquared(begin_point) < square(2.0f)) && (i != j)) {
end_point->SetNextWaypoint({begin_point});
begin_point->SetPreviousWaypoint({end_point});
}
++j;
++j;
}
++i;
}
// Tying up loose ends.
// Loop through all exit nodes of topology segments,
// connect any dangling endpoints to the nearest entry point
// of another topology segment.
i = 0u;
for (auto &end_point : exit_node_list) {
if (end_point->GetNextWaypoint().size() == 0) {
j = 0u;
float min_distance = INFINITE_DISTANCE;
SimpleWaypointPtr closest_connection;
for (auto &begin_point : entry_node_list) {
float new_distance = end_point->DistanceSquared(begin_point);
if (new_distance < min_distance && i != j) {
min_distance = new_distance;
closest_connection = begin_point;
}
++j;
}
const cg::Vector3D end_point_vector = end_point->GetForwardVector();
cg::Vector3D relative_vector = closest_connection->GetLocation() - end_point->GetLocation();
relative_vector = relative_vector.MakeUnitVector();
const float relative_dot = cg::Math::Dot(end_point_vector, relative_vector);
if (relative_dot < LANE_CHANGE_ANGULAR_THRESHOLD) {
uint64_t count = LANE_CHANGE_LOOK_AHEAD;
while (count > 0u) {
closest_connection = closest_connection->GetNextWaypoint()[0];
--count;
}
}
end_point->SetNextWaypoint({closest_connection});
}
++i;
}
// Localizing waypoints into grids.
for (auto &simple_waypoint: dense_topology) {
const cg::Location loc = simple_waypoint->GetLocation();
const std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y));
if (waypoint_grid.find(grid_key) == waypoint_grid.end()) {
waypoint_grid.insert({grid_key, {simple_waypoint}});
} else {
waypoint_grid.at(grid_key).insert(simple_waypoint);
}
}
// Linking lane change connections.
for (auto &simple_waypoint:dense_topology) {
if (!simple_waypoint->CheckJunction()) {
FindAndLinkLaneChange(simple_waypoint);
}
}
// Linking any unconnected segments.
for (auto& swp: dense_topology) {
if (swp->GetNextWaypoint().size() == 0) {
SimpleWaypointPtr nearest_sample = GetWaypointInVicinity(swp->GetLocation()
+ cg::Location(swp->GetForwardVector() * 0.2f));
swp->SetNextWaypoint({nearest_sample});
nearest_sample->SetPreviousWaypoint({swp});
}
}
MakeGeodesiGridCenters();
}
std::pair<int, int> InMemoryMap::MakeGridId (float x, float y) {
return {static_cast<int>(std::floor(x/GRID_SIZE)), static_cast<int>(std::floor(y/GRID_SIZE))};
std::pair<int, int> InMemoryMap::MakeGridId (float x, float y, bool vehicle_or_pedestrian) {
if (vehicle_or_pedestrian) {
return {static_cast<int>(std::floor(x/GRID_SIZE)), static_cast<int>(std::floor(y/GRID_SIZE))};
} else {
return {static_cast<int>(std::floor(x/PED_GRID_SIZE)), static_cast<int>(std::floor(y/PED_GRID_SIZE))};
}
}
std::string InMemoryMap::MakeGridKey (std::pair<int , int> grid_key) {
@ -151,7 +169,7 @@ namespace MapConstants {
SimpleWaypointPtr InMemoryMap::GetWaypointInVicinity(cg::Location location) {
const std::pair<int, int> grid_ids = MakeGridId(location.x, location.y);
const std::pair<int, int> grid_ids = MakeGridId(location.x, location.y, true);
SimpleWaypointPtr closest_waypoint = nullptr;
float closest_distance = INFINITE_DISTANCE;
@ -189,6 +207,38 @@ namespace MapConstants {
return closest_waypoint;
}
SimpleWaypointPtr InMemoryMap::GetPedWaypoint(cg::Location location) {
const std::pair<int, int> grid_ids = MakeGridId(location.x, location.y, false);
SimpleWaypointPtr closest_waypoint = nullptr;
float closest_distance = INFINITE_DISTANCE;
// Search all surrounding grids for closest waypoint.
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j});
if (ped_waypoint_grid.find(grid_key) != ped_waypoint_grid.end()) {
const auto& waypoint_set = ped_waypoint_grid.at(grid_key);
if (closest_waypoint == nullptr) {
closest_waypoint = *waypoint_set.begin();
}
for (auto &simple_waypoint: waypoint_set) {
if (simple_waypoint->DistanceSquared(location) < std::pow(closest_distance, 2)) {
closest_waypoint = simple_waypoint;
closest_distance = simple_waypoint->DistanceSquared(location);
}
}
}
}
}
return closest_waypoint;
}
SimpleWaypointPtr InMemoryMap::GetWaypoint(const cg::Location &location) const {
SimpleWaypointPtr closest_waypoint;
@ -255,5 +305,27 @@ namespace MapConstants {
}
}
void InMemoryMap::MakeGeodesiGridCenters() {
for (auto& swp: dense_topology) {
GeoGridId ggid = swp->CheckJunction()? swp->GetJunctionId(): swp->GetGeodesicGridId();
if (geodesic_grid_center.find(ggid) == geodesic_grid_center.end()) {
geodesic_grid_center.insert({ggid, swp->GetLocation()});
} else {
cg::Location& grid_loc = geodesic_grid_center.at(ggid);
grid_loc = (grid_loc + swp->GetLocation())/2;
}
}
}
cg::Location InMemoryMap::GetGeodesicGridCenter(GeoGridId ggid) {
cg::Location grid_center;
if (geodesic_grid_center.find(ggid) != geodesic_grid_center.end()) {
grid_center = geodesic_grid_center.at(ggid);
} else {
grid_center = cg::Location();
}
return grid_center;
}
} // namespace traffic_manager
} // namespace carla

23
LibCarla/source/carla/trafficmanager/InMemoryMap.h
View File

@ -18,6 +18,7 @@
#include "carla/geom/Math.h"
#include "carla/Memory.h"
#include "carla/road/Lane.h"
#include "carla/road/RoadTypes.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
@ -32,6 +33,8 @@ namespace traffic_manager {
using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using NodeList = std::vector<SimpleWaypointPtr>;
using RawNodeList = std::vector<WaypointPtr>;
using GeoGridId = crd::JuncId;
/// This class builds a discretized local map-cache.
/// Instantiate the class with map topology from the simulator
@ -41,15 +44,20 @@ namespace traffic_manager {
private:
/// Object to hold sparse topology received by the constructor.
TopologyList _topology;
RawNodeList raw_dense_topology;
/// Structure to hold all custom waypoint objects after
/// interpolation of sparse topology.
NodeList dense_topology;
/// Grid localization map for all waypoints in the system.
std::unordered_map<std::string, std::unordered_set<SimpleWaypointPtr>> waypoint_grid;
using WaypointGrid = std::unordered_map<std::string, std::unordered_set<SimpleWaypointPtr>>;
WaypointGrid waypoint_grid;
/// Larger localization map for all waypoints to be used for localizing pedestrians.
WaypointGrid ped_waypoint_grid;
/// Geodesic grid topology.
std::unordered_map<GeoGridId, cg::Location> geodesic_grid_center;
/// Method to generate the grid ids for given co-ordinates.
std::pair<int, int> MakeGridId(float x, float y);
std::pair<int, int> MakeGridId(float x, float y, bool vehicle_or_pedestrian);
/// Method to generate map key for waypoint_grid.
std::string MakeGridKey(std::pair<int, int> gird_id);
@ -59,12 +67,12 @@ namespace traffic_manager {
public:
InMemoryMap(TopologyList topology);
InMemoryMap(RawNodeList _raw_dense_topology);
~InMemoryMap();
/// This method constructs the local map with a resolution of
/// sampling_resolution.
void SetUp(float sampling_resolution);
void SetUp();
/// This method returns the closest waypoint to a given location on the map.
SimpleWaypointPtr GetWaypoint(const cg::Location &location) const;
@ -72,10 +80,15 @@ namespace traffic_manager {
/// This method returns closest waypoint in the vicinity of the given co-ordinates.
SimpleWaypointPtr GetWaypointInVicinity(cg::Location location);
SimpleWaypointPtr GetPedWaypoint(cg::Location location);
/// This method returns the full list of discrete samples of the map in the
/// local cache.
NodeList GetDenseTopology() const;
void MakeGeodesiGridCenters();
cg::Location GetGeodesicGridCenter(GeoGridId ggid);
};
} // namespace traffic_manager

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

@ -11,14 +11,15 @@ namespace traffic_manager {
namespace LocalizationConstants {
static const float WAYPOINT_TIME_HORIZON = 3.0f;
static const float WAYPOINT_TIME_HORIZON = 5.0f;
static const float MINIMUM_HORIZON_LENGTH = 30.0f;
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f;
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 4.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 / 3.6f;
static const float MINIMUM_LANE_CHANGE_DISTANCE = 20.0f;
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.93969f;
static const float MINIMUM_LANE_CHANGE_DISTANCE = 50.0f;
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f;
static const uint64_t UNREGISTERED_ACTORS_SCAN_INTERVAL = 10;
} // namespace LocalizationConstants
@ -32,7 +33,8 @@ 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),
@ -40,36 +42,33 @@ namespace LocalizationConstants {
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;
collision_frame_selector = true;
collision_frame_ready = false;
traffic_light_frame_selector = true;
// Initializing the number of vehicles to zero in the begining.
number_of_vehicles = 0u;
// Initializing messenger states to initiate data writes
// preemptively since this is the first stage in the pipeline.
planner_messenger_state = planner_messenger->GetState() - 1;
collision_messenger_state = collision_messenger->GetState() - 1;
traffic_light_messenger_state = traffic_light_messenger->GetState() - 1;
// Initializing the registered actors container state.
registered_actors_state = -1;
}
LocalizationStage::~LocalizationStage() {}
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;
const auto current_traffic_light_frame =
traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b;
// Looping over registered actors.
for (uint64_t i = 0u; i < actor_list.size(); ++i) {
@ -142,12 +141,23 @@ namespace LocalizationConstants {
uint64_t selection_index = 0u;
// Pseudo-randomized path selection if found more than one choice.
if (next_waypoints.size() > 1) {
selection_index = static_cast<uint>(rand()) % next_waypoints.size();
selection_index = static_cast<uint64_t>(rand()) % next_waypoints.size();
}
PushWaypoint(waypoint_buffer, actor_id, next_waypoints.at(selection_index));
SimpleWaypointPtr next_wp = next_waypoints.at(selection_index);
if (next_wp == nullptr) {
for (auto& wp: next_waypoints) {
if (wp != nullptr) {
next_wp = wp;
break;
}
}
}
PushWaypoint(waypoint_buffer, actor_id, next_wp);
}
// Updating geodesic grid position for actor.
track_traffic.UpdateGridPosition(actor_id, waypoint_buffer);
// Generating output.
const float target_point_distance = std::max(std::ceil(vehicle_velocity * TARGET_WAYPOINT_TIME_HORIZON),
TARGET_WAYPOINT_HORIZON_LENGTH);
@ -188,7 +198,8 @@ namespace LocalizationConstants {
}
bool approaching_junction = false;
if (waypoint_buffer.front()->CheckJunction() || (look_ahead_point->CheckJunction() && !(waypoint_buffer.front()->CheckJunction()))) {
if (waypoint_buffer.front()->CheckJunction() ||
(look_ahead_point->CheckJunction() && !(waypoint_buffer.front()->CheckJunction()))) {
if (speed_limit > HIGHWAY_SPEED) {
for (uint64_t j = 0u; (j < look_ahead_index) && !approaching_junction; ++j) {
SimpleWaypointPtr swp = waypoint_buffer.at(j);
@ -201,30 +212,6 @@ namespace LocalizationConstants {
}
}
// Clean up tracking list by remove vehicles that are too far away.
const ActorIdSet current_tracking_list = track_traffic.GetOverlappingVehicles(actor_id);
for (ActorId tracking_id: current_tracking_list) {
if (!waypoint_buffer.empty()) {
const cg::Location tracking_location = actor_list.at(
vehicle_id_to_index.at(tracking_id))->GetLocation();
const cg::Location buffer_front_loc = waypoint_buffer.front()->GetLocation();
const cg::Location buffer_mid_lock = waypoint_buffer.at(
static_cast<unsigned long>(std::floor(waypoint_buffer.size()/2)))->GetLocation();
const cg::Location buffer_back_loc = waypoint_buffer.back()->GetLocation();
const double squared_buffer_length = std::pow(
buffer_front_loc.Distance(buffer_mid_lock) +
buffer_mid_lock.Distance(buffer_back_loc), 2);
if (cg::Math::DistanceSquared(vehicle_location, tracking_location) > squared_buffer_length) {
track_traffic.RemoveOverlappingVehicle(actor_id, tracking_id);
track_traffic.RemoveOverlappingVehicle(tracking_id, actor_id);
}
}
}
// Editing output frames.
LocalizationToPlannerData &planner_message = current_planner_frame->at(i);
planner_message.actor = vehicle;
@ -232,28 +219,28 @@ namespace LocalizationConstants {
planner_message.distance = distance;
planner_message.approaching_true_junction = approaching_junction;
// Reading current messenger state of the collision stage before modifying it's frame.
if ((collision_messenger->GetState() != collision_messenger_state) &&
!collision_frame_ready) {
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);
LocalizationToCollisionData &collision_message = current_collision_frame->at(i);
collision_message.actor = vehicle;
collision_message.buffer = waypoint_buffer;
collision_message.overlapping_actors.clear();
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});
}
collision_message.closest_waypoint = waypoint_buffer.front();
collision_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index);
LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i);
traffic_light_message.actor = vehicle;
traffic_light_message.closest_waypoint = waypoint_buffer.front();
traffic_light_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index);
}
if ((collision_messenger->GetState() != collision_messenger_state)
&& !collision_frame_ready) {
collision_frame_ready = true;
}
}
void LocalizationStage::DataReceiver() {
@ -264,21 +251,18 @@ namespace LocalizationConstants {
if (registered_actors_state != registered_actors.GetState()) {
actor_list = registered_actors.GetList();
uint64_t index = 0u;
for (auto &actor: actor_list) {
vehicle_id_to_index.insert({actor->GetId(), index});
++index;
}
registered_actors_state = registered_actors.GetState();
}
// Allocating new containers for the changed number of registered vehicles.
if (number_of_vehicles != actor_list.size()) {
number_of_vehicles = static_cast<uint>(actor_list.size());
number_of_vehicles = static_cast<uint64_t>(actor_list.size());
// Allocating the buffer lists.
buffer_list = std::make_shared<BufferList>(number_of_vehicles);
// Allocating output frames to be shared with the motion planner stage.
@ -296,46 +280,15 @@ namespace LocalizationConstants {
void LocalizationStage::DataSender() {
// Since send/receive calls on messenger objects can block if the other
// end hasn't received/sent data, choose to block on only those stages
// which takes the most priority (which needs the highest rate of data feed)
// to run the system well.
const DataPacket<std::shared_ptr<LocalizationToPlannerFrame>> planner_data_packet = {
planner_messenger_state,
planner_frame_selector ? planner_frame_a : planner_frame_b
};
planner_messenger->Push(planner_frame_selector ? planner_frame_a : planner_frame_b);
planner_frame_selector = !planner_frame_selector;
planner_messenger_state = planner_messenger->SendData(planner_data_packet);
// Send data to collision stage only if it has finished
// processing, received the previous message and started processing it.
int collision_messenger_current_state = collision_messenger->GetState();
if ((collision_messenger_current_state != collision_messenger_state) &&
collision_frame_ready) {
const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b;
collision_messenger->Push(current_collision_frame);
collision_frame_selector = !collision_frame_selector;
const DataPacket<std::shared_ptr<LocalizationToCollisionFrame>> collision_data_packet = {
collision_messenger_state,
collision_frame_selector ? collision_frame_a : collision_frame_b
};
collision_messenger_state = collision_messenger->SendData(collision_data_packet);
collision_frame_selector = !collision_frame_selector;
collision_frame_ready = false;
}
// Send data to traffic light stage only if it has finished
// processing, received the previous message and started processing it.
const int traffic_light_messenger_current_state = traffic_light_messenger->GetState();
if (traffic_light_messenger_current_state != traffic_light_messenger_state) {
const DataPacket<std::shared_ptr<LocalizationToTrafficLightFrame>> traffic_light_data_packet = {
traffic_light_messenger_state,
traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b
};
traffic_light_messenger_state = traffic_light_messenger->SendData(traffic_light_data_packet);
traffic_light_frame_selector = !traffic_light_frame_selector;
}
traffic_light_messenger->Push(traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b);
traffic_light_frame_selector = !traffic_light_frame_selector;
}
void LocalizationStage::DrawBuffer(Buffer &buffer) {
@ -350,55 +303,66 @@ namespace LocalizationConstants {
const uint64_t waypoint_id = waypoint->GetId();
buffer.push_back(waypoint);
track_traffic.UpdatePassingVehicle(waypoint_id, actor_id);
const ActorIdSet current_actors = track_traffic.GetPassingVehicles(actor_id);
const ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(waypoint_id);
ActorIdSet actor_set_difference;
std::set_difference(
new_overlapping_actors.begin(), new_overlapping_actors.end(),
current_actors.begin(), current_actors.end(),
std::inserter(actor_set_difference, actor_set_difference.end())
);
for (auto new_actor_id: actor_set_difference) {
track_traffic.UpdateOverlappingVehicle(actor_id, new_actor_id);
track_traffic.UpdateOverlappingVehicle(new_actor_id, actor_id);
}
}
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()) {
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 ActorIdSet current_actors = track_traffic.GetPassingVehicles(removed_waypoint_id);
const ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(buffer.front()->GetId());
ActorIdSet actor_set_difference;
std::set_difference(
current_actors.begin(), current_actors.end(),
new_overlapping_actors.begin(), new_overlapping_actors.end(),
std::inserter(actor_set_difference, actor_set_difference.end())
);
for (auto& old_actor_id: actor_set_difference) {
track_traffic.RemoveOverlappingVehicle(actor_id, old_actor_id);
track_traffic.RemoveOverlappingVehicle(old_actor_id, actor_id);
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});
}
}
} else {
const ActorIdSet currently_tracked_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
for (auto& tracked_id: currently_tracked_vehicles) {
track_traffic.RemoveOverlappingVehicle(actor_id, tracked_id);
track_traffic.RemoveOverlappingVehicle(tracked_id, actor_id);
// 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()) {
cg::Location actor_location = actor_info.second->GetLocation();
SimpleWaypointPtr nearest_waypoint = nullptr;
const auto unregistered_type = actor_info.second->GetTypeId();
if (unregistered_type[0] == 'v') {
nearest_waypoint = local_map.GetWaypointInVicinity(actor_location);
} else if (unregistered_type[0] == 'w') {
nearest_waypoint = local_map.GetPedWaypoint(actor_location);
}
if (nearest_waypoint == nullptr) {
nearest_waypoint = local_map.GetWaypoint(actor_location);
}
track_traffic.UpdateUnregisteredGridPosition(actor_info.first, 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) {
@ -406,6 +370,7 @@ namespace LocalizationConstants {
const ActorId actor_id = vehicle->GetId();
const cg::Location vehicle_location = vehicle->GetLocation();
const float vehicle_velocity = vehicle->GetVelocity().Length();
const float speed_limit = boost::static_pointer_cast<cc::Vehicle>(vehicle)->GetSpeedLimit();
const Buffer& waypoint_buffer = buffer_list->at(vehicle_id_to_index.at(actor_id));
const SimpleWaypointPtr& current_waypoint = waypoint_buffer.front();
@ -414,7 +379,7 @@ namespace LocalizationConstants {
const auto left_waypoint = current_waypoint->GetLeftWaypoint();
const auto right_waypoint = current_waypoint->GetRightWaypoint();
if (!force) {
if (!force && current_waypoint != nullptr) {
const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
@ -424,46 +389,55 @@ namespace LocalizationConstants {
++i) {
const ActorId &other_vehicle_id = *i;
const Buffer& other_buffer = buffer_list->at(vehicle_id_to_index.at(other_vehicle_id));
const SimpleWaypointPtr& other_current_waypoint = other_buffer.front();
const cg::Location other_location = other_current_waypoint->GetLocation();
bool distant_lane_availability = false;
const auto other_neighbouring_lanes = {
other_current_waypoint->GetLeftWaypoint(),
other_current_waypoint->GetRightWaypoint()};
// This is totally ignoring unregistered actors during lane changes.
// Need to fix this. Only a temporary solution.
if (vehicle_id_to_index.find(other_vehicle_id) != vehicle_id_to_index.end()) {
const Buffer& other_buffer = buffer_list->at(vehicle_id_to_index.at(other_vehicle_id));
for (auto& candidate_lane_wp: other_neighbouring_lanes) {
if (candidate_lane_wp != nullptr &&
track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0 &&
vehicle_velocity < HIGHWAY_SPEED) {
distant_lane_availability = true;
}
}
if (!other_buffer.empty()) {
const SimpleWaypointPtr& other_current_waypoint = other_buffer.front();
const cg::Location other_location = other_current_waypoint->GetLocation();
const cg::Vector3D reference_heading = current_waypoint->GetForwardVector();
const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector();
bool distant_lane_availability = false;
const auto other_neighbouring_lanes = {
other_current_waypoint->GetLeftWaypoint(),
other_current_waypoint->GetRightWaypoint()};
if (other_current_waypoint != nullptr) {
for (auto& candidate_lane_wp: other_neighbouring_lanes) {
if (candidate_lane_wp != nullptr &&
track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0 &&
speed_limit < HIGHWAY_SPEED) {
distant_lane_availability = true;
}
}
if (other_vehicle_id != actor_id &&
!current_waypoint->CheckJunction() &&
!other_current_waypoint->CheckJunction() &&
cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) {
const cg::Vector3D reference_heading = current_waypoint->GetForwardVector();
const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector();
const float squared_vehicle_distance = cg::Math::DistanceSquared(other_location, vehicle_location);
const float deviation_dot = DeviationDotProduct(vehicle, other_location);
if (other_vehicle_id != actor_id &&
!current_waypoint->CheckJunction() &&
!other_current_waypoint->CheckJunction() &&
cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) {
if (deviation_dot > 0.0f) {
const float squared_vehicle_distance = cg::Math::DistanceSquared(other_location, vehicle_location);
const float deviation_dot = DeviationDotProduct(vehicle, other_location);
if (distant_lane_availability &&
squared_vehicle_distance > std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) {
if (deviation_dot > 0.0f) {
need_to_change_lane = true;
} else if (squared_vehicle_distance < std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) {
if (distant_lane_availability &&
squared_vehicle_distance > std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) {
need_to_change_lane = false;
abort_lane_change = true;
need_to_change_lane = true;
} else if (squared_vehicle_distance < std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) {
need_to_change_lane = false;
abort_lane_change = true;
}
}
}
}
}
}
}
@ -481,9 +455,9 @@ namespace LocalizationConstants {
std::vector<SimpleWaypointPtr> candidate_points;
if (force) {
if (direction) {
if (direction && left_waypoint != nullptr) {
candidate_points.push_back(left_waypoint);
} else {
} else if (!direction && right_waypoint != nullptr) {
candidate_points.push_back(right_waypoint);
}
} else {

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

@ -31,6 +31,7 @@
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PipelineStage.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
#include "carla/trafficmanager/PerformanceDiagnostics.h"
namespace carla {
namespace traffic_manager {
@ -49,14 +50,9 @@ namespace traffic_manager {
private:
/// Variables to remember messenger states.
int planner_messenger_state;
int collision_messenger_state;
int traffic_light_messenger_state;
/// Section keys to switch between the output data frames.
bool planner_frame_selector;
bool collision_frame_selector;
bool collision_frame_ready;
bool traffic_light_frame_selector;
/// Output data frames to be shared with the motion planner stage.
std::shared_ptr<LocalizationToPlannerFrame> planner_frame_a;
@ -86,11 +82,13 @@ 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;
/// Map connecting actor ids to indices of data arrays.
std::unordered_map<ActorId, uint> vehicle_id_to_index;
std::unordered_map<ActorId, uint64_t> vehicle_id_to_index;
/// Number of vehicles currently registered with the traffic manager.
uint64_t number_of_vehicles;
/// Used to only calculate the extended buffer once at junctions
@ -101,6 +99,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.
uint64_t unregistered_scan_duration = 0;
/// A structure used to keep track of actors spawned outside of traffic
/// manager.
std::unordered_map<ActorId, Actor> unregistered_actors;
/// Code snippet execution time profiler.
SnippetProfiler snippet_profiler;
/// A simple method used to draw waypoint buffer ahead of a vehicle.
void DrawBuffer(Buffer &buffer);
@ -110,6 +115,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:
@ -121,7 +128,8 @@ namespace traffic_manager {
AtomicActorSet &registered_actors,
InMemoryMap &local_map,
Parameters &parameters,
cc::DebugHelper &debug_helper);
cc::DebugHelper &debug_helper,
cc::World& world);
~LocalizationStage();

140
LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
View File

@ -9,6 +9,12 @@
namespace carla {
namespace traffic_manager {
namespace LocalizationConstants {
const static uint64_t BUFFER_STEP_THROUGH = 10u;
} // namespace LocalizationConstants
using namespace LocalizationConstants;
float DeviationCrossProduct(Actor actor, const cg::Location &target_location) {
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
@ -53,37 +59,137 @@ namespace traffic_manager {
TrackTraffic::TrackTraffic() {}
void TrackTraffic::UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id) {
void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint) {
if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) {
ActorIdSet& actor_set = overlapping_vehicles.at(actor_id);
if (actor_set.find(other_id) == actor_set.end()) {
actor_set.insert(other_id);
// Add actor entry if not present.
if (actor_to_grids.find(actor_id) == actor_to_grids.end()) {
actor_to_grids.insert({actor_id, {}});
}
std::unordered_set<GeoGridId>& current_grids = actor_to_grids.at(actor_id);
// Clear current actor from all grids containing current actor.
for (auto& grid_id: current_grids) {
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);
}
}
}
// Clear all grids current actor is tracking.
current_grids.clear();
// Step through buffer and update grid list for actor and actor list for grids.
if (waypoint != nullptr) {
GeoGridId ggid = waypoint->GetGeodesicGridId();
current_grids.insert(ggid);
// Add grid entry if not present.
if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
grid_to_actors.insert({ggid, {}});
}
ActorIdSet& actor_ids = grid_to_actors.at(ggid);
if (actor_ids.find(actor_id) == actor_ids.end()) {
actor_ids.insert(actor_id);
}
} else {
overlapping_vehicles.insert({actor_id, {other_id}});
}
}
void TrackTraffic::RemoveOverlappingVehicle(ActorId actor_id, ActorId other_id) {
if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) {
ActorIdSet& actor_set = overlapping_vehicles.at(actor_id);
if (actor_set.find(actor_id) != actor_set.end()) {
actor_set.erase(other_id);
void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer& buffer) {
// Add actor entry if not present.
if (actor_to_grids.find(actor_id) == actor_to_grids.end()) {
actor_to_grids.insert({actor_id, {}});
}
std::unordered_set<GeoGridId>& current_grids = actor_to_grids.at(actor_id);
// Clear current actor from all grids containing current actor.
for (auto& grid_id: current_grids) {
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);
}
}
}
// Clear all grids current actor is tracking.
current_grids.clear();
// Step through buffer and update grid list for actor and actor list for grids.
if (!buffer.empty()) {
uint64_t buffer_size = buffer.size();
uint64_t step_size = static_cast<uint64_t>(std::floor(buffer_size/BUFFER_STEP_THROUGH));
for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) {
GeoGridId ggid = buffer.at(std::min(i* step_size, buffer_size-1u))->GetGeodesicGridId();
current_grids.insert(ggid);
// Add grid entry if not present.
if (grid_to_actors.find(ggid) == grid_to_actors.end()) {
grid_to_actors.insert({ggid, {}});
}
ActorIdSet& actor_ids = grid_to_actors.at(ggid);
if (actor_ids.find(actor_id) == actor_ids.end()) {
actor_ids.insert(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);
}
}
std::unordered_set<GeoGridId> TrackTraffic::GetGridIds(ActorId actor_id) {
std::unordered_set<GeoGridId> grid_ids;
if (actor_to_grids.find(actor_id) != actor_to_grids.end()) {
grid_ids = actor_to_grids.at(actor_id);
}
return grid_ids;
}
std::unordered_map<GeoGridId, ActorIdSet> TrackTraffic::GetGridActors() {
return grid_to_actors;
}
void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
@ -97,7 +203,6 @@ namespace traffic_manager {
waypoint_overlap_tracker.insert({waypoint_id, {actor_id}});
}
}
void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
@ -122,6 +227,7 @@ namespace traffic_manager {
} else {
return ActorIdSet();
}
}
} // namespace traffic_manager

23
LibCarla/source/carla/trafficmanager/LocalizationUtils.h
View File

@ -7,8 +7,11 @@
#pragma once
#include "carla/client/Actor.h"
#include "carla/client/ActorList.h"
#include "carla/client/Vehicle.h"
#include "carla/client/World.h"
#include "carla/geom/Location.h"
#include "carla/road/RoadTypes.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
@ -23,6 +26,7 @@ namespace traffic_manager {
using ActorIdSet = std::unordered_set<ActorId>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using Buffer = std::deque<SimpleWaypointPtr>;
using GeoGridId = carla::road::JuncId;
class TrackTraffic{
@ -30,10 +34,12 @@ namespace traffic_manager {
/// Structure to keep track of overlapping waypoints between vehicles.
using WaypointOverlap = std::unordered_map<uint64_t, ActorIdSet>;
WaypointOverlap waypoint_overlap_tracker;
/// Structure to keep track of vehicles with overlapping paths.
std::unordered_map<ActorId, ActorIdSet> overlapping_vehicles;
/// Stored vehicle id set record.
ActorIdSet actor_id_set_record;
/// Geodesic grids occupied by actors's paths.
std::unordered_map<ActorId, std::unordered_set<GeoGridId>> actor_to_grids;
/// Actors currently passing through grids.
std::unordered_map<GeoGridId, ActorIdSet> grid_to_actors;
public:
TrackTraffic();
@ -43,11 +49,16 @@ namespace traffic_manager {
void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
ActorIdSet GetPassingVehicles(uint64_t waypoint_id);
/// Methods to update, remove and retrieve vehicles with overlapping vehicles.
void UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id);
void RemoveOverlappingVehicle(ActorId actor_id, ActorId other_id);
ActorIdSet GetOverlappingVehicles(ActorId actor_id);
void UpdateGridPosition(const ActorId actor_id, const Buffer& buffer);
void UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint);
ActorIdSet GetOverlappingVehicles(ActorId actor_id);
/// Method to delete actor data from tracking.
void DeleteActor(ActorId actor_id);
std::unordered_set<GeoGridId> GetGridIds(ActorId actor_id);
std::unordered_map<GeoGridId, ActorIdSet> GetGridActors();
};
/// Returns the cross product (z component value) between the vehicle's

82
LibCarla/source/carla/trafficmanager/Messenger.h
View File

@ -8,6 +8,7 @@
#include <atomic>
#include <chrono>
#include <deque>
#include <condition_variable>
#include <mutex>
@ -16,18 +17,6 @@ namespace traffic_manager {
using namespace std::chrono_literals;
template <typename Data>
struct DataPacket {
int id;
Data data;
};
/// This class is the template for messaging functionality between
/// different stage classes to send and receive data.
/// One object of this type can only facilitate receiving data from
/// a sender stage and passing the data onto a receiver stage.
/// The class maintains state internally and blocks send or receive
/// requests until data is available/successfully passed on.
template <typename Data>
class Messenger {
@ -35,11 +24,8 @@ namespace traffic_manager {
/// Flag used to wake up and join any waiting function calls on this object.
std::atomic<bool> stop_messenger;
/// State variable that will progress upon every successful communication
/// between the sender and receiver.
std::atomic<int> state_counter;
/// Member used to hold data sent by the sender.
Data data;
std::deque<Data> d_queue;
/// Mutex used to manage contention between the sender and receiver.
std::mutex data_modification_mutex;
/// Variable to conditionally block sender in case waiting for the receiver.
@ -50,58 +36,60 @@ namespace traffic_manager {
public:
Messenger() {
state_counter.store(0);
stop_messenger.store(false);
}
~Messenger() {}
/// This method receives data from a sender, stores in a member and
/// increments state.
int SendData(DataPacket<Data> packet) {
void Push(Data data) {
std::unique_lock<std::mutex> lock(data_modification_mutex);
while (state_counter.load() == packet.id && !stop_messenger.load()) {
send_condition.wait_for(lock, 1ms, [=] {return state_counter.load() != packet.id;});
while (!d_queue.empty() && !stop_messenger.load()) {
send_condition.wait_for(lock, 1ms, [=] {
return (d_queue.empty() && stop_messenger.load());
});
}
if(!stop_messenger.load()){
d_queue.push_front(data);
receive_condition.notify_one();
}
data = packet.data;
state_counter.store(state_counter.load() + 1);
int present_state = state_counter.load();
receive_condition.notify_one();
return present_state;
}
/// This method presents stored data to the receiver and increments state.
DataPacket<Data> ReceiveData(int old_state) {
Data Peek() {
std::unique_lock<std::mutex> lock(data_modification_mutex);
while (state_counter.load() == old_state && !stop_messenger.load()) {
receive_condition.wait_for(lock, 1ms, [=] {return state_counter.load() != old_state;});
while (d_queue.empty() && !stop_messenger.load()) {
receive_condition.wait_for(lock, 1ms, [=] {
return (!d_queue.empty() && stop_messenger.load());
});
}
state_counter.store(state_counter.load() + 1);
DataPacket<Data> packet = {state_counter.load(), data};
send_condition.notify_one();
return packet;
if(!stop_messenger.load()) {
Data data = d_queue.back();
return data;
}
return Data();
}
/// This method returns the current value of the state counter.
int GetState() {
return state_counter.load();
void Pop() {
std::unique_lock<std::mutex> lock(data_modification_mutex);
if (!(d_queue.empty() && stop_messenger.load())) {
d_queue.pop_back();
send_condition.notify_one();
}
}
/// This method unblocks any waiting calls on this object.
void Stop() {
stop_messenger.store(true);
}
/// This method restores regular functionality of the messenger.
/// This method needs to be called if the messenger has to be
/// used again after a call to the Stop() method.
void Start() {
stop_messenger.store(false);
}
void Stop() {
stop_messenger.store(true);
d_queue.clear();
send_condition.notify_one();
receive_condition.notify_one();
}
};
} // namespace traffic_manager

13
LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h
View File

@ -28,11 +28,14 @@ namespace traffic_manager {
/// Alias used for the list of buffers in the localization stage.
using BufferList = std::vector<Buffer>;
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
/// Data types.
/// Type of data sent by the localization stage to the motion planner stage.
struct LocalizationToPlannerData {
carla::SharedPtr<cc::Actor> actor;
Actor actor;
float deviation;
float distance;
bool approaching_true_junction;
@ -48,9 +51,11 @@ namespace traffic_manager {
/// Type of data sent by the localization stage to the collision stage.
struct LocalizationToCollisionData {
carla::SharedPtr<cc::Actor> actor;
Actor actor;
Buffer buffer;
std::unordered_set<carla::ActorId> overlapping_actors;
std::unordered_map<ActorId, Actor> overlapping_actors;
std::shared_ptr<SimpleWaypoint> closest_waypoint;
std::shared_ptr<SimpleWaypoint> junction_look_ahead_waypoint;
};
/// Type of data sent by the collision stage to the motion planner stage.
@ -60,7 +65,7 @@ namespace traffic_manager {
/// Type of data sent by the localization stage to the traffic light stage.
struct LocalizationToTrafficLightData {
carla::SharedPtr<cc::Actor> actor;
Actor actor;
std::shared_ptr<SimpleWaypoint> closest_waypoint;
std::shared_ptr<SimpleWaypoint> junction_look_ahead_waypoint;
};

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

@ -44,14 +44,6 @@ namespace PlannerConstants {
// Initializing the output frame selector.
frame_selector = true;
// Initializing messenger states.
localization_messenger_state = localization_messenger->GetState();
collision_messenger_state = collision_messenger->GetState();
traffic_light_messenger_state = traffic_light_messenger->GetState();
// Initializing this messenger to preemptively write since it precedes
// batch control stage.
control_messenger_state = control_messenger->GetState() - 1;
// Initializing number of vehicles to zero in the beginning.
number_of_vehicles = 0u;
}
@ -64,7 +56,12 @@ namespace PlannerConstants {
const auto current_control_frame = frame_selector ? control_frame_a : control_frame_b;
// Looping over all vehicles.
for (uint64_t i = 0u; i < number_of_vehicles; ++i) {
for (uint64_t i = 0u;
i < number_of_vehicles &&
localization_frame != nullptr &&
collision_frame != nullptr &&
traffic_light_frame != nullptr;
++i) {
const LocalizationToPlannerData &localization_data = localization_frame->at(i);
const Actor actor = localization_data.actor;
@ -87,12 +84,11 @@ namespace PlannerConstants {
traffic_manager::StateEntry previous_state;
previous_state = pid_state_map.at(actor_id);
// Increase speed if on highway.
const float speed_limit = vehicle->GetSpeedLimit() / 3.6f;
// Change PID parameters if on highway.
const float dynamic_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f;
if (speed_limit > HIGHWAY_SPEED) {
if (current_velocity > HIGHWAY_SPEED) {
longitudinal_parameters = highway_longitudinal_parameters;
lateral_parameters = highway_lateral_parameters;
} else {
@ -117,10 +113,8 @@ namespace PlannerConstants {
lateral_parameters);
// In case of collision or traffic light
if ((collision_frame != nullptr && traffic_light_frame != nullptr) &&
((collision_messenger_state != 0 && collision_frame->at(i).hazard) ||
(traffic_light_messenger_state != 0 &&
traffic_light_frame->at(i).traffic_light_hazard))) {
if ((collision_frame != nullptr && collision_frame->at(i).hazard) ||
(traffic_light_frame != nullptr && traffic_light_frame->at(i).traffic_light_hazard)) {
current_state.deviation_integral = 0.0f;
current_state.velocity_integral = 0.0f;
@ -144,31 +138,15 @@ namespace PlannerConstants {
void MotionPlannerStage::DataReceiver() {
const auto localization_packet = localization_messenger->ReceiveData(localization_messenger_state);
localization_frame = localization_packet.data;
localization_messenger_state = localization_packet.id;
// Block on receive call only if new data is available on the messenger.
const int collision_messenger_current_state = collision_messenger->GetState();
if (collision_messenger_current_state != collision_messenger_state) {
const auto collision_packet = collision_messenger->ReceiveData(collision_messenger_state);
collision_frame = collision_packet.data;
collision_messenger_state = collision_packet.id;
}
// Block on receive call only if new data is available on the messenger.
const int traffic_light_messenger_current_state = traffic_light_messenger->GetState();
if (traffic_light_messenger_current_state != traffic_light_messenger_state) {
const auto traffic_light_packet = traffic_light_messenger->ReceiveData(traffic_light_messenger_state);
traffic_light_frame = traffic_light_packet.data;
traffic_light_messenger_state = traffic_light_packet.id;
}
localization_frame = localization_messenger->Peek();
collision_frame = collision_messenger->Peek();
traffic_light_frame = traffic_light_messenger->Peek();
// Allocating new containers for the changed number of registered vehicles.
if (localization_frame != nullptr &&
number_of_vehicles != (*localization_frame.get()).size()) {
number_of_vehicles = static_cast<uint>((*localization_frame.get()).size());
number_of_vehicles = static_cast<uint64_t>((*localization_frame.get()).size());
// Allocate output frames.
control_frame_a = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
control_frame_b = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
@ -177,17 +155,21 @@ namespace PlannerConstants {
void MotionPlannerStage::DataSender() {
DataPacket<std::shared_ptr<PlannerToControlFrame>> data_packet = {
control_messenger_state,
frame_selector ? control_frame_a : control_frame_b
};
localization_messenger->Pop();
collision_messenger->Pop();
traffic_light_messenger->Pop();
control_messenger->Push(frame_selector ? control_frame_a : control_frame_b);
frame_selector = !frame_selector;
control_messenger_state = control_messenger->SendData(data_packet);
}
void MotionPlannerStage::DrawPIDValues(const boost::shared_ptr<cc::Vehicle> vehicle, const float throttle, const float brake) {
debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,2.0f), std::to_string(throttle), false, {0u, 255u, 0u}, 0.005f);
debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,4.0f), std::to_string(brake), false, {255u, 0u, 0u}, 0.005f);
void MotionPlannerStage::DrawPIDValues(const boost::shared_ptr<cc::Vehicle> vehicle,
const float throttle, const float brake) {
debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,2.0f),
std::to_string(throttle), false, {0u, 255u, 0u}, 0.005f);
debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0.0f,0.0f,4.0f),
std::to_string(brake), false, {255u, 0u, 0u}, 0.005f);
}
} // namespace traffic_manager

5
LibCarla/source/carla/trafficmanager/MotionPlannerStage.h
View File

@ -38,11 +38,6 @@ namespace traffic_manager {
/// Selection key to switch between the output frames.
bool frame_selector;
/// Variables to remember messenger states.
int localization_messenger_state;
int control_messenger_state;
int collision_messenger_state;
int traffic_light_messenger_state;
/// Pointers to data frames to be shared with the batch control stage
std::shared_ptr<PlannerToControlFrame> control_frame_a;
std::shared_ptr<PlannerToControlFrame> control_frame_b;

7
LibCarla/source/carla/trafficmanager/PIDController.cpp
View File

@ -41,7 +41,10 @@ namespace PIDControllerConstants {
// Calculating dt for 'D' and 'I' controller components.
const chr::duration<float> duration = current_state.time_instance - previous_state.time_instance;
const float dt = duration.count();
(void) duration; //const float dt = duration.count(); Remove.
// PID will be stable only over 20 FPS.
const float dt = 1/20.0f;
// Calculating integrals.
current_state.deviation_integral = angular_deviation * dt + previous_state.deviation_integral;
@ -86,7 +89,7 @@ namespace PIDControllerConstants {
lateral_parameters[2] * (present_state.deviation -
previous_state.deviation) / dt;
steer = std::max(-1.0f, std::min(steer, 1.0f));
steer = std::max(-0.8f, std::min(steer, 0.8f));
return ActuationSignal{throttle, brake, steer};
}

30
LibCarla/source/carla/trafficmanager/Parameters.cpp
View File

@ -16,11 +16,13 @@ Parameters::Parameters() {}
Parameters::~Parameters() {}
void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
percentage_difference_from_speed_limit.AddEntry({actor->GetId(), percentage});
float new_percentage = std::min(100.0f,percentage);
percentage_difference_from_speed_limit.AddEntry({actor->GetId(), new_percentage});
}
void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) {
global_percentage_difference_from_limit = percentage;
float new_percentage = std::min(100.0f,percentage);
global_percentage_difference_from_limit = new_percentage;
}
void Parameters::SetCollisionDetection(
@ -69,10 +71,10 @@ void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
}
void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
if (distance > 0.0f) {
const auto entry = std::make_pair(actor->GetId(), distance);
distance_to_leading_vehicle.AddEntry(entry);
}
float new_distance = std::max(0.0f,distance);
const auto entry = std::make_pair(actor->GetId(), new_distance);
distance_to_leading_vehicle.AddEntry(entry);
}
float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) {
@ -142,17 +144,17 @@ float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) {
}
void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) {
if (perc > 0.0f) {
const auto entry = std::make_pair(actor->GetId(), perc);
perc_run_traffic_light.AddEntry(entry);
}
float new_perc = cg::Math::Clamp(perc,0.0f,100.0f);
const auto entry = std::make_pair(actor->GetId(), new_perc);
perc_run_traffic_light.AddEntry(entry);
}
void Parameters::SetPercentageIgnoreActors(const ActorPtr &actor, const float perc) {
if (perc > 0.0f) {
const auto entry = std::make_pair(actor->GetId(), perc);
perc_ignore_actors.AddEntry(entry);
}
float new_perc = cg::Math::Clamp(perc,0.0f,100.0f);
const auto entry = std::make_pair(actor->GetId(), new_perc);
perc_ignore_actors.AddEntry(entry);
}
float Parameters::GetPercentageRunningLight(const ActorPtr &actor) {

1
LibCarla/source/carla/trafficmanager/Parameters.h
View File

@ -18,6 +18,7 @@ namespace carla {
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;

25
LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp
View File

@ -18,22 +18,29 @@ namespace traffic_manager {
inter_update_duration = chr::duration<float>(0);
}
void PerformanceDiagnostics::RegisterUpdate() {
void PerformanceDiagnostics::RegisterUpdate(bool begin_or_end) {
const auto current_time = chr::system_clock::now();
const chr::duration<float> throughput_count_duration = current_time - throughput_clock;
if (throughput_count_duration.count() > 1.0f) {
if (begin_or_end) {
const chr::duration<float> throughput_count_duration = current_time - throughput_clock;
if (throughput_count_duration.count() > 1.0f) {
throughput_clock = current_time;
throughput_counter = 0u;
//std::cout << "Stage: " + stage_name + ", throughput: " << throughput_counter
// << ", avg. cycle duration: " << 1000* inter_update_duration.count()
// << " ms" << std::endl;
throughput_clock = current_time;
throughput_counter = 0u;
} else {
++throughput_counter;
}
inter_update_clock = current_time;
} else {
++throughput_counter;
const chr::duration<float> last_update_duration = current_time - inter_update_clock;
inter_update_duration = (inter_update_duration + last_update_duration) / 2.0f;
inter_update_clock = current_time;
}
}

55
LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h
View File

@ -8,6 +8,7 @@
#include <chrono>
#include <string>
#include <unordered_map>
#include "carla/Logging.h"
@ -16,24 +17,72 @@ namespace traffic_manager {
namespace chr = std::chrono;
using TimePoint = chr::time_point<chr::system_clock, chr::nanoseconds>;
class PerformanceDiagnostics {
private:
/// Stage name.
std::string stage_name;
/// Throughput clock.
chr::time_point<chr::system_clock, chr::nanoseconds> throughput_clock;
TimePoint throughput_clock;
/// Throughput counter.
uint64_t throughput_counter;
/// Inter-update clock.
chr::time_point<chr::system_clock, chr::nanoseconds> inter_update_clock;
TimePoint inter_update_clock;
/// Inter-update duration.
chr::duration<float> inter_update_duration;
public:
PerformanceDiagnostics(std::string name);
void RegisterUpdate();
void RegisterUpdate(bool begin_or_end);
};
class SnippetProfiler {
private:
std::unordered_map<std::string, TimePoint> print_clocks;
std::unordered_map<std::string, TimePoint> snippet_clocks;
std::unordered_map<std::string, chr::duration<float>> snippet_durations;
public:
SnippetProfiler(){};
void MeasureExecutionTime(std::string snippet_name, bool begin_or_end) {
if (print_clocks.find(snippet_name) == print_clocks.end()) {
print_clocks.insert({snippet_name, chr::system_clock::now()});
}
if (snippet_clocks.find(snippet_name) == snippet_clocks.end()) {
snippet_clocks.insert({snippet_name, TimePoint()});
}
if (snippet_durations.find(snippet_name) == snippet_durations.end()) {
snippet_durations.insert({snippet_name, chr::duration<float>()});
}
TimePoint current_time = chr::system_clock::now();
TimePoint& print_clock = print_clocks.at(snippet_name);
TimePoint& snippet_clock = snippet_clocks.at(snippet_name);
chr::duration<float>& snippet_duration = snippet_durations.at(snippet_name);
if (begin_or_end) {
snippet_clock = current_time;
} else {
chr::duration<float> measured_duration = current_time - snippet_clock;
snippet_duration = (measured_duration + snippet_duration) / 2.0f;
}
chr::duration<float> print_duration = current_time - print_clock;
if (print_duration.count() > 1.0f) {
std::cout << "Snippet name : " << snippet_name << ", "
<< "avg. duration : " << 1000 * snippet_duration.count()
<< " ms" << std::endl;
print_clock = current_time;
}
}
};
} // namespace traffic_manager

93
LibCarla/source/carla/trafficmanager/PipelineStage.cpp
View File

@ -10,99 +10,42 @@ namespace carla {
namespace traffic_manager {
PipelineStage::PipelineStage(std::string stage_name)
: performance_diagnostics(PerformanceDiagnostics(stage_name)) {
run_stage.store(true);
run_receiver.store(true);
run_action.store(false);
run_sender.store(false);
: stage_name(stage_name),
performance_diagnostics(PerformanceDiagnostics(stage_name)) {
run_stage.store(false);
}
PipelineStage::~PipelineStage() {}
PipelineStage::~PipelineStage() {
worker_thread->join();
worker_thread.release();
}
void PipelineStage::Start() {
data_receiver = std::make_unique<std::thread>(&PipelineStage::ReceiverThreadManager, this);
action_thread = std::make_unique<std::thread>(&PipelineStage::ActionThreadManager, this);
data_sender = std::make_unique<std::thread>(&PipelineStage::SenderThreadManager, this);
run_stage.store(true);
worker_thread = std::make_unique<std::thread>(&PipelineStage::Update, this);
}
void PipelineStage::Stop() {
run_stage.store(false);
data_receiver->join();
action_thread->join();
data_sender->join();
}
void PipelineStage::ReceiverThreadManager() {
while (run_stage.load()) {
std::unique_lock<std::mutex> lock(thread_coordination_mutex);
// Wait for notification from sender thread and
// break waiting if the stage is stopped.
while (!run_receiver.load() && run_stage.load()) {
wake_receiver_notifier.wait_for(lock, 1ms, [=] {return run_receiver.load();});
}
lock.unlock();
run_receiver.store(false);
void PipelineStage::Update() {
while (run_stage.load()){
// Receive data.
DataReceiver();
// Receive data.
if (run_stage.load()) {
DataReceiver();
}
// Notify action thread.
run_action.store(true);
wake_action_notifier.notify_one();
}
}
void PipelineStage::ActionThreadManager() {
while (run_stage.load()) {
performance_diagnostics.RegisterUpdate();
std::unique_lock<std::mutex> lock(thread_coordination_mutex);
// Wait for notification from receiver thread.
while (!run_action.load() && run_stage.load()) {
wake_action_notifier.wait_for(lock, 1ms, [=] {return run_action.load();});
}
lock.unlock();
run_action.store(false);
// Run action.
if (run_stage.load()) {
if(run_stage.load()){
performance_diagnostics.RegisterUpdate(true);
Action();
performance_diagnostics.RegisterUpdate(false);
}
// Notify sender.
run_sender.store(true);
wake_sender_notifier.notify_one();
}
}
void PipelineStage::SenderThreadManager() {
while (run_stage.load()) {
std::unique_lock<std::mutex> lock(thread_coordination_mutex);
// Wait for notification from action thread.
while (!run_sender.load() && run_stage.load()) {
wake_sender_notifier.wait_for(lock, 1ms, [=] {return run_sender.load();});
}
lock.unlock();
run_sender.store(false);
// Send data.
if (run_stage.load()) {
// Receive data.
if(run_stage.load()) {
DataSender();
}
// Notify receiver.
run_receiver.store(true);
wake_receiver_notifier.notify_one();
}
}

33
LibCarla/source/carla/trafficmanager/PipelineStage.h
View File

@ -17,6 +17,7 @@
#include <thread>
#include <vector>
#include "carla/Logging.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/Messenger.h"
@ -34,37 +35,17 @@ namespace traffic_manager {
private:
/// Pointer to receiver thread instance.
std::unique_ptr<std::thread> data_receiver;
/// Pointer to sender thread instance.
std::unique_ptr<std::thread> data_sender;
/// Pointer to worker thread instance.
std::unique_ptr<std::thread> action_thread;
/// Flag to allow/block receiver.
std::atomic<bool> run_receiver;
/// Flag to allow/block sender.
std::atomic<bool> run_sender;
/// Flag to allow/block workers.
std::atomic<bool> run_action;
std::unique_ptr<std::thread> worker_thread;
/// Flag to start/stop stage.
std::atomic<bool> run_stage;
/// Mutex used to co-ordinate between receiver, workers, and sender.
std::mutex thread_coordination_mutex;
/// Variables to conditionally block receiver, workers, and sender.
std::condition_variable wake_receiver_notifier;
std::condition_variable wake_action_notifier;
std::condition_variable wake_sender_notifier;
/// Stage name string.
std::string stage_name;
private:
/// Object to track stage performance.
PerformanceDiagnostics performance_diagnostics;
/// Method to manage receiver thread.
void ReceiverThreadManager();
/// Method to manage worker threads.
void ActionThreadManager();
/// Method to manage sender thread.
void SenderThreadManager();
void Update();
protected:

31
LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp
View File

@ -22,6 +22,10 @@ namespace traffic_manager {
return next_waypoints;
}
std::vector<SimpleWaypointPtr> SimpleWaypoint::GetPreviousWaypoint() const {
return previous_waypoints;
}
WaypointPtr SimpleWaypoint::GetWaypoint() const {
return waypoint;
}
@ -50,7 +54,14 @@ namespace traffic_manager {
for (auto &simple_waypoint: waypoints) {
next_waypoints.push_back(simple_waypoint);
}
return static_cast<uint>(waypoints.size());
return static_cast<uint64_t>(waypoints.size());
}
uint64_t SimpleWaypoint::SetPreviousWaypoint(const std::vector<SimpleWaypointPtr> &waypoints) {
for (auto &simple_waypoint: waypoints) {
previous_waypoints.push_back(simple_waypoint);
}
return static_cast<uint64_t>(waypoints.size());
}
void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr _waypoint) {
@ -99,5 +110,23 @@ namespace traffic_manager {
return (next_waypoints.size() > 1);
}
void SimpleWaypoint::SetGeodesicGridId(GeoGridId _geodesic_grid_id) {
geodesic_grid_id = _geodesic_grid_id;
}
GeoGridId SimpleWaypoint::GetGeodesicGridId() {
GeoGridId grid_id;
if (waypoint->IsJunction()) {
grid_id = waypoint->GetJunctionId();
} else {
grid_id = geodesic_grid_id;
}
return grid_id;
}
GeoGridId SimpleWaypoint::GetJunctionId() const {
return waypoint->GetJunctionId();
}
} // namespace traffic_manager
} // namespace carla

19
LibCarla/source/carla/trafficmanager/SimpleWaypoint.h
View File

@ -14,6 +14,7 @@
#include "carla/geom/Math.h"
#include "carla/geom/Vector3D.h"
#include "carla/Memory.h"
#include "carla/road/RoadTypes.h"
namespace carla {
namespace traffic_manager {
@ -21,6 +22,7 @@ namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
using GeoGridId = carla::road::JuncId;
/// This is a simple wrapper class on Carla's waypoint object.
/// The class is used to represent discrete samples of the world map.
@ -34,10 +36,14 @@ namespace traffic_manager {
WaypointPtr waypoint;
/// List of pointers to next connecting waypoints.
std::vector<SimpleWaypointPtr> next_waypoints;
/// List of pointers to previous connecting waypoints.
std::vector<SimpleWaypointPtr> previous_waypoints;
/// Pointer to left lane change waypoint.
SimpleWaypointPtr next_left_waypoint;
/// Pointer to right lane change waypoint.
SimpleWaypointPtr next_right_waypoint;
/// Integer placing the waypoint into a geodesic grid.
GeoGridId geodesic_grid_id = 0;
public:
@ -53,6 +59,9 @@ namespace traffic_manager {
/// Returns the list of next waypoints.
std::vector<SimpleWaypointPtr> GetNextWaypoint() const;
/// Returns the list of previous waypoints.
std::vector<SimpleWaypointPtr> GetPreviousWaypoint() const;
/// Returns the vector along the waypoint's direction.
cg::Vector3D GetForwardVector() const;
@ -62,6 +71,9 @@ namespace traffic_manager {
/// This method is used to set the next waypoints.
uint64_t SetNextWaypoint(const std::vector<SimpleWaypointPtr> &next_waypoints);
/// This method is used to set the previous waypoints.
uint64_t SetPreviousWaypoint(const std::vector<SimpleWaypointPtr> &next_waypoints);
/// This method is used to set the closest left waypoint for a lane change.
void SetLeftWaypoint(SimpleWaypointPtr waypoint);
@ -74,6 +86,13 @@ namespace traffic_manager {
/// This method is used to get the closest right waypoint for a lane change.
SimpleWaypointPtr GetRightWaypoint();
/// Accessor methods for geodesic grid id.
void SetGeodesicGridId(GeoGridId _geodesic_grid_id);
GeoGridId GetGeodesicGridId();
/// Metod to retreive junction id of the waypoint.
GeoGridId GetJunctionId() const;
/// Calculates the distance from the object's waypoint to the passed
/// location.
float Distance(const cg::Location &location) const;

28
LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp
View File

@ -28,13 +28,6 @@ namespace traffic_manager {
// Initializing output frame selector.
frame_selector = true;
// Initializing messenger state.
localization_messenger_state = localization_messenger->GetState();
// Initializing this messenger state to preemptively write
// since this stage precedes motion planner stage.
planner_messenger_state = planner_messenger->GetState() - 1;
// Initializing number of vehicles to zero in the beginning.
number_of_vehicles = 0u;
}
@ -46,7 +39,7 @@ namespace traffic_manager {
// Selecting the output frame based on the selection key.
const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;
// Looping over registered actors.
for (uint64_t i = 0u; i < number_of_vehicles; ++i) {
for (uint64_t i = 0u; i < number_of_vehicles && localization_frame != nullptr; ++i) {
bool traffic_light_hazard = false;
const LocalizationToTrafficLightData &data = localization_frame->at(i);
@ -68,11 +61,8 @@ namespace traffic_manager {
traffic_light_state = TLS::Green;
// We determine to stop if the current position of the vehicle is not a
// junction,
// a point on the path beyond a threshold (velocity-dependent) distance
// is inside the junction and there is a red or yellow light.
// junction and there is a red or yellow light.
if (!closest_waypoint->CheckJunction() &&
look_ahead_point->CheckJunction() &&
ego_vehicle->IsAtTrafficLight() &&
traffic_light_state != TLS::Green) {
@ -151,15 +141,13 @@ namespace traffic_manager {
}
void TrafficLightStage::DataReceiver() {
const auto packet = localization_messenger->ReceiveData(localization_messenger_state);
localization_frame = packet.data;
localization_messenger_state = packet.id;
localization_frame = localization_messenger->Peek();
// Allocating new containers for the changed number of registered vehicles.
if (localization_frame != nullptr &&
number_of_vehicles != (*localization_frame.get()).size()) {
number_of_vehicles = static_cast<uint>((*localization_frame.get()).size());
number_of_vehicles = static_cast<uint64_t>((*localization_frame.get()).size());
// Allocating output frames.
planner_frame_a = std::make_shared<TrafficLightToPlannerFrame>(number_of_vehicles);
planner_frame_b = std::make_shared<TrafficLightToPlannerFrame>(number_of_vehicles);
@ -168,12 +156,10 @@ namespace traffic_manager {
void TrafficLightStage::DataSender() {
const DataPacket<std::shared_ptr<TrafficLightToPlannerFrame>> packet{
planner_messenger_state,
frame_selector ? planner_frame_a : planner_frame_b
};
localization_messenger->Pop();
planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b);
frame_selector = !frame_selector;
planner_messenger_state = planner_messenger->SendData(packet);
}
void TrafficLightStage::DrawLight(TLS traffic_light_state, const Actor &ego_actor) const {

3
LibCarla/source/carla/trafficmanager/TrafficLightStage.h
View File

@ -44,9 +44,6 @@ namespace traffic_manager {
private:
/// Variables to remember messenger states.
int localization_messenger_state;
int planner_messenger_state;
/// Selection key to switch between output frames.
bool frame_selector;
/// Pointer data frame received from localization stage.

27
LibCarla/source/carla/trafficmanager/TrafficManager.cpp
View File

@ -28,11 +28,9 @@ namespace traffic_manager {
using WorldMap = carla::SharedPtr<cc::Map>;
const WorldMap world_map = world.GetMap();
const auto dao = CarlaDataAccessLayer(world_map);
using Topology = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
const Topology topology = dao.GetTopology();
local_map = std::make_shared<traffic_manager::InMemoryMap>(topology);
local_map->SetUp(0.1f);
const RawNodeList raw_dense_topology = world_map->GenerateWaypoints(0.1f);
local_map = std::make_shared<traffic_manager::InMemoryMap>(raw_dense_topology);
local_map->SetUp();
parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit);
@ -48,12 +46,13 @@ namespace traffic_manager {
localization_planner_messenger, localization_collision_messenger,
localization_traffic_light_messenger,
registered_actors, *local_map.get(),
parameters, debug_helper);
parameters, debug_helper,
world);
collision_stage = std::make_unique<CollisionStage>(
"Collision stage",
localization_collision_messenger, collision_planner_messenger,
world, parameters, debug_helper);
parameters, debug_helper);
traffic_light_stage = std::make_unique<TrafficLightStage>(
"Traffic light stage",
@ -81,6 +80,7 @@ namespace traffic_manager {
}
TrafficManager::~TrafficManager() {
Stop();
}
@ -90,10 +90,10 @@ namespace traffic_manager {
if (singleton_pointer == nullptr) {
const std::vector<float> longitudinal_param = {2.0f, 0.15f, 0.01f};
const std::vector<float> longitudinal_highway_param = {4.0f, 0.15f, 0.01f};
const std::vector<float> lateral_param = {10.0f, 0.0f, 0.1f};
const std::vector<float> lateral_highway_param = {6.0f, 0.0f, 0.3f};
const std::vector<float> longitudinal_param = {2.0f, 0.05f, 0.07f};
const std::vector<float> longitudinal_highway_param = {4.0f, 0.02f, 0.03f};
const std::vector<float> lateral_param = {10.0f, 0.02f, 1.0f};
const std::vector<float> lateral_highway_param = {9.0f, 0.02f, 1.0f};
const float perc_difference_from_limit = 30.0f;
TrafficManager* tm_ptr = new TrafficManager(
@ -127,10 +127,6 @@ namespace traffic_manager {
registered_actors.Remove(actor_list);
}
void TrafficManager::DestroyVehicle(const ActorPtr &actor) {
registered_actors.Destroy(actor);
}
void TrafficManager::Start() {
localization_collision_messenger->Start();
@ -161,6 +157,7 @@ namespace traffic_manager {
traffic_light_stage->Stop();
planner_stage->Stop();
control_stage->Stop();
}
void TrafficManager::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {

8
LibCarla/source/carla/trafficmanager/TrafficManager.h
View File

@ -23,7 +23,6 @@
#include "carla/trafficmanager/AtomicActorSet.h"
#include "carla/trafficmanager/AtomicMap.h"
#include "carla/trafficmanager/BatchControlStage.h"
#include "carla/trafficmanager/CarlaDataAccessLayer.h"
#include "carla/trafficmanager/CollisionStage.h"
#include "carla/trafficmanager/InMemoryMap.h"
#include "carla/trafficmanager/LocalizationStage.h"
@ -110,8 +109,8 @@ namespace traffic_manager {
/// This method unregisters a vehicle from traffic manager.
void UnregisterVehicles(const std::vector<ActorPtr> &actor_list);
/// This method kills a vehicle.
void DestroyVehicle(const ActorPtr &actor);
/// This method kills a vehicle. (Not working right now)
/// void DestroyVehicle(const ActorPtr &actor);
/// Set target velocity specific to a vehicle.
void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage);
@ -148,6 +147,9 @@ namespace traffic_manager {
/// Method to reset all traffic lights.
void ResetAllTrafficLights();
/// Return the world object
const cc::World &GetWorld() { return world; };
/// Destructor.
~TrafficManager();

97
LibCarla/source/carla/trafficmanager/VicinityGrid.cpp
View File

@ -1,97 +0,0 @@
// Copyright (c) 2019 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 "VicinityGrid.h"
namespace carla {
namespace traffic_manager {
static const float GRID_SIZE = 10.0f;
VicinityGrid::VicinityGrid() {}
VicinityGrid::~VicinityGrid() {}
std::string VicinityGrid::MakeKey(std::pair<int, int> grid_ids) {
return std::to_string(grid_ids.first) + std::to_string(grid_ids.second);
}
std::pair<int, int> VicinityGrid::UpdateGrid(Actor actor) {
const ActorId actor_id = actor->GetId();
const cg::Location location = actor->GetLocation();
const int first = static_cast<int>(std::floor(location.x / GRID_SIZE));
const int second = static_cast<int>(std::floor(location.y / GRID_SIZE));
const std::string new_grid_id = MakeKey({first, second});
// If the actor exists in the grid.
if (actor_to_grid_id.find(actor_id) != actor_to_grid_id.end()) {
const std::string old_grid_id = actor_to_grid_id.at(actor_id);
// If the actor has moved into a new road/section/lane.
if (old_grid_id != new_grid_id) {
std::unique_lock<std::shared_timed_mutex> lock(modification_mutex);
// Update the actor's grid id.
actor_to_grid_id.at(actor_id) = new_grid_id;
// Remove the actor from the old grid position and update the new position.
grid_to_actor_id.at(old_grid_id).erase(actor_id);
if (grid_to_actor_id.find(new_grid_id) != grid_to_actor_id.end()) {
grid_to_actor_id.at(new_grid_id).insert(actor_id);
} else {
grid_to_actor_id.insert({new_grid_id, {actor_id}});
}
}
}
// If the actor is new, then add entries to map.
else {
std::unique_lock<std::shared_timed_mutex> lock(modification_mutex);
actor_to_grid_id.insert({actor_id, new_grid_id});
if (grid_to_actor_id.find(new_grid_id) != grid_to_actor_id.end()) {
grid_to_actor_id.at(new_grid_id).insert(actor_id);
} else {
grid_to_actor_id.insert({new_grid_id, {actor_id}});
}
}
// Return updated grid position.
return {first, second};
}
std::unordered_set<carla::ActorId> VicinityGrid::GetActors(Actor actor) {
const std::pair<int, int> grid_ids = UpdateGrid(actor);
std::shared_lock<std::shared_timed_mutex> lock(modification_mutex);
std::unordered_set<ActorId> actors;
// Search all surrounding grids and find any vehicles in them.
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
const std::string grid_key = MakeKey({grid_ids.first + i, grid_ids.second + j});
if (grid_to_actor_id.find(grid_key) != grid_to_actor_id.end()) {
const auto &grid_actor_set = grid_to_actor_id.at(grid_key);
actors.insert(grid_actor_set.begin(), grid_actor_set.end());
}
}
}
return actors;
}
void VicinityGrid::EraseActor(ActorId actor_id) {
const std::string grid_key = actor_to_grid_id.at(actor_id);
actor_to_grid_id.erase(actor_id);
grid_to_actor_id.at(grid_key).erase(actor_id);
}
} // namespace traffic_manager
} // namespace carla

67
LibCarla/source/carla/trafficmanager/VicinityGrid.h
View File

@ -1,67 +0,0 @@
// Copyright (c) 2019 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>.
#pragma once
#include <cmath>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include "carla/client/Actor.h"
#include "carla/geom/Location.h"
#include "carla/rpc/Actor.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
using ActorId = carla::ActorId;
using Actor = carla::SharedPtr<cc::Actor>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using Buffer = std::deque<SimpleWaypointPtr>;
/// This class maintains vehicle positions in grid segments.
/// This is used in the collision stage to filter vehicles.
class VicinityGrid {
private:
/// Mutex to manage contention between modifiers and readers.
std::shared_timed_mutex modification_mutex;
/// Map connecting grid id to set of actor ids.
std::unordered_map<std::string, std::unordered_set<ActorId>> grid_to_actor_id;
/// Map connecting actor id to grid key.
std::unordered_map<ActorId, std::string> actor_to_grid_id;
/// Key generator for a given grid.
std::string MakeKey(std::pair<int, int> grid_ids);
public:
VicinityGrid();
~VicinityGrid();
/// Returns a set of actors in the vicinity of a given actor.
std::unordered_set<ActorId> GetActors(Actor actor);
/// Updates the grid position of the given actor and returns new grid id.
std::pair<int, int> UpdateGrid(Actor actor);
/// Removes actor.
void EraseActor(ActorId actor_id);
};
} // namespace traffic_manager
} // namespace carla

71
PythonAPI/carla/source/libcarla/Client.cpp
View File

@ -4,9 +4,12 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include <carla/PythonUtil.h>
#include <carla/client/Client.h>
#include <carla/client/World.h>
#include "carla/PythonUtil.h"
#include "carla/client/Client.h"
#include "carla/client/World.h"
#include "carla/Logging.h"
#include "carla/rpc/ActorId.h"
#include "carla/trafficmanager/TrafficManager.h"
#include <boost/python/stl_iterator.hpp>
@ -43,9 +46,69 @@ static auto ApplyBatchCommandsSync(
boost::python::stl_input_iterator<CommandType>(commands),
boost::python::stl_input_iterator<CommandType>()};
boost::python::list result;
for (auto &response : self.ApplyBatchSync(std::move(cmds), do_tick)) {
auto responses = self.ApplyBatchSync(cmds, do_tick);
for (auto &response : responses) {
result.append(std::move(response));
}
// check for autopilot command
carla::traffic_manager::TrafficManager *tm = nullptr;
std::vector<carla::traffic_manager::ActorPtr> vehicles_to_enable;
std::vector<carla::traffic_manager::ActorPtr> vehicles_to_disable;
for (size_t i=0; i<cmds.size(); ++i) {
if (!responses[i].HasError()) {
bool isAutopilot = false;
bool autopilotValue = false;
// check SpawnActor command
if (cmds[i].command.type() == typeid(carla::rpc::Command::SpawnActor)) {
// check inside 'do_after'
auto &spawn = boost::get<carla::rpc::Command::SpawnActor>(cmds[i].command);
for (auto &cmd : spawn.do_after) {
if (cmd.command.type() == typeid(carla::rpc::Command::SetAutopilot)) {
autopilotValue = boost::get<carla::rpc::Command::SetAutopilot>(cmd.command).enabled;
isAutopilot = true;
}
}
}
// check SetAutopilot command
if (cmds[i].command.type() == typeid(carla::rpc::Command::SetAutopilot)) {
autopilotValue = boost::get<carla::rpc::Command::SetAutopilot>(cmds[i].command).enabled;
isAutopilot = true;
}
// check if found any SetAutopilot command
if (isAutopilot) {
// get the id
carla::rpc::ActorId id = static_cast<carla::rpc::ActorId>(responses[i].Get());
// get traffic manager instance
if (!tm) {
tm = &carla::traffic_manager::TrafficManager::GetInstance(
carla::traffic_manager::TrafficManager::GetUniqueLocalClient());
}
// get all actors
carla::SharedPtr<carla::client::Actor> actor;
if (tm) {
actor = tm->GetWorld().GetActor(id);
}
// check to enable or disable
if (actor) {
if (autopilotValue) {
vehicles_to_enable.push_back(actor);
} else {
vehicles_to_disable.push_back(actor);
}
}
}
}
}
// check if any autopilot command was sent
if ((vehicles_to_enable.size() || vehicles_to_disable.size()) && tm) {
tm->RegisterVehicles(vehicles_to_enable);
tm->UnregisterVehicles(vehicles_to_disable);
}
return result;
}

23
PythonAPI/carla/source/libcarla/TrafficManager.cpp
View File

@ -23,18 +23,17 @@ void export_trafficmanager() {
class_<Parameters>("TM_Parameters").def(vector_indexing_suite<Parameters>());
class_<carla::traffic_manager::TrafficManager, boost::noncopyable>("TrafficManager", no_init)
.def("register_vehicles", &carla::traffic_manager::TrafficManager::RegisterVehicles)
.def("unregister_vehicles", &carla::traffic_manager::TrafficManager::UnregisterVehicles)
.def("set_vehicle_max_speed_difference", &carla::traffic_manager::TrafficManager::SetPercentageSpeedDifference)
.def("set_global_max_speed_difference", &carla::traffic_manager::TrafficManager::SetGlobalPercentageSpeedDifference)
.def("set_collision_detection", &carla::traffic_manager::TrafficManager::SetCollisionDetection)
.def("force_lane_change", &carla::traffic_manager::TrafficManager::SetForceLaneChange)
.def("set_auto_lane_change", &carla::traffic_manager::TrafficManager::SetAutoLaneChange)
.def("set_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetDistanceToLeadingVehicle)
.def("reset_traffic_lights", &carla::traffic_manager::TrafficManager::ResetAllTrafficLights)
.def("destroy_vehicle", &carla::traffic_manager::TrafficManager::DestroyVehicle)
.def("ignore_actors_percentage", &carla::traffic_manager::TrafficManager::SetPercentageIgnoreActors)
.def("ignore_lights_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningLight);
.def("register_vehicles", &carla::traffic_manager::TrafficManager::RegisterVehicles)
.def("unregister_vehicles", &carla::traffic_manager::TrafficManager::UnregisterVehicles)
.def("set_vehicle_max_speed_difference", &carla::traffic_manager::TrafficManager::SetPercentageSpeedDifference)
.def("set_global_max_speed_difference", &carla::traffic_manager::TrafficManager::SetGlobalPercentageSpeedDifference)
.def("set_collision_detection", &carla::traffic_manager::TrafficManager::SetCollisionDetection)
.def("force_lane_change", &carla::traffic_manager::TrafficManager::SetForceLaneChange)
.def("set_auto_lane_change", &carla::traffic_manager::TrafficManager::SetAutoLaneChange)
.def("set_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetDistanceToLeadingVehicle)
.def("reset_traffic_lights", &carla::traffic_manager::TrafficManager::ResetAllTrafficLights)
.def("ignore_actors_percentage", &carla::traffic_manager::TrafficManager::SetPercentageIgnoreActors)
.def("ignore_lights_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningLight);
def("GetTrafficManager", &carla::traffic_manager::TrafficManager::GetInstance, return_value_policy<reference_existing_object>());

4
PythonAPI/examples/tm_spawn_npc.py
View File

@ -76,7 +76,6 @@ def main():
client.set_timeout(2.0)
try:
traffic_manager = None
world = client.get_world()
blueprints = world.get_blueprint_library().filter(args.filterv)
blueprintsWalkers = world.get_blueprint_library().filter(args.filterw)
@ -193,7 +192,6 @@ def main():
print('Spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list)))
traffic_manager = carla.GetTrafficManager(client)
time.sleep(1)
for v in vehicles_list:
@ -203,8 +201,6 @@ def main():
time.sleep(1)
finally:
if traffic_manager:
del traffic_manager
print('Destroying %d vehicles.\n' % len(vehicles_list))
client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list])

13
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.cpp
View File

@ -161,6 +161,19 @@ void ATrafficLightBase::NotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle)
}
}
void ATrafficLightBase::UnNotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle)
{
if (IsValid(Vehicle))
{
auto Controller = Cast<AWheeledVehicleAIController>(Vehicle->GetController());
if (Controller != nullptr)
{
Controller->SetTrafficLight(nullptr);
Controller->SetTrafficLightState(ETrafficLightState::Green);
}
}
}
void ATrafficLightBase::SetGreenTime(float InGreenTime)
{
GreenTime = InGreenTime;

3
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightBase.h
View File

@ -54,6 +54,9 @@ public:
UFUNCTION(Category = "Traffic Light", BlueprintCallable)
void NotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle);
UFUNCTION(Category = "Traffic Light", BlueprintCallable)
void UnNotifyWheeledVehicle(ACarlaWheeledVehicle *Vehicle);
UFUNCTION(Category = "Traffic Light", BlueprintCallable)
void SetGreenTime(float InGreenTime);

318
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
View File

@ -135,11 +135,7 @@ void AWheeledVehicleAIController::Tick(const float DeltaTime)
return;
}
if (bAutopilotEnabled)
{
Vehicle->ApplyVehicleControl(TickAutopilotController(), EVehicleInputPriority::Autopilot);
}
else if (!bControlIsSticky)
if (!bAutopilotEnabled && !bControlIsSticky)
{
Vehicle->ApplyVehicleControl(FVehicleControl{}, EVehicleInputPriority::Relaxation);
}
@ -170,29 +166,6 @@ void AWheeledVehicleAIController::ConfigureAutopilot(const bool Enable, const bo
}
TrafficLightState = ETrafficLightState::Green;
/// @todo Workaround for a race condition between client and server when
/// enabling autopilot right after initializing a vehicle.
if (bAutopilotEnabled)
{
for (TActorIterator<ARoutePlanner> It(GetWorld()); It; ++It)
{
ARoutePlanner *RoutePlanner = *It;
// Check if we are inside this route planner.
TSet<AActor *> OverlappingActors;
RoutePlanner->TriggerVolume->GetOverlappingActors(
OverlappingActors,
ACarlaWheeledVehicle::StaticClass());
for (auto *Actor : OverlappingActors)
{
if (Actor == Vehicle)
{
RoutePlanner->AssignRandomRoute(*this);
return;
}
}
}
}
}
// =============================================================================
@ -212,292 +185,3 @@ void AWheeledVehicleAIController::SetFixedRoute(
TargetLocations.emplace(Location);
}
}
// =============================================================================
// -- AI -----------------------------------------------------------------------
// =============================================================================
FVehicleControl AWheeledVehicleAIController::TickAutopilotController()
{
#if WITH_EDITOR // This happens in simulation mode in editor.
if (Vehicle == nullptr)
{
bAutopilotEnabled = false;
return {};
}
#endif // WITH_EDITOR
check(Vehicle != nullptr);
FVector Direction;
float Steering;
if (!TargetLocations.empty())
{
Steering = GoToNextTargetLocation(Direction);
}
else
{
Steering = RoadMap != nullptr ? CalcStreeringValue(Direction) : 0.0f;
Direction = Vehicle->GetVehicleTransform().GetRotation().Rotator().Vector();
}
// Speed in km/h.
const auto Speed = Vehicle->GetVehicleForwardSpeed() * 0.036f;
float Throttle;
if (TrafficLightState != ETrafficLightState::Green)
{
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::WaitingForRedLight);
Throttle = Stop(Speed);
}
else if (IsThereAnObstacleAhead(*Vehicle, Speed, Direction))
{
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::ObstacleAhead);
Throttle = Stop(Speed);
}
else
{
Throttle = Move(Speed);
}
FVehicleControl AutopilotControl;
if (Throttle < 0.001f)
{
AutopilotControl.Brake = 1.0f;
AutopilotControl.Throttle = 0.0f;
}
else
{
AutopilotControl.Brake = 0.0f;
AutopilotControl.Throttle = Throttle;
}
AutopilotControl.Steer = Steering;
return AutopilotControl;
}
float AWheeledVehicleAIController::GoToNextTargetLocation(FVector &Direction)
{
// Get middle point between the two front wheels.
const auto CurrentLocation = [&]() {
const auto &Wheels = Vehicle->GetVehicleMovementComponent()->Wheels;
check((Wheels.Num() > 1) && (Wheels[0u] != nullptr) && (Wheels[1u] != nullptr));
return (Wheels[0u]->Location + Wheels[1u]->Location) / 2.0f;
} ();
const auto Target = [&]() {
const auto &Result = TargetLocations.front();
return FVector{Result.X, Result.Y, CurrentLocation.Z};
} ();
if (Target.Equals(CurrentLocation, 200.0f))
{
TargetLocations.pop();
if (!TargetLocations.empty())
{
return GoToNextTargetLocation(Direction);
}
else
{
return RoadMap != nullptr ? CalcStreeringValue(Direction) : 0.0f;
}
}
Direction = (Target - CurrentLocation).GetSafeNormal();
const FVector &Forward = GetPawn()->GetActorForwardVector();
float dirAngle = Direction.UnitCartesianToSpherical().Y;
float actorAngle = Forward.UnitCartesianToSpherical().Y;
dirAngle *= (180.0f / PI);
actorAngle *= (180.0 / PI);
float angle = dirAngle - actorAngle;
if (angle > 180.0f)
{
angle -= 360.0f;
}
else if (angle < -180.0f)
{
angle += 360.0f;
}
float Steering = 0.0f;
if (angle < -MaximumSteerAngle)
{
Steering = -1.0f;
}
else if (angle > MaximumSteerAngle)
{
Steering = 1.0f;
}
else
{
Steering += angle / MaximumSteerAngle;
}
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::FollowingFixedRoute);
return Steering;
}
float AWheeledVehicleAIController::CalcStreeringValue(FVector &direction)
{
float steering = 0;
FVector BoxExtent = Vehicle->GetVehicleBoundingBoxExtent();
FVector forward = Vehicle->GetActorForwardVector();
FVector rightSensorPosition(BoxExtent.X / 2.0f, (BoxExtent.Y / 2.0f) + 100.0f, 0.0f);
FVector leftSensorPosition(BoxExtent.X / 2.0f, -(BoxExtent.Y / 2.0f) - 100.0f, 0.0f);
float forwardMagnitude = BoxExtent.X / 2.0f;
float Magnitude =
(float) sqrt(pow((double) leftSensorPosition.X, 2.0) + pow((double) leftSensorPosition.Y, 2.0));
// same for the right and left
float offset = FGenericPlatformMath::Acos(forwardMagnitude / Magnitude);
float actorAngle = forward.UnitCartesianToSpherical().Y;
float sinR = FGenericPlatformMath::Sin(actorAngle + offset);
float cosR = FGenericPlatformMath::Cos(actorAngle + offset);
float sinL = FGenericPlatformMath::Sin(actorAngle - offset);
float cosL = FGenericPlatformMath::Cos(actorAngle - offset);
rightSensorPosition.Y = sinR * Magnitude;
rightSensorPosition.X = cosR * Magnitude;
leftSensorPosition.Y = sinL * Magnitude;
leftSensorPosition.X = cosL * Magnitude;
FVector rightPositon = GetPawn()->GetActorLocation() + FVector(rightSensorPosition.X,
rightSensorPosition.Y,
0.0f);
FVector leftPosition = GetPawn()->GetActorLocation() + FVector(leftSensorPosition.X,
leftSensorPosition.Y,
0.0f);
FRoadMapPixelData rightRoadData = RoadMap->GetDataAt(rightPositon);
if (!rightRoadData.IsRoad())
{
steering -= 0.2f;
}
FRoadMapPixelData leftRoadData = RoadMap->GetDataAt(leftPosition);
if (!leftRoadData.IsRoad())
{
steering += 0.2f;
}
FRoadMapPixelData roadData = RoadMap->GetDataAt(GetPawn()->GetActorLocation());
if (!roadData.IsRoad())
{
steering = 0.0f;
}
else if (roadData.HasDirection())
{
direction = roadData.GetDirection();
FVector right = rightRoadData.GetDirection();
FVector left = leftRoadData.GetDirection();
forward.Z = 0.0f;
float dirAngle = direction.UnitCartesianToSpherical().Y;
float rightAngle = right.UnitCartesianToSpherical().Y;
float leftAngle = left.UnitCartesianToSpherical().Y;
dirAngle *= (180.0f / PI);
rightAngle *= (180.0 / PI);
leftAngle *= (180.0 / PI);
actorAngle *= (180.0 / PI);
float min = dirAngle - 90.0f;
if (min < -180.0f)
{
min = 180.0f + (min + 180.0f);
}
float max = dirAngle + 90.0f;
if (max > 180.0f)
{
max = -180.0f + (max - 180.0f);
}
if (dirAngle < -90.0 || dirAngle > 90.0)
{
if (rightAngle < min && rightAngle > max)
{
steering -= 0.2f;
}
if (leftAngle < min && leftAngle > max)
{
steering += 0.2f;
}
}
else
{
if (rightAngle < min || rightAngle > max)
{
steering -= 0.2f;
}
if (leftAngle < min || leftAngle > max)
{
steering += 0.2f;
}
}
float angle = dirAngle - actorAngle;
if (angle > 180.0f)
{
angle -= 360.0f;
}
else if (angle < -180.0f)
{
angle += 360.0f;
}
if (angle < -MaximumSteerAngle)
{
steering = -1.0f;
}
else if (angle > MaximumSteerAngle)
{
steering = 1.0f;
}
else
{
steering += angle / MaximumSteerAngle;
}
}
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::FreeDriving);
return steering;
}
float AWheeledVehicleAIController::Stop(const float Speed)
{
return (Speed >= 1.0f ? -Speed / SpeedLimit : 0.0f);
}
float AWheeledVehicleAIController::Move(const float Speed)
{
if (Speed >= SpeedLimit)
{
return Stop(Speed);
}
else if (Speed >= SpeedLimit - 10.0f)
{
return 0.5f;
}
else
{
return 1.0f;
}
}

22
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.h
View File

@ -210,28 +210,6 @@ public:
/// @}
private:
FVehicleControl TickAutopilotController();
/// Returns steering value.
float GoToNextTargetLocation(FVector &Direction);
/// Returns steering value.
float CalcStreeringValue(FVector &Direction);
/// Returns throttle value.
float Stop(float Speed);
/// Returns throttle value.
float Move(float Speed);
/// @}
// ===========================================================================
// -- Member variables -------------------------------------------------------
// ===========================================================================
/// @{
private:
UPROPERTY()

2
Util/ContentVersions.txt
View File

@ -29,4 +29,4 @@
0.9.5: 20190404_c7b464a
0.9.6: 20190710_0097e66
0.9.7: 20191211_f63ea8b
0.9.7: 20191221_c88604b