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

Fixing code formatting

This commit is contained in:
dotero 2019-12-11 14:12:17 +01:00 committed by bernat
parent 2362f71216
commit e9a2935a5a
39 changed files with 1069 additions and 904 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LibCarla/source/carla/Memory.h
View File

@ -1,4 +1,4 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// 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.

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

@ -1,4 +1,4 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// 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.
@ -6,8 +6,8 @@
#include "carla/client/Vehicle.h"
#include "carla/client/detail/Simulator.h"
#include "carla/client/ActorList.h"
#include "carla/client/detail/Simulator.h"
#include "carla/client/TrafficLight.h"
#include "carla/Memory.h"
#include "carla/rpc/TrafficLightState.h"
@ -34,8 +34,6 @@ namespace client {
_is_control_sticky(GetControlIsSticky(GetAttributes())) {}
void Vehicle::SetAutopilot(bool enabled) {
// GetEpisode().Lock()->SetVehicleAutopilot(*this, enabled);
// std::cout << "Calling SetAutopilot" << std::endl;
if (enabled) {
TM &tm = TM::GetInstance(TM::GetUniqueLocalClient());
tm.RegisterVehicles({carla::SharedPtr<carla::client::Actor>(this)});

5
LibCarla/source/carla/client/Vehicle.h
View File

@ -1,4 +1,4 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// 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.
@ -7,12 +7,13 @@
#pragma once
#include "carla/client/Actor.h"
#include "carla/rpc/TrafficLightState.h"
#include "carla/rpc/VehicleControl.h"
#include "carla/rpc/VehiclePhysicsControl.h"
#include "carla/rpc/TrafficLightState.h"
namespace carla {
namespace client {
class TrafficLight;
class Vehicle : public Actor {

100
LibCarla/source/carla/trafficmanager/AtomicActorSet.h
View File

@ -1,3 +1,9 @@
// 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 <mutex>
@ -9,68 +15,68 @@
namespace traffic_manager {
namespace cc = carla::client;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
namespace cc = carla::client;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
class AtomicActorSet {
class AtomicActorSet {
private:
private:
std::mutex modification_mutex;
std::unordered_map<ActorId, ActorPtr> actor_set;
int state_counter;
std::mutex modification_mutex;
std::unordered_map<ActorId, ActorPtr> actor_set;
int state_counter;
public:
public:
AtomicActorSet(): state_counter(0) {}
AtomicActorSet(): state_counter(0) {}
std::vector<ActorPtr> GetList() {
std::vector<ActorPtr> GetList() {
std::lock_guard<std::mutex> lock(modification_mutex);
std::vector<ActorPtr> actor_list;
for (auto it = actor_set.begin(); it != actor_set.end(); ++it) {
actor_list.push_back(it->second);
}
return actor_list;
}
std::lock_guard<std::mutex> lock(modification_mutex);
std::vector<ActorPtr> actor_list;
for (auto it = actor_set.begin(); it != actor_set.end(); ++it) {
actor_list.push_back(it->second);
}
return actor_list;
}
void Insert(std::vector<ActorPtr> actor_list) {
void Insert(std::vector<ActorPtr> actor_list) {
std::lock_guard<std::mutex> lock(modification_mutex);
for (auto &actor: actor_list) {
actor_set.insert({actor->GetId(), actor});
}
++state_counter;
}
std::lock_guard<std::mutex> lock(modification_mutex);
for (auto &actor: actor_list) {
actor_set.insert({actor->GetId(), actor});
}
++state_counter;
}
void Remove(std::vector<ActorPtr> actor_list) {
void Remove(std::vector<ActorPtr> actor_list) {
std::lock_guard<std::mutex> lock(modification_mutex);
for (auto& actor: actor_list) {
actor_set.erase(actor->GetId());
}
++state_counter;
}
std::lock_guard<std::mutex> lock(modification_mutex);
for (auto& actor: actor_list) {
actor_set.erase(actor->GetId());
}
++state_counter;
}
void Destroy(ActorPtr actor) {
void Destroy(ActorPtr actor) {
std::lock_guard<std::mutex> lock(modification_mutex);
actor_set.erase(actor->GetId());
actor->Destroy();
++state_counter;
}
std::lock_guard<std::mutex> lock(modification_mutex);
actor_set.erase(actor->GetId());
actor->Destroy();
++state_counter;
}
int GetState() {
int GetState() {
std::lock_guard<std::mutex> lock(modification_mutex);
return state_counter;
}
std::lock_guard<std::mutex> lock(modification_mutex);
return state_counter;
}
bool Contains(ActorId id) {
bool Contains(ActorId id) {
return actor_set.find(id) != actor_set.end();
}
};
return actor_set.find(id) != actor_set.end();
}
};
}
} // namespace traffic_manager

58
LibCarla/source/carla/trafficmanager/AtomicMap.h
View File

@ -1,42 +1,52 @@
// 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 <mutex>
#include <unordered_map>
template <typename Key, typename Value>
class AtomicMap {
namespace traffic_manager {
private:
template <typename Key, typename Value>
class AtomicMap {
std::mutex map_mutex;
std::unordered_map<Key, Value> map;
private:
public:
std::mutex map_mutex;
std::unordered_map<Key, Value> map;
AtomicMap() {}
public:
void AddEntry(const std::pair<Key, Value> &entry) {
AtomicMap() {}
std::lock_guard<std::mutex> lock(map_mutex);
map.insert(entry);
}
void AddEntry(const std::pair<Key, Value> &entry) {
bool Contains(const Key &key) {
std::lock_guard<std::mutex> lock(map_mutex);
map.insert(entry);
}
std::lock_guard<std::mutex> lock(map_mutex);
return map.find(key) != map.end();
}
bool Contains(const Key &key) {
Value &GetValue(const Key &key) {
std::lock_guard<std::mutex> lock(map_mutex);
return map.find(key) != map.end();
}
std::lock_guard<std::mutex> lock(map_mutex);
return map.at(key);
}
Value &GetValue(const Key &key) {
void RemoveEntry(const Key &key) {
std::lock_guard<std::mutex> lock(map_mutex);
return map.at(key);
}
std::lock_guard<std::mutex> lock(map_mutex);
map.erase(key);
}
void RemoveEntry(const Key &key) {
};
std::lock_guard<std::mutex> lock(map_mutex);
map.erase(key);
}
};
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 "BatchControlStage.h"
namespace traffic_manager {
@ -25,8 +31,8 @@ namespace traffic_manager {
cr::VehicleControl vehicle_control;
PlannerToControlData &element = data_frame->at(i);
carla::ActorId actor_id = element.actor_id;
const PlannerToControlData &element = data_frame->at(i);
const carla::ActorId actor_id = element.actor_id;
vehicle_control.throttle = element.throttle;
vehicle_control.brake = element.brake;
vehicle_control.steer = element.steer;
@ -61,4 +67,5 @@ namespace traffic_manager {
// Limiting updates to 100 frames per second.
std::this_thread::sleep_for(10ms);
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <memory>
@ -13,8 +19,8 @@
namespace traffic_manager {
namespace cc = carla::client;
namespace cr = carla::rpc;
namespace cc = carla::client;
namespace cr = carla::rpc;
/// This class receives actuation signals (throttle, brake, steer)
/// from MotionPlannerStage class and communicates these signals to
@ -39,9 +45,9 @@ namespace cr = carla::rpc;
public:
BatchControlStage(
std::string stage_name,
std::shared_ptr<PlannerToControlMessenger> messenger,
cc::Client &carla_client);
std::string stage_name,
std::shared_ptr<PlannerToControlMessenger> messenger,
cc::Client &carla_client);
~BatchControlStage();
void DataReceiver() override;
@ -52,4 +58,4 @@ namespace cr = carla::rpc;
};
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 traffic_manager {
@ -10,4 +16,4 @@ namespace traffic_manager {
std::vector<std::pair<WaypointPtr, WaypointPtr>> CarlaDataAccessLayer::GetTopology() const {
return world_map->GetTopology();
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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>
@ -8,7 +14,7 @@
namespace traffic_manager {
namespace cc = carla::client;
namespace cc = carla::client;
/// This class is responsible for retrieving data from the server.
class CarlaDataAccessLayer {
@ -28,4 +34,4 @@ namespace cc = carla::client;
};
}
} // namespace traffic_manager

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

@ -1,8 +1,15 @@
// 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 "CollisionStage.h"
namespace traffic_manager {
namespace CollisionStageConstants {
static const float VERTICAL_OVERLAP_THRESHOLD = 2.0f;
static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f;
static const float EXTENSION_SQUARE_POINT = 7.5f;
@ -11,10 +18,11 @@ namespace CollisionStageConstants {
static const float HIGHWAY_TIME_HORIZON = 5.0f;
static const float CRAWL_SPEED = 10.0f / 3.6f;
static const float BOUNDARY_EDGE_LENGTH = 2.0f;
//static const float MINIMUM_GRID_EXTENSION = 10.0f;
static const float MAX_COLLISION_RADIUS = 100.0f;
}
using namespace CollisionStageConstants;
} // namespace CollisionStageConstants
using namespace CollisionStageConstants;
CollisionStage::CollisionStage(
std::string stage_name,
@ -46,20 +54,20 @@ namespace CollisionStageConstants {
CollisionStage::~CollisionStage() {}
void CollisionStage::Action() {
auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;
const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;
// Handle vehicles not spawned by TrafficManager.
auto current_time = chr::system_clock::now();
chr::duration<double> diff = current_time - last_world_actors_pass_instance;
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) {
auto world_actors = world.GetActors()->Filter("vehicle.*");
auto world_walker = world.GetActors()->Filter("walker.*");
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()) {
auto unregistered_id = actor->GetId();
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});
@ -67,7 +75,7 @@ namespace CollisionStageConstants {
}
// Scanning for pedestrians.
for (auto walker: *world_walker.get()) {
auto unregistered_id = walker->GetId();
const auto unregistered_id = walker->GetId();
if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) {
unregistered_actors.insert({unregistered_id, walker});
}
@ -92,9 +100,9 @@ namespace CollisionStageConstants {
// Looping over registered actors.
for (uint i = 0u; i < number_of_vehicles; ++i) {
LocalizationToCollisionData &data = localization_frame->at(i);
Actor ego_actor = data.actor;
ActorId ego_actor_id = ego_actor->GetId();
const LocalizationToCollisionData &data = localization_frame->at(i);
const Actor ego_actor = data.actor;
const ActorId ego_actor_id = ego_actor->GetId();
// Retrieve actors around the path of the ego vehicle.
std::unordered_set<ActorId> actor_id_list = GetPotentialVehicleObstacles(ego_actor);
@ -102,13 +110,13 @@ namespace CollisionStageConstants {
bool collision_hazard = false;
// Generate number between 0 and 100
int r = rand() % 101;
const int r = rand() % 101;
// 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) {
ActorId actor_id = *j;
const ActorId actor_id = *j;
try {
Actor actor = nullptr;
@ -118,20 +126,14 @@ namespace CollisionStageConstants {
actor = unregistered_actors.at(actor_id);
}
cg::Location ego_location = ego_actor->GetLocation();
cg::Location other_location = actor->GetLocation();
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)) {
// debug_helper.DrawLine(
// ego_location + cg::Location(0, 0, 2),
// other_location + cg::Location(0, 0, 2),
// 0.2f, {255u, 0u, 0u}, 0.1f
// ); THE PROBLEM IS SOMEWHERE HERE WITH COLLISIONS, IT RETURNS FALSE FOR COLLISION HAZARD
if (parameters.GetCollisionDetection(ego_actor, actor) &&
NegotiateCollision(ego_actor, actor)) {
@ -154,7 +156,7 @@ namespace CollisionStageConstants {
}
void CollisionStage::DataReceiver() {
auto packet = localization_messenger->ReceiveData(localization_messenger_state);
const auto packet = localization_messenger->ReceiveData(localization_messenger_state);
localization_frame = packet.data;
localization_messenger_state = packet.id;
@ -182,7 +184,7 @@ namespace CollisionStageConstants {
void CollisionStage::DataSender() {
DataPacket<std::shared_ptr<CollisionToPlannerFrame>> packet{
const DataPacket<std::shared_ptr<CollisionToPlannerFrame>> packet{
planner_messenger_state,
frame_selector ? planner_frame_a : planner_frame_b
};
@ -200,65 +202,57 @@ namespace CollisionStageConstants {
auto& other_packet = localization_frame->at(vehicle_id_to_index.at(other_vehicle->GetId()));
Buffer& other_buffer = other_packet.buffer;
cg::Location reference_location = reference_vehicle->GetLocation();
cg::Location other_location = other_vehicle->GetLocation();
const cg::Location reference_location = reference_vehicle->GetLocation();
const cg::Location other_location = other_vehicle->GetLocation();
cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector();
const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector();
cg::Vector3D reference_to_other = other_location - reference_location;
reference_to_other = reference_to_other.MakeUnitVector();
auto reference_vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(reference_vehicle);
auto other_vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(other_vehicle);
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()) {
Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle));
Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle));
Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle));
Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle));
const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle));
const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle));
const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle));
const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle));
double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon);
double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon);
const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon);
const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon);
auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector();
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
&&
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)
)
)
) {
if (inter_geodesic_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()) {
float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x;
float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x;
float vehicle_length_sum = reference_vehicle_length + other_vehicle_length;
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;
float bbox_extension_length = GetBoundingBoxExtention(reference_vehicle);
const float bbox_extension_length = GetBoundingBoxExtention(reference_vehicle);
if ((cg::Math::Dot(reference_heading, reference_to_other) > 0) &&
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))) {
@ -285,26 +279,26 @@ namespace CollisionStageConstants {
LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor) const {
LocationList bbox = GetBoundary(actor);
const LocationList bbox = GetBoundary(actor);
if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) {
cg::Location vehicle_location = actor->GetLocation();
const cg::Location vehicle_location = actor->GetLocation();
float bbox_extension = GetBoundingBoxExtention(actor);
float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor);
if (specific_distance_margin > 0) {
const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor);
if (specific_distance_margin > 0.0f) {
bbox_extension = std::max(specific_distance_margin, bbox_extension);
}
auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer;
const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer;
LocationList left_boundary;
LocationList right_boundary;
auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
float width = vehicle->GetBoundingBox().extent.y;
float length = vehicle->GetBoundingBox().extent.x;
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const float width = vehicle->GetBoundingBox().extent.y;
const float length = vehicle->GetBoundingBox().extent.x;
SimpleWaypointPtr boundary_start = waypoint_buffer.front();
uint boundary_start_index = 0u;
@ -316,7 +310,7 @@ namespace CollisionStageConstants {
SimpleWaypointPtr boundary_end = nullptr;
SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index);
auto vehicle_reference = boost::static_pointer_cast<cc::Vehicle>(actor);
const auto vehicle_reference = boost::static_pointer_cast<cc::Vehicle>(actor);
// At non-signalized junctions, we extend the boundary across the junction
// and in all other situations, boundary length is velocity-dependent.
bool reached_distance = false;
@ -330,14 +324,14 @@ namespace CollisionStageConstants {
boundary_end->DistanceSquared(current_point) > std::pow(BOUNDARY_EDGE_LENGTH, 2) ||
reached_distance) {
cg::Vector3D heading_vector = current_point->GetForwardVector();
cg::Location location = current_point->GetLocation();
cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0);
const cg::Vector3D heading_vector = current_point->GetForwardVector();
const cg::Location location = current_point->GetLocation();
cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
perpendicular_vector = perpendicular_vector.MakeUnitVector();
// Direction determined for the left-handed system.
cg::Vector3D scaled_perpendicular = perpendicular_vector * width;
const cg::Vector3D scaled_perpendicular = perpendicular_vector * width;
left_boundary.push_back(location + cg::Location(scaled_perpendicular));
right_boundary.push_back(location + cg::Location(-1 * scaled_perpendicular));
right_boundary.push_back(location + cg::Location(-1.0f * scaled_perpendicular));
boundary_end = current_point;
}
@ -366,16 +360,17 @@ namespace CollisionStageConstants {
float CollisionStage::GetBoundingBoxExtention(const Actor &actor) const {
float velocity = actor->GetVelocity().Length();
const float velocity = actor->GetVelocity().Length();
float bbox_extension = BOUNDARY_EXTENSION_MINIMUM;
if (velocity > HIGHWAY_SPEED) {
bbox_extension = HIGHWAY_TIME_HORIZON * velocity;
} else if (velocity < CRAWL_SPEED) {
bbox_extension = BOUNDARY_EXTENSION_MINIMUM;
} else {
bbox_extension = std::sqrt(EXTENSION_SQUARE_POINT * velocity) +
velocity * TIME_HORIZON +
BOUNDARY_EXTENSION_MINIMUM;
bbox_extension = std::sqrt(
EXTENSION_SQUARE_POINT * velocity) +
velocity * TIME_HORIZON +
BOUNDARY_EXTENSION_MINIMUM;
}
return bbox_extension;
@ -385,9 +380,9 @@ namespace CollisionStageConstants {
vicinity_grid.UpdateGrid(ego_vehicle);
auto& data_packet = localization_frame->at(vehicle_id_to_index.at(ego_vehicle->GetId()));
Buffer &waypoint_buffer = data_packet.buffer;
float velocity = ego_vehicle->GetVelocity().Length();
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) {
@ -400,7 +395,7 @@ namespace CollisionStageConstants {
}
LocationList CollisionStage::GetBoundary(const Actor &actor) const {
auto actor_type = actor->GetTypeId();
const auto actor_type = actor->GetTypeId();
cg::BoundingBox bbox;
cg::Location location;
@ -408,40 +403,41 @@ namespace CollisionStageConstants {
if (actor_type[0] == 'v') {
auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
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') {
auto walker = boost::static_pointer_cast<cc::Walker>(actor);
const auto walker = boost::static_pointer_cast<cc::Walker>(actor);
bbox = walker->GetBoundingBox();
location = walker->GetLocation();
heading_vector = walker->GetTransform().GetForwardVector();
}
cg::Vector3D extent = bbox.extent;
heading_vector.z = 0;
cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0);
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);
// Four corners of the vehicle in top view clockwise order (left-handed
// system).
cg::Vector3D x_boundary_vector = heading_vector * extent.x;
cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y;
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 * x_boundary_vector - y_boundary_vector),
location + cg::Location(-1 * x_boundary_vector + y_boundary_vector),
location + cg::Location(x_boundary_vector + y_boundary_vector),
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),
};
}
void CollisionStage::DrawBoundary(const LocationList &boundary) const {
for (uint i = 0u; i < boundary.size(); ++i) {
debug_helper.DrawLine(
boundary[i] + cg::Location(0, 0, 1),
boundary[(i + 1) % boundary.size()] + cg::Location(0, 0, 1),
boundary[i] + cg::Location(0.0f, 0.0f, 1.0f),
boundary[(i + 1) % boundary.size()] + cg::Location(0.0f, 0.0f, 1.0f),
0.1f, {255u, 0u, 0u}, 0.1f);
}
}
}
} // namespace traffic_manager

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

@ -1,12 +1,18 @@
// 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 <algorithm>
#include <cmath>
#include <deque>
#include <stdlib.h>
#include <string>
#include <unordered_map>
#include <vector>
#include <stdlib.h>
#include "boost/geometry.hpp"
#include "boost/geometry/geometries/point_xy.hpp"
@ -30,10 +36,10 @@
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
namespace chr = std::chrono;
namespace bg = boost::geometry;
namespace cc = carla::client;
namespace cg = carla::geom;
namespace chr = std::chrono;
namespace bg = boost::geometry;
using ActorId = carla::ActorId;
using Actor = carla::SharedPtr<cc::Actor>;
@ -107,12 +113,12 @@ namespace bg = boost::geometry;
public:
CollisionStage(
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);
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);
~CollisionStage();
@ -124,4 +130,4 @@ namespace bg = boost::geometry;
};
}
} // namespace traffic_manager

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

@ -1,8 +1,15 @@
// 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 "InMemoryMap.h"
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();
@ -10,7 +17,9 @@ namespace MapConstants {
// Cosine of the angle.
static const float LANE_CHANGE_ANGULAR_THRESHOLD = 0.5f;
static const float GRID_SIZE = 4.0f;
}
} // namespace MapConstants
using namespace MapConstants;
InMemoryMap::InMemoryMap(TopologyList topology) {
@ -23,7 +32,8 @@ namespace MapConstants {
NodeList entry_node_list;
NodeList exit_node_list;
auto distance_squared = [](cg::Location l1, cg::Location l2) {
auto distance_squared =
[](cg::Location l1, cg::Location l2) {
return cg::Math::DistanceSquared(l1, l2);
};
auto square = [](float input) {return std::pow(input, 2);};
@ -32,10 +42,10 @@ namespace MapConstants {
for (auto &pair : _topology) {
// Looping through every topology segment.
WaypointPtr begin_waypoint = pair.first;
WaypointPtr end_waypoint = pair.second;
cg::Location begin_location = begin_waypoint->GetTransform().location;
cg::Location end_location = end_waypoint->GetTransform().location;
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;
if (distance_squared(begin_location, end_location) > square(ZERO_LENGTH)) {
@ -66,7 +76,7 @@ namespace MapConstants {
}
// Linking segments.
uint i = 0, j = 0;
uint 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) and i != j) {
@ -81,10 +91,10 @@ namespace MapConstants {
// Loop through all exit nodes of topology segments,
// connect any dangling endpoints to the nearest entry point
// of another topology segment.
i = 0;
i = 0u;
for (auto &end_point : exit_node_list) {
if (end_point->GetNextWaypoint().size() == 0) {
j = 0;
j = 0u;
float min_distance = INFINITE_DISTANCE;
SimpleWaypointPtr closest_connection;
for (auto &begin_point : entry_node_list) {
@ -95,13 +105,13 @@ namespace MapConstants {
}
++j;
}
cg::Vector3D end_point_vector = end_point->GetForwardVector();
const cg::Vector3D end_point_vector = end_point->GetForwardVector();
cg::Vector3D relative_vector = closest_connection->GetLocation() - end_point->GetLocation();
relative_vector = relative_vector.MakeUnitVector();
float relative_dot = cg::Math::Dot(end_point_vector, relative_vector);
const float relative_dot = cg::Math::Dot(end_point_vector, relative_vector);
if (relative_dot < LANE_CHANGE_ANGULAR_THRESHOLD) {
uint count = LANE_CHANGE_LOOK_AHEAD;
while (count > 0) {
while (count > 0u) {
closest_connection = closest_connection->GetNextWaypoint()[0];
--count;
}
@ -113,8 +123,8 @@ namespace MapConstants {
// Localizing waypoints into grids.
for (auto &simple_waypoint: dense_topology) {
cg::Location loc = simple_waypoint->GetLocation();
std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y));
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 {
@ -140,7 +150,7 @@ namespace MapConstants {
SimpleWaypointPtr InMemoryMap::GetWaypointInVicinity(cg::Location location) {
std::pair<int, int> grid_ids = MakeGridId(location.x, location.y);
const std::pair<int, int> grid_ids = MakeGridId(location.x, location.y);
SimpleWaypointPtr closest_waypoint = nullptr;
float closest_distance = INFINITE_DISTANCE;
@ -148,10 +158,10 @@ namespace MapConstants {
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j});
const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j});
if (waypoint_grid.find(grid_key) != waypoint_grid.end()) {
auto& waypoint_set = waypoint_grid.at(grid_key);
const auto& waypoint_set = waypoint_grid.at(grid_key);
if (closest_waypoint == nullptr) {
closest_waypoint = *waypoint_set.begin();
}
@ -183,7 +193,7 @@ namespace MapConstants {
SimpleWaypointPtr closest_waypoint;
float min_distance = INFINITE_DISTANCE;
for (auto &simple_waypoint : dense_topology) {
float current_distance = simple_waypoint->DistanceSquared(location);
const float current_distance = simple_waypoint->DistanceSquared(location);
if (current_distance < min_distance) {
min_distance = current_distance;
closest_waypoint = simple_waypoint;
@ -198,16 +208,16 @@ namespace MapConstants {
void InMemoryMap::FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint) {
WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange();
auto change_right = crd::element::LaneMarking::LaneChange::Right;
auto change_left = crd::element::LaneMarking::LaneChange::Left;
auto change_both = crd::element::LaneMarking::LaneChange::Both;
const WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
const crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange();
const auto change_right = crd::element::LaneMarking::LaneChange::Right;
const auto change_left = crd::element::LaneMarking::LaneChange::Left;
const auto change_both = crd::element::LaneMarking::LaneChange::Both;
try {
if (lane_change == change_right || lane_change == change_both) {
WaypointPtr right_waypoint = raw_waypoint->GetRight();
const WaypointPtr right_waypoint = raw_waypoint->GetRight();
if (right_waypoint != nullptr &&
right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
(right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
@ -222,7 +232,7 @@ namespace MapConstants {
if (lane_change == change_left || lane_change == change_both) {
WaypointPtr left_waypoint = raw_waypoint->GetLeft();
const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
if (left_waypoint != nullptr &&
left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
(left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
@ -237,10 +247,11 @@ namespace MapConstants {
} catch (const std::invalid_argument &e) {
cg::Location loc = reference_waypoint->GetLocation();
carla::log_info(
"Unable to link lane change connection at: "
+ std::to_string(loc.x) + " "
+ std::to_string(loc.y) + " "
+ std::to_string(loc.z));
"Unable to link lane change connection at: "
+ std::to_string(loc.x) + " "
+ std::to_string(loc.y) + " "
+ std::to_string(loc.z));
}
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <chrono>
@ -17,9 +23,9 @@
namespace traffic_manager {
namespace cg = carla::geom;
namespace cc = carla::client;
namespace crd = carla::road;
namespace cg = carla::geom;
namespace cc = carla::client;
namespace crd = carla::road;
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
@ -71,4 +77,4 @@ namespace crd = carla::road;
};
}
} // namespace traffic_manager

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

@ -1,8 +1,15 @@
// 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 "LocalizationStage.h"
namespace traffic_manager {
namespace LocalizationConstants {
static const float WAYPOINT_TIME_HORIZON = 3.0f;
static const float MINIMUM_HORIZON_LENGTH = 30.0f;
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f;
@ -11,7 +18,9 @@ namespace LocalizationConstants {
static const float HIGHWAY_SPEED = 50 / 3.6f;
static const float MINIMUM_LANE_CHANGE_DISTANCE = 20.0f;
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.93969f;
}
} // namespace LocalizationConstants
using namespace LocalizationConstants;
LocalizationStage::LocalizationStage(
@ -55,18 +64,18 @@ namespace LocalizationConstants {
void LocalizationStage::Action() {
// Selecting output frames based on selector keys.
auto current_planner_frame = planner_frame_selector ? planner_frame_a : planner_frame_b;
auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b;
auto current_traffic_light_frame =
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 (uint i = 0u; i < actor_list.size(); ++i) {
Actor vehicle = actor_list.at(i);
ActorId actor_id = vehicle->GetId();
cg::Location vehicle_location = vehicle->GetLocation();
float vehicle_velocity = vehicle->GetVelocity().Length();
const Actor vehicle = actor_list.at(i);
const ActorId actor_id = vehicle->GetId();
const cg::Location vehicle_location = vehicle->GetLocation();
const float vehicle_velocity = vehicle->GetVelocity().Length();
//TODO: Improve search so it doesn't do it every loop..
auto search = idle_time.find(actor_id);
@ -74,7 +83,7 @@ namespace LocalizationConstants {
idle_time[actor_id] = chr::system_clock::now();
}
float horizon_size = std::max(
const float horizon_size = std::max(
WAYPOINT_TIME_HORIZON * std::sqrt(vehicle_velocity * 10.0f),
MINIMUM_HORIZON_LENGTH);
@ -84,9 +93,8 @@ namespace LocalizationConstants {
if (!waypoint_buffer.empty()) {
float dot_product = DeviationDotProduct(vehicle, waypoint_buffer.front()->GetLocation(), true);
while (dot_product <= 0 && !waypoint_buffer.empty()) {
while (dot_product <= 0.0f && !waypoint_buffer.empty()) {
//ResetIdleTime(vehicle);
PopWaypoint(waypoint_buffer, actor_id);
if (!waypoint_buffer.empty()) {
dot_product = DeviationDotProduct(vehicle, waypoint_buffer.front()->GetLocation(), true);
@ -104,10 +112,10 @@ namespace LocalizationConstants {
}
// Assign a lane change.
SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(vehicle);
bool force_lane_change = lane_change_info.change_lane;
bool lane_change_direction = lane_change_info.direction;
const SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
const ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(vehicle);
const bool force_lane_change = lane_change_info.change_lane;
const bool lane_change_direction = lane_change_info.direction;
if ((parameters.GetAutoLaneChange(vehicle) || force_lane_change) &&
!front_waypoint->CheckJunction()) {
@ -117,7 +125,7 @@ namespace LocalizationConstants {
if (change_over_point != nullptr) {
auto number_of_pops = waypoint_buffer.size();
for (uint j = 0; j < number_of_pops; ++j) {
for (uint j = 0u; j < number_of_pops; ++j) {
PopWaypoint(waypoint_buffer, actor_id);
}
@ -140,7 +148,7 @@ namespace LocalizationConstants {
}
// Generating output.
float target_point_distance = std::max(std::ceil(vehicle_velocity * TARGET_WAYPOINT_TIME_HORIZON),
const float target_point_distance = std::max(std::ceil(vehicle_velocity * TARGET_WAYPOINT_TIME_HORIZON),
TARGET_WAYPOINT_HORIZON_LENGTH);
SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
for (uint j = 0u;
@ -150,38 +158,22 @@ namespace LocalizationConstants {
++j) {
target_waypoint = waypoint_buffer.at(j);
}
cg::Location target_location = target_waypoint->GetLocation();
const cg::Location target_location = target_waypoint->GetLocation();
float dot_product = DeviationDotProduct(vehicle, target_location);
float cross_product = DeviationCrossProduct(vehicle, target_location);
dot_product = 1 - dot_product;
if (cross_product < 0) {
dot_product *= -1;
dot_product = 1.0f - dot_product;
if (cross_product < 0.0f) {
dot_product *= -1.0f;
}
/* Calculate the distance between the car and the trajectory (TODO: Use in the PID)
auto Vehicle = boost::static_pointer_cast<cc::Vehicle>(vehicle);
// Calculate the parameters of the line
cg::Vector3D velocity = Vehicle->GetVelocity();
velocity.z = 0;
cg::Location trajectory_location_ini = waypoint_buffer.front()->GetLocation();
trajectory_location_ini.z = 0;
cg::Location trajectory_location_fin = target_location;
trajectory_location_fin.z = 0;
cg::Vector3D trajectory_vector = trajectory_location_fin - trajectory_location_ini;
float a = trajectory_vector.x;
float b = -trajectory_vector.y;
float c = b*trajectory_location_ini.y - a*trajectory_location_ini.x;
// Distance from the vehicle to the desired trajectory
float distance = (a*vehicle_location.x + b*vehicle_location.y + c) /
std::sqrt(std::pow(a,2.0f)+std::pow(b,2.0f));*/
float distance = 0; // TODO: use in PID
float distance = 0.0f; // TODO: use in PID
// Filtering out false junctions on highways.
// On highways, if there is only one possible path and the section is
// marked as intersection, ignore it.
auto vehicle_reference = boost::static_pointer_cast<cc::Vehicle>(vehicle);
float speed_limit = vehicle_reference->GetSpeedLimit();
float look_ahead_distance = std::max(2 * vehicle_velocity, MINIMUM_JUNCTION_LOOK_AHEAD);
const auto vehicle_reference = boost::static_pointer_cast<cc::Vehicle>(vehicle);
const float speed_limit = vehicle_reference->GetSpeedLimit();
const float look_ahead_distance = std::max(2.0f * vehicle_velocity, MINIMUM_JUNCTION_LOOK_AHEAD);
SimpleWaypointPtr look_ahead_point = waypoint_buffer.front();
uint look_ahead_index = 0u;
@ -209,20 +201,21 @@ namespace LocalizationConstants {
}
// Clean up tracking list by remove vehicles that are too far away.
ActorIdSet current_tracking_list = track_traffic.GetOverlappingVehicles(actor_id);
const ActorIdSet current_tracking_list = track_traffic.GetOverlappingVehicles(actor_id);
for (ActorId tracking_id: current_tracking_list) {
if (!waypoint_buffer.empty()) {
cg::Location tracking_location = actor_list.at(
vehicle_id_to_index.at(tracking_id))->GetLocation();
const cg::Location tracking_location = actor_list.at(
vehicle_id_to_index.at(tracking_id))->GetLocation();
cg::Location buffer_front_loc = waypoint_buffer.front()->GetLocation();
cg::Location buffer_mid_lock = waypoint_buffer.at(
static_cast<uint>(std::floor(waypoint_buffer.size()/2)))->GetLocation();
cg::Location buffer_back_loc = waypoint_buffer.back()->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();
double squared_buffer_length = std::pow(buffer_front_loc.Distance(buffer_mid_lock)
+ buffer_mid_lock.Distance(buffer_back_loc), 2);
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);
@ -246,7 +239,7 @@ namespace LocalizationConstants {
collision_message.actor = vehicle;
collision_message.buffer = waypoint_buffer;
collision_message.overlapping_actors = track_traffic.GetOverlappingVehicles(actor_id);
}
}
LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i);
traffic_light_message.actor = vehicle;
@ -260,32 +253,8 @@ namespace LocalizationConstants {
collision_frame_ready = true;
}
//for (uint i = 0u; i < actor_list.size(); ++i) {
// CheckIdleTime(actor_list.at(i));
//}
}
/*void LocalizationStage::ResetIdleTime(Actor actor) {
idle_time[actor->GetId()] = chr::system_clock::now();
}
void LocalizationStage::CheckIdleTime(Actor actor) {
chr::duration<float> time_difference = chr::system_clock::now() - idle_time[actor->GetId()];
//if (time_difference.count() > 60.0f) { // 60 seconds
//std::cout << "The time difference is: " << time_difference.count() << "s for actor id: " << actor->GetId() << std::endl;
// TODO: Kill it and don't break stuff. Right now, it breaks stuff.
//}
}
void LocalizationStage::KillVehicle(Actor actor) {
// TODO: Fix this properly, either here or in another (and probably better) loop.
std::vector<boost::shared_ptr<carla::client::Actor>> to_remove;
to_remove.push_back(actor);
//registered_actors.Destroy(to_remove);
debug_helper.DrawString(actor->GetLocation() + cg::Location(0,0,2), "I should be dead.", false, {255u, 0u, 0u}, 0.1f);
}*/
void LocalizationStage::DataReceiver() {
// Building a list of registered actors and
@ -331,7 +300,7 @@ namespace LocalizationConstants {
// which takes the most priority (which needs the highest rate of data feed)
// to run the system well.
DataPacket<std::shared_ptr<LocalizationToPlannerFrame>> planner_data_packet = {
const DataPacket<std::shared_ptr<LocalizationToPlannerFrame>> planner_data_packet = {
planner_messenger_state,
planner_frame_selector ? planner_frame_a : planner_frame_b
};
@ -344,10 +313,10 @@ namespace LocalizationConstants {
if ((collision_messenger_current_state != collision_messenger_state) &&
collision_frame_ready) {
DataPacket<std::shared_ptr<LocalizationToCollisionFrame>> collision_data_packet = {
collision_messenger_state,
collision_frame_selector ? collision_frame_a : collision_frame_b
};
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;
@ -356,12 +325,12 @@ namespace LocalizationConstants {
// Send data to traffic light stage only if it has finished
// processing, received the previous message and started processing it.
int traffic_light_messenger_current_state = traffic_light_messenger->GetState();
const int traffic_light_messenger_current_state = traffic_light_messenger->GetState();
if (traffic_light_messenger_current_state != traffic_light_messenger_state) {
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
};
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;
@ -377,18 +346,18 @@ namespace LocalizationConstants {
void LocalizationStage::PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint) {
uint64_t waypoint_id = waypoint->GetId();
const uint64_t waypoint_id = waypoint->GetId();
buffer.push_back(waypoint);
track_traffic.UpdatePassingVehicle(waypoint_id, actor_id);
ActorIdSet current_actors = track_traffic.GetOverlappingVehicles(actor_id);
ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(waypoint_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())
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) {
@ -400,20 +369,20 @@ namespace LocalizationConstants {
void LocalizationStage::PopWaypoint(Buffer& buffer, ActorId actor_id) {
uint64_t removed_waypoint_id = buffer.front()->GetId();
const uint64_t removed_waypoint_id = buffer.front()->GetId();
buffer.pop_front();
track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id);
if (!buffer.empty()) {
ActorIdSet current_actors = track_traffic.GetPassingVehicles(removed_waypoint_id);
ActorIdSet new_overlapping_actors = track_traffic.GetPassingVehicles(buffer.front()->GetId());
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())
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) {
@ -423,7 +392,7 @@ namespace LocalizationConstants {
}
} else {
ActorIdSet currently_tracked_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
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);
@ -433,20 +402,20 @@ namespace LocalizationConstants {
SimpleWaypointPtr LocalizationStage::AssignLaneChange(Actor vehicle, bool force, bool direction) {
ActorId actor_id = vehicle->GetId();
cg::Location vehicle_location = vehicle->GetLocation();
float vehicle_velocity = vehicle->GetVelocity().Length();
const ActorId actor_id = vehicle->GetId();
const cg::Location vehicle_location = vehicle->GetLocation();
const float vehicle_velocity = vehicle->GetVelocity().Length();
Buffer& waypoint_buffer = buffer_list->at(vehicle_id_to_index.at(actor_id));
SimpleWaypointPtr& current_waypoint = waypoint_buffer.front();
const Buffer& waypoint_buffer = buffer_list->at(vehicle_id_to_index.at(actor_id));
const SimpleWaypointPtr& current_waypoint = waypoint_buffer.front();
bool need_to_change_lane = false;
auto left_waypoint = current_waypoint->GetLeftWaypoint();
auto right_waypoint = current_waypoint->GetRightWaypoint();
const auto left_waypoint = current_waypoint->GetLeftWaypoint();
const auto right_waypoint = current_waypoint->GetRightWaypoint();
if (!force) {
auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
bool abort_lane_change = false;
for (auto i = blocking_vehicles.begin();
@ -454,13 +423,14 @@ namespace LocalizationConstants {
++i) {
const ActorId &other_vehicle_id = *i;
Buffer& other_buffer = buffer_list->at(vehicle_id_to_index.at(other_vehicle_id));
SimpleWaypointPtr& other_current_waypoint = other_buffer.front();
cg::Location other_location = other_current_waypoint->GetLocation();
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;
auto other_neighbouring_lanes = {other_current_waypoint->GetLeftWaypoint(),
other_current_waypoint->GetRightWaypoint()};
const auto other_neighbouring_lanes = {
other_current_waypoint->GetLeftWaypoint(),
other_current_waypoint->GetRightWaypoint()};
for (auto& candidate_lane_wp: other_neighbouring_lanes) {
if (candidate_lane_wp != nullptr &&
@ -470,16 +440,16 @@ namespace LocalizationConstants {
}
}
cg::Vector3D reference_heading = current_waypoint->GetForwardVector();
cg::Vector3D other_heading = other_current_waypoint->GetForwardVector();
const cg::Vector3D reference_heading = current_waypoint->GetForwardVector();
const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector();
if (other_vehicle_id != actor_id &&
!current_waypoint->CheckJunction() &&
!other_current_waypoint->CheckJunction() &&
cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) {
float squared_vehicle_distance = cg::Math::DistanceSquared(other_location, vehicle_location);
float deviation_dot = DeviationDotProduct(vehicle, other_location);
const float squared_vehicle_distance = cg::Math::DistanceSquared(other_location, vehicle_location);
const float deviation_dot = DeviationDotProduct(vehicle, other_location);
if (deviation_dot > 0.0f) {
@ -502,7 +472,7 @@ namespace LocalizationConstants {
need_to_change_lane = true;
}
float change_over_distance = std::max(2.0f*vehicle_velocity, 10.0f);
const float change_over_distance = std::max(2.0f*vehicle_velocity, 10.0f);
bool possible_to_lane_change = false;
SimpleWaypointPtr change_over_point = nullptr;
@ -537,7 +507,7 @@ namespace LocalizationConstants {
}
if (need_to_change_lane && possible_to_lane_change) {
auto starting_point = change_over_point;
const auto starting_point = change_over_point;
while (change_over_point->DistanceSquared(starting_point) < std::pow(change_over_distance, 2) &&
!change_over_point->CheckJunction()) {
change_over_point = change_over_point->GetNextWaypoint()[0];
@ -548,4 +518,4 @@ namespace LocalizationConstants {
}
}
}
} // namespace traffic_manager

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

@ -1,13 +1,19 @@
// 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 <algorithm>
#include <chrono>
#include <cmath>
#include <ctime>
#include <deque>
#include <memory>
#include <mutex>
#include <unordered_map>
#include <ctime>
#include <chrono>
#include "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
@ -28,8 +34,8 @@
namespace traffic_manager {
using namespace std::chrono;
namespace cc = carla::client;
using namespace std::chrono;
namespace cc = carla::client;
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
using ActorIdSet = std::unordered_set<ActorId>;
@ -95,13 +101,8 @@ namespace cc = carla::client;
/// Map of all vehicles' idle time
std::unordered_map<ActorId, chr::time_point<chr::_V2::system_clock, chr::nanoseconds>> idle_time;
/// A simple method used to draw waypoint buffer ahead of a vehicle.
void DrawBuffer(Buffer &buffer);
/// Methods for idle vehicle elimination.
/* void ResetIdleTime(Actor actor);
void CheckIdleTime(Actor actor);
void KillVehicle(Actor actor);*/
/// Method to determine lane change and obtain target lane waypoint.
SimpleWaypointPtr AssignLaneChange(Actor vehicle, bool force, bool direction);
@ -112,14 +113,14 @@ namespace cc = carla::client;
public:
LocalizationStage(
std::string stage_name,
std::shared_ptr<LocalizationToPlannerMessenger> planner_messenger,
std::shared_ptr<LocalizationToCollisionMessenger> collision_messenger,
std::shared_ptr<LocalizationToTrafficLightMessenger> traffic_light_messenger,
AtomicActorSet &registered_actors,
InMemoryMap &local_map,
Parameters &parameters,
cc::DebugHelper &debug_helper);
std::string stage_name,
std::shared_ptr<LocalizationToPlannerMessenger> planner_messenger,
std::shared_ptr<LocalizationToCollisionMessenger> collision_messenger,
std::shared_ptr<LocalizationToTrafficLightMessenger> traffic_light_messenger,
AtomicActorSet &registered_actors,
InMemoryMap &local_map,
Parameters &parameters,
cc::DebugHelper &debug_helper);
~LocalizationStage();
@ -131,4 +132,4 @@ namespace cc = carla::client;
};
}
} // namespace traffic_manager

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

@ -1,82 +1,87 @@
// 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 "carla/trafficmanager/LocalizationUtils.h"
namespace traffic_manager {
float DeviationCrossProduct(Actor actor, const cg::Location &target_location) {
float DeviationCrossProduct(Actor actor, const cg::Location &target_location) {
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
heading_vector.z = 0;
heading_vector = heading_vector.MakeUnitVector();
cg::Location next_vector = target_location - actor->GetLocation();
next_vector.z = 0;
if (next_vector.Length() > 2.0f * std::numeric_limits<float>::epsilon()) {
next_vector = next_vector.MakeUnitVector();
float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x;
return cross_z;
} else {
return 0;
}
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
heading_vector.z = 0.0f;
heading_vector = heading_vector.MakeUnitVector();
cg::Location next_vector = target_location - actor->GetLocation();
next_vector.z = 0.0f;
if (next_vector.Length() > 2.0f * std::numeric_limits<float>::epsilon()) {
next_vector = next_vector.MakeUnitVector();
const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x;
return cross_z;
} else {
return 0.0f;
}
}
float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset) {
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
heading_vector.z = 0.0f;
heading_vector = heading_vector.MakeUnitVector();
cg::Location next_vector;
if (!rear_offset) {
next_vector = target_location - actor->GetLocation();
} else {
const auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(actor);
const float vehicle_half_length = vehicle_ptr->GetBoundingBox().extent.x;
next_vector = target_location - (cg::Location(-1* vehicle_half_length * heading_vector)
+ vehicle_ptr->GetLocation());
}
float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset) {
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
heading_vector.z = 0;
heading_vector = heading_vector.MakeUnitVector();
cg::Location next_vector;
if (!rear_offset) {
next_vector = target_location - actor->GetLocation();
} else {
auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(actor);
float vehicle_half_length = vehicle_ptr->GetBoundingBox().extent.x;
next_vector = target_location - (cg::Location(-1* vehicle_half_length * heading_vector)
+ vehicle_ptr->GetLocation());
}
next_vector.z = 0;
if (next_vector.Length() > 2.0f * std::numeric_limits<float>::epsilon()) {
next_vector = next_vector.MakeUnitVector();
float dot_product = cg::Math::Dot(next_vector, heading_vector);
return dot_product;
} else {
return 0;
}
next_vector.z = 0.0f;
if (next_vector.Length() > 2.0f * std::numeric_limits<float>::epsilon()) {
next_vector = next_vector.MakeUnitVector();
const float dot_product = cg::Math::Dot(next_vector, heading_vector);
return dot_product;
} else {
return 0.0f;
}
}
TrackTraffic::TrackTraffic() {}
TrackTraffic::TrackTraffic() {}
void TrackTraffic::UpdateOverlappingVehicle(ActorId actor_id, ActorId other_id) {
void TrackTraffic::UpdateOverlappingVehicle(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(other_id) == actor_set.end()) {
actor_set.insert(other_id);
}
} else {
overlapping_vehicles.insert({actor_id, {other_id}});
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);
}
} 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::RemoveOverlappingVehicle(ActorId actor_id, ActorId other_id) {
ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_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);
}
}
}
ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) {
if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) {
return overlapping_vehicles.at(actor_id);
} else {
return ActorIdSet();
}
if (overlapping_vehicles.find(actor_id) != overlapping_vehicles.end()) {
return overlapping_vehicles.at(actor_id);
} else {
return ActorIdSet();
}
}
void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) {
@ -118,4 +123,4 @@ namespace traffic_manager {
}
}
}
} // namespace traffic_manager

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

@ -1,3 +1,10 @@
// 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 "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
@ -8,46 +15,46 @@
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
using ActorIdSet = std::unordered_set<ActorId>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using Buffer = std::deque<SimpleWaypointPtr>;
namespace cc = carla::client;
namespace cg = carla::geom;
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
using ActorIdSet = std::unordered_set<ActorId>;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
using Buffer = std::deque<SimpleWaypointPtr>;
class TrackTraffic{
class TrackTraffic{
private:
/// 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;
private:
/// 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;
public:
TrackTraffic(/* args */);
public:
TrackTraffic();
/// Methods to update, remove and retrieve vehicles passing through a waypoint.
void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
ActorIdSet GetPassingVehicles(uint64_t waypoint_id);
/// Methods to update, remove and retrieve vehicles passing through a waypoint.
void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id);
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);
/// 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);
};
};
/// Returns the cross product (z component value) between the vehicle's
/// heading vector and the vector along the direction to the next
/// target waypoint on the horizon.
float DeviationCrossProduct(Actor actor, const cg::Location &target_location);
/// Returns the dot product between the vehicle's heading vector and
/// the vector along the direction to the next target waypoint on the horizon.
float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset=false);
/// Returns the cross product (z component value) between the vehicle's
/// heading vector and the vector along the direction to the next
/// target waypoint on the horizon.
float DeviationCrossProduct(Actor actor, const cg::Location &target_location);
/// Returns the dot product between the vehicle's heading vector and
/// the vector along the direction to the next target waypoint on the horizon.
float DeviationDotProduct(Actor actor, const cg::Location &target_location, bool rear_offset=false);
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <atomic>
@ -97,4 +103,4 @@ namespace traffic_manager {
};
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <memory>
@ -12,7 +18,7 @@
namespace traffic_manager {
namespace cc = carla::client;
namespace cc = carla::client;
/// Convenience typing.
@ -82,4 +88,5 @@ namespace cc = carla::client;
using LocalizationToTrafficLightMessenger = Messenger<std::shared_ptr<LocalizationToTrafficLightFrame>>;
using CollisionToPlannerMessenger = Messenger<std::shared_ptr<CollisionToPlannerFrame>>;
using TrafficLightToPlannerMessenger = Messenger<std::shared_ptr<TrafficLightToPlannerFrame>>;
}
} // namespace traffic_manager

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

@ -1,11 +1,19 @@
// 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 "MotionPlannerStage.h"
namespace traffic_manager {
namespace PlannerConstants {
static const float HIGHWAY_SPEED = 50 / 3.6f;
}
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
} // namespace PlannerConstants
using namespace PlannerConstants;
MotionPlannerStage::MotionPlannerStage(
@ -52,25 +60,25 @@ namespace PlannerConstants {
void MotionPlannerStage::Action() {
// Selecting an output frame.
auto current_control_frame = frame_selector ? control_frame_a : control_frame_b;
const auto current_control_frame = frame_selector ? control_frame_a : control_frame_b;
// Looping over all vehicles.
for (uint i = 0u; i < number_of_vehicles; ++i) {
LocalizationToPlannerData &localization_data = localization_frame->at(i);
Actor actor = localization_data.actor;
float current_deviation = localization_data.deviation;
float current_distance = localization_data.distance;
const LocalizationToPlannerData &localization_data = localization_frame->at(i);
const Actor actor = localization_data.actor;
const float current_deviation = localization_data.deviation;
const float current_distance = localization_data.distance;
ActorId actor_id = actor->GetId();
const ActorId actor_id = actor->GetId();
auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
float current_velocity = vehicle->GetVelocity().Length();
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const float current_velocity = vehicle->GetVelocity().Length();
auto current_time = chr::system_clock::now();
const auto current_time = chr::system_clock::now();
if (pid_state_map.find(actor_id) == pid_state_map.end()) {
auto initial_state = StateEntry{0.0f, 0.0f, 0.0f, chr::system_clock::now(), 0.0f, 0.0f, 0.0f};
const auto initial_state = StateEntry{0.0f, 0.0f, 0.0f, chr::system_clock::now(), 0.0f, 0.0f, 0.0f};
pid_state_map.insert({actor_id, initial_state});
}
@ -79,9 +87,9 @@ namespace PlannerConstants {
previous_state = pid_state_map.at(actor_id);
// Increase speed if on highway.
float speed_limit = vehicle->GetSpeedLimit() / 3.6f;
const float speed_limit = vehicle->GetSpeedLimit() / 3.6f;
float dynamic_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f;
const float dynamic_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f;
if (speed_limit > HIGHWAY_SPEED) {
longitudinal_parameters = highway_longitudinal_parameters;
@ -130,28 +138,27 @@ namespace PlannerConstants {
message.throttle = actuation_signal.throttle;
message.brake = actuation_signal.brake;
message.steer = actuation_signal.steer;
//DrawPIDValues(vehicle, actuation_signal.throttle, actuation_signal.brake);
}
}
void MotionPlannerStage::DataReceiver() {
auto localization_packet = localization_messenger->ReceiveData(localization_messenger_state);
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.
int collision_messenger_current_state = collision_messenger->GetState();
const int collision_messenger_current_state = collision_messenger->GetState();
if (collision_messenger_current_state != collision_messenger_state) {
auto collision_packet = collision_messenger->ReceiveData(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.
int traffic_light_messenger_current_state = traffic_light_messenger->GetState();
const int traffic_light_messenger_current_state = traffic_light_messenger->GetState();
if (traffic_light_messenger_current_state != traffic_light_messenger_state) {
auto traffic_light_packet = traffic_light_messenger->ReceiveData(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;
}
@ -170,15 +177,16 @@ namespace PlannerConstants {
void MotionPlannerStage::DataSender() {
DataPacket<std::shared_ptr<PlannerToControlFrame>> data_packet = {
control_messenger_state,
frame_selector ? control_frame_a : control_frame_b
};
control_messenger_state,
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,0,2), std::to_string(throttle), false, {0u, 255u, 0u}, 0.005f);
debug_helper.DrawString(vehicle->GetLocation() + cg::Location(0,0,4), std::to_string(brake), false, {255u, 0u, 0u}, 0.005f);
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

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

@ -1,3 +1,9 @@
// 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 <chrono>
@ -14,8 +20,8 @@
namespace traffic_manager {
namespace chr = std::chrono;
namespace cc = carla::client;
namespace chr = std::chrono;
namespace cc = carla::client;
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::rpc::ActorId;
@ -71,17 +77,17 @@ namespace cc = carla::client;
public:
MotionPlannerStage(
std::string stage_name,
std::shared_ptr<LocalizationToPlannerMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> collision_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> traffic_light_messenger,
std::shared_ptr<PlannerToControlMessenger> control_messenger,
Parameters &parameters,
std::vector<float> longitudinal_parameters,
std::vector<float> highway_longitudinal_parameters,
std::vector<float> lateral_parameters,
std::vector<float> highway_lateral_parameters,
cc::DebugHelper &debug_helper);
std::string stage_name,
std::shared_ptr<LocalizationToPlannerMessenger> localization_messenger,
std::shared_ptr<CollisionToPlannerMessenger> collision_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> traffic_light_messenger,
std::shared_ptr<PlannerToControlMessenger> control_messenger,
Parameters &parameters,
std::vector<float> longitudinal_parameters,
std::vector<float> highway_longitudinal_parameters,
std::vector<float> lateral_parameters,
std::vector<float> highway_lateral_parameters,
cc::DebugHelper &debug_helper);
~MotionPlannerStage();
@ -95,4 +101,4 @@ namespace cc = carla::client;
};
}
} // namespace traffic_manager

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

@ -1,11 +1,20 @@
// 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 "PIDController.h"
namespace traffic_manager {
namespace PIDControllerConstants {
const float MAX_THROTTLE = 0.7f;
const float MAX_BRAKE = 1.0f;
}
} // namespace PIDControllerConstants
using namespace PIDControllerConstants;
PIDController::PIDController() {}
@ -29,8 +38,8 @@ namespace PIDControllerConstants {
};
// Calculating dt for 'D' and 'I' controller components.
chr::duration<float> duration = current_state.time_instance - previous_state.time_instance;
float dt = duration.count();
const chr::duration<float> duration = current_state.time_instance - previous_state.time_instance;
const float dt = duration.count();
// Calculating integrals.
current_state.deviation_integral = angular_deviation * dt + previous_state.deviation_integral;
@ -47,11 +56,12 @@ namespace PIDControllerConstants {
const std::vector<float> &lateral_parameters) const {
// Calculating dt for updating the integral component.
chr::duration<float> duration = present_state.time_instance - previous_state.time_instance;
float dt = duration.count();
const chr::duration<float> duration = present_state.time_instance - previous_state.time_instance;
const float dt = duration.count();
// Longitudinal PID calculation.
float expr_v = longitudinal_parameters[0] * present_state.velocity +
const float expr_v =
longitudinal_parameters[0] * present_state.velocity +
longitudinal_parameters[1] * present_state.velocity_integral +
longitudinal_parameters[2] * (present_state.velocity -
previous_state.velocity) / dt;
@ -68,24 +78,15 @@ namespace PIDControllerConstants {
}
// Lateral PID calculation.
float steer = lateral_parameters[0] * present_state.deviation +
float steer =
lateral_parameters[0] * present_state.deviation +
lateral_parameters[1] * present_state.deviation_integral +
lateral_parameters[2] * (present_state.deviation -
previous_state.deviation) / dt;
/*float steer = lateral_parameters[0] * present_state.distance +
lateral_parameters[1] * present_state.distance_integral +
lateral_parameters[2] * (present_state.distance -
previous_state.distance) / dt;
std::cout << "--------------------------------------\n";
std::cout << steer << "\n";
std::cout << lateral_parameters[0] * present_state.distance << "\n";
std::cout << lateral_parameters[1] * present_state.distance_integral << "\n";
std::cout << lateral_parameters[2] * (present_state.distance - previous_state.distance) / dt << "\n";
*/
steer = std::max(-1.0f, std::min(steer, 1.0f));
return ActuationSignal{throttle, brake, steer};
}
}
} // namespace traffic_manager

10
LibCarla/source/carla/trafficmanager/PIDController.h
View File

@ -1,3 +1,9 @@
// 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 <chrono>
@ -6,7 +12,7 @@
namespace traffic_manager {
namespace chr = std::chrono;
namespace chr = std::chrono;
using TimeInstance = chr::time_point<chr::_V2::system_clock, chr::nanoseconds>;
/// Structure to hold the actuation signals.
@ -57,4 +63,4 @@ namespace chr = std::chrono;
};
}
} // namespace traffic_manager

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

@ -1,174 +1,181 @@
#include "Parameters.h"
// 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 <random>
#include "Parameters.h"
namespace traffic_manager {
Parameters::Parameters() {}
Parameters::Parameters() {}
Parameters::~Parameters() {}
Parameters::~Parameters() {}
void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
percentage_difference_from_speed_limit.AddEntry({actor->GetId(), percentage});
void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
percentage_difference_from_speed_limit.AddEntry({actor->GetId(), percentage});
}
void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) {
global_percentage_difference_from_limit = percentage;
}
void Parameters::SetCollisionDetection(
const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision) {
const ActorId reference_id = reference_actor->GetId();
const ActorId other_id = other_actor->GetId();
if (detect_collision) {
if (ignore_collision.Contains(reference_id)) {
std::shared_ptr<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
if (actor_set->Contains(other_id)) {
actor_set->Remove({other_actor});
}
}
} else {
void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) {
global_percentage_difference_from_limit = percentage;
if (ignore_collision.Contains(reference_id)) {
std::shared_ptr<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
if (!actor_set->Contains(other_id)) {
actor_set->Insert({other_actor});
}
} else {
std::shared_ptr<AtomicActorSet> actor_set = std::make_shared<AtomicActorSet>();
actor_set->Insert({other_actor});
auto entry = std::make_pair(reference_id, actor_set);
ignore_collision.AddEntry(entry);
}
}
}
void Parameters::SetCollisionDetection(
const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision) {
void Parameters::SetForceLaneChange(const ActorPtr &actor, const bool direction) {
ActorId reference_id = reference_actor->GetId();
ActorId other_id = other_actor->GetId();
const ChangeLaneInfo lane_change_info = {true, direction};
const auto entry = std::make_pair(actor->GetId(), lane_change_info);
force_lane_change.AddEntry(entry);
}
if (detect_collision) {
void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
if (ignore_collision.Contains(reference_id)) {
std::shared_ptr<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
if (actor_set->Contains(other_id)) {
actor_set->Remove({other_actor});
}
}
} else {
const auto entry = std::make_pair(actor->GetId(), enable);
auto_lane_change.AddEntry(entry);
}
if (ignore_collision.Contains(reference_id)) {
std::shared_ptr<AtomicActorSet> actor_set = ignore_collision.GetValue(reference_id);
if (!actor_set->Contains(other_id)) {
actor_set->Insert({other_actor});
}
} else {
std::shared_ptr<AtomicActorSet> actor_set = std::make_shared<AtomicActorSet>();
actor_set->Insert({other_actor});
auto entry = std::make_pair(reference_id, actor_set);
ignore_collision.AddEntry(entry);
}
}
}
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);
}
}
void Parameters::SetForceLaneChange(const ActorPtr &actor, const bool direction) {
float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) {
ChangeLaneInfo lane_change_info = {true, direction};
auto entry = std::make_pair(actor->GetId(), lane_change_info);
force_lane_change.AddEntry(entry);
}
const ActorId actor_id = actor->GetId();
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const float speed_limit = vehicle->GetSpeedLimit();
float percentage_difference = global_percentage_difference_from_limit;
void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
if (percentage_difference_from_speed_limit.Contains(actor_id)) {
percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id);
}
auto entry = std::make_pair(actor->GetId(), enable);
auto_lane_change.AddEntry(entry);
}
return speed_limit * (1.0f - percentage_difference/100.0f);
}
void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
if (distance > 0.0f) {
auto entry = std::make_pair(actor->GetId(), distance);
distance_to_leading_vehicle.AddEntry(entry);
}
}
bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor) {
float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) {
const ActorId reference_actor_id = reference_actor->GetId();
const ActorId other_actor_id = other_actor->GetId();
bool avoid_collision = true;
ActorId actor_id = actor->GetId();
auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
float speed_limit = vehicle->GetSpeedLimit();
float percentage_difference = global_percentage_difference_from_limit;
if (ignore_collision.Contains(reference_actor_id) &&
ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) {
avoid_collision = false;
}
if (percentage_difference_from_speed_limit.Contains(actor_id)) {
percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id);
}
return avoid_collision;
}
return speed_limit * (1.0f - percentage_difference/100.0f);
}
ChangeLaneInfo Parameters::GetForceLaneChange(const ActorPtr &actor) {
bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor) {
const ActorId actor_id = actor->GetId();
ChangeLaneInfo change_lane_info;
ActorId reference_actor_id = reference_actor->GetId();
ActorId other_actor_id = other_actor->GetId();
bool avoid_collision = true;
if (force_lane_change.Contains(actor_id)) {
change_lane_info = force_lane_change.GetValue(actor_id);
}
if (ignore_collision.Contains(reference_actor_id) &&
ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) {
avoid_collision = false;
}
force_lane_change.RemoveEntry(actor_id);
return avoid_collision;
}
return change_lane_info;
}
ChangeLaneInfo Parameters::GetForceLaneChange(const ActorPtr &actor) {
bool Parameters::GetAutoLaneChange(const ActorPtr &actor) {
ActorId actor_id = actor->GetId();
ChangeLaneInfo change_lane_info;
const ActorId actor_id = actor->GetId();
bool auto_lane_change_policy = true;
if (force_lane_change.Contains(actor_id)) {
change_lane_info = force_lane_change.GetValue(actor_id);
}
if (auto_lane_change.Contains(actor_id)) {
auto_lane_change_policy = auto_lane_change.GetValue(actor_id);
}
force_lane_change.RemoveEntry(actor_id);
return auto_lane_change_policy;
}
return change_lane_info;
}
float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) {
bool Parameters::GetAutoLaneChange(const ActorPtr &actor) {
const ActorId actor_id = actor->GetId();
float distance_margin = -1.0f;
ActorId actor_id = actor->GetId();
bool auto_lane_change_policy = true;
if (distance_to_leading_vehicle.Contains(actor_id)) {
distance_margin = distance_to_leading_vehicle.GetValue(actor_id);
}
if (auto_lane_change.Contains(actor_id)) {
auto_lane_change_policy = auto_lane_change.GetValue(actor_id);
}
return distance_margin;
}
return auto_lane_change_policy;
}
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 Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) {
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);
}
}
ActorId actor_id = actor->GetId();
float distance_margin = -1;
float Parameters::GetPercentageRunningLight(const ActorPtr &actor) {
if (distance_to_leading_vehicle.Contains(actor_id)) {
distance_margin = distance_to_leading_vehicle.GetValue(actor_id);
}
const ActorId actor_id = actor->GetId();
float percentage = 0.0f;
return distance_margin;
}
if (perc_run_traffic_light.Contains(actor_id)) {
percentage = perc_run_traffic_light.GetValue(actor_id);
}
void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) {
if (perc > 0.0f) {
auto entry = std::make_pair(actor->GetId(), perc);
perc_run_traffic_light.AddEntry(entry);
}
}
return percentage;
}
void Parameters::SetPercentageIgnoreActors(const ActorPtr &actor, const float perc) {
if (perc > 0.0f) {
auto entry = std::make_pair(actor->GetId(), perc);
perc_ignore_actors.AddEntry(entry);
}
}
float Parameters::GetPercentageIgnoreActors(const ActorPtr &actor) {
float Parameters::GetPercentageRunningLight(const ActorPtr &actor) {
const ActorId actor_id = actor->GetId();
float percentage = 0.0f;
ActorId actor_id = actor->GetId();
float percentage = 0.0f;
if (perc_ignore_actors.Contains(actor_id)) {
percentage = perc_ignore_actors.GetValue(actor_id);
}
if (perc_run_traffic_light.Contains(actor_id)) {
percentage = perc_run_traffic_light.GetValue(actor_id);
}
return percentage;
}
return percentage;
}
float Parameters::GetPercentageIgnoreActors(const ActorPtr &actor) {
ActorId actor_id = actor->GetId();
float percentage = 0.0f;
if (perc_ignore_actors.Contains(actor_id)) {
percentage = perc_ignore_actors.GetValue(actor_id);
}
return percentage;
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 "carla/client/Actor.h"
@ -10,89 +16,91 @@
namespace traffic_manager {
namespace cc = carla::client;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
namespace cc = carla::client;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
struct ChangeLaneInfo {
bool change_lane = false;
bool direction = false;
};
struct ChangeLaneInfo {
bool change_lane = false;
bool direction = false;
};
class Parameters {
class Parameters {
private:
private:
/// Target velocity map for individual vehicles.
AtomicMap<ActorId, float> percentage_difference_from_speed_limit;
/// Global target velocity limit % difference.
float global_percentage_difference_from_limit = 0;
/// Map containing a set of actors to be ignored during collision detection.
AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> ignore_collision;
/// Map containing distance to leading vehicle command.
AtomicMap<ActorId, float> distance_to_leading_vehicle;
/// Map containing force lane change commands.
AtomicMap<ActorId, ChangeLaneInfo> force_lane_change;
/// Map containing auto lane change commands.
AtomicMap<ActorId, bool> auto_lane_change;
/// Map containing % of running a traffic light.
AtomicMap<ActorId, float> perc_run_traffic_light;
/// Map containing % of ignoring actors.
AtomicMap<ActorId, float> perc_ignore_actors;
/// Target velocity map for individual vehicles.
AtomicMap<ActorId, float> percentage_difference_from_speed_limit;
/// Global target velocity limit % difference.
float global_percentage_difference_from_limit = 0;
/// Map containing a set of actors to be ignored during collision detection.
AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> ignore_collision;
/// Map containing distance to leading vehicle command.
AtomicMap<ActorId, float> distance_to_leading_vehicle;
/// Map containing force lane change commands.
AtomicMap<ActorId, ChangeLaneInfo> force_lane_change;
/// Map containing auto lane change commands.
AtomicMap<ActorId, bool> auto_lane_change;
/// Map containing % of running a traffic light.
AtomicMap<ActorId, float> perc_run_traffic_light;
/// Map containing % of ignoring actors.
AtomicMap<ActorId, float> perc_ignore_actors;
public:
Parameters();
~Parameters();
public:
Parameters();
~Parameters();
/// Set target velocity specific to a vehicle.
void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage);
/// Set target velocity specific to a vehicle.
void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage);
/// Set global target velocity.
void SetGlobalPercentageSpeedDifference(float const percentage);
/// Set global target velocity.
void SetGlobalPercentageSpeedDifference(float const percentage);
/// Set collision detection rules between vehicles.
void SetCollisionDetection(const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision);
/// Set collision detection rules between vehicles.
void SetCollisionDetection(
const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision);
/// Method to force lane change on a vehicle.
/// Direction flag can be set to true for left and false for right.
void SetForceLaneChange(const ActorPtr &actor, const bool direction);
/// Method to force lane change on a vehicle.
/// Direction flag can be set to true for left and false for right.
void SetForceLaneChange(const ActorPtr &actor, const bool direction);
/// Enable / disable automatic lane change on a vehicle.
void SetAutoLaneChange(const ActorPtr &actor, const bool enable);
/// Enable / disable automatic lane change on a vehicle.
void SetAutoLaneChange(const ActorPtr &actor, const bool enable);
/// Method to specify how much distance a vehicle should maintain to
/// the leading vehicle.
void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance);
/// Method to specify how much distance a vehicle should maintain to
/// the leading vehicle.
void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance);
/// Method to query target velocity for a vehicle.
float GetVehicleTargetVelocity(const ActorPtr &actor);
/// Method to query target velocity for a vehicle.
float GetVehicleTargetVelocity(const ActorPtr &actor);
/// Method to query collision avoidance rule between a pair of vehicles.
bool GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor);
/// Method to query collision avoidance rule between a pair of vehicles.
bool GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor);
/// Method to query lane change command for a vehicle.
ChangeLaneInfo GetForceLaneChange(const ActorPtr &actor);
/// Method to query lane change command for a vehicle.
ChangeLaneInfo GetForceLaneChange(const ActorPtr &actor);
/// Method to query auto lane change rule for a vehicle.
bool GetAutoLaneChange(const ActorPtr &actor);
/// Method to query auto lane change rule for a vehicle.
bool GetAutoLaneChange(const ActorPtr &actor);
/// Method to query distance to leading vehicle for a given vehicle.
float GetDistanceToLeadingVehicle(const ActorPtr &actor);
/// Method to query distance to leading vehicle for a given vehicle.
float GetDistanceToLeadingVehicle(const ActorPtr &actor);
/// Method to set % to run any traffic light.
void SetPercentageRunningLight(const ActorPtr &actor, const float perc);
/// Method to set % to run any traffic light.
void SetPercentageRunningLight(const ActorPtr &actor, const float perc);
/// Method to set % to ignore any actor.
void SetPercentageIgnoreActors(const ActorPtr &actor, const float perc);
/// Method to set % to ignore any actor.
void SetPercentageIgnoreActors(const ActorPtr &actor, const float perc);
/// Method to get % to run any traffic light.
float GetPercentageRunningLight(const ActorPtr &actor);
/// Method to get % to run any traffic light.
float GetPercentageRunningLight(const ActorPtr &actor);
/// Method to get % to ignore any actor.
float GetPercentageIgnoreActors(const ActorPtr &actor);
/// Method to get % to ignore any actor.
float GetPercentageIgnoreActors(const ActorPtr &actor);
};
}
};
} // namespace traffic_manager

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

@ -1,41 +1,39 @@
// 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 "carla/trafficmanager/PerformanceDiagnostics.h"
namespace traffic_manager {
PerformanceDiagnostics::PerformanceDiagnostics(std::string stage_name)
: stage_name(stage_name) {
PerformanceDiagnostics::PerformanceDiagnostics(std::string stage_name)
: stage_name(stage_name) {
throughput_clock = chr::system_clock::now();
throughput_counter = 0;
inter_update_clock = chr::system_clock::now();
inter_update_duration = chr::duration<float>(0);
throughput_clock = chr::system_clock::now();
throughput_counter = 0u;
inter_update_clock = chr::system_clock::now();
inter_update_duration = chr::duration<float>(0);
}
void PerformanceDiagnostics::RegisterUpdate() {
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) {
throughput_clock = current_time;
throughput_counter = 0u;
} 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;
}
}
void PerformanceDiagnostics::RegisterUpdate() {
auto current_time = chr::system_clock::now();
chr::duration<float> throughput_count_duration = current_time - throughput_clock;
if (throughput_count_duration.count() > 1.0f) {
/* std::cout << (
"Stage name : " + stage_name
+ ", throughput : "
+ std::to_string(throughput_counter) + " fps"
+ ", average update duration : "
+ std::to_string(inter_update_duration.count() * 1000.0f) + " ms"
+ "\n"
); */
throughput_clock = current_time;
throughput_counter = 0;
} else {
++throughput_counter;
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;
}
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <chrono>
@ -7,25 +13,26 @@
namespace traffic_manager {
namespace chr = std::chrono;
namespace chr = std::chrono;
class PerformanceDiagnostics {
class PerformanceDiagnostics {
private:
/// Stage name.
std::string stage_name;
/// Throughput clock.
chr::time_point<chr::_V2::system_clock, chr::nanoseconds> throughput_clock;
/// Throughput counter.
uint throughput_counter;
/// Inter-update clock.
chr::time_point<chr::_V2::system_clock, chr::nanoseconds> inter_update_clock;
/// Inter-update duration.
chr::duration<float> inter_update_duration;
private:
/// Stage name.
std::string stage_name;
/// Throughput clock.
chr::time_point<chr::_V2::system_clock, chr::nanoseconds> throughput_clock;
/// Throughput counter.
uint throughput_counter;
/// Inter-update clock.
chr::time_point<chr::_V2::system_clock, chr::nanoseconds> inter_update_clock;
/// Inter-update duration.
chr::duration<float> inter_update_duration;
public:
PerformanceDiagnostics(std::string name);
public:
PerformanceDiagnostics(std::string name);
void RegisterUpdate();
};
}
void RegisterUpdate();
};
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 "PipelineStage.h"
namespace traffic_manager {
@ -99,4 +105,4 @@ namespace traffic_manager {
}
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <atomic>
@ -84,4 +90,4 @@ namespace traffic_manager {
};
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 "SimpleWaypoint.h"
namespace traffic_manager {
@ -48,9 +54,9 @@ namespace traffic_manager {
void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr _waypoint) {
cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0) {
const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f) {
next_left_waypoint = _waypoint;
} else {
throw std::invalid_argument("Argument not on the left side!");
@ -59,9 +65,9 @@ namespace traffic_manager {
void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr _waypoint) {
cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0) {
const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0.0f) {
next_right_waypoint = _waypoint;
} else {
throw std::invalid_argument("Argument not on the right side!");
@ -92,4 +98,4 @@ namespace traffic_manager {
return (next_waypoints.size() > 1);
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <stdexcept>
@ -11,8 +17,8 @@
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
namespace cc = carla::client;
namespace cg = carla::geom;
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
/// This is a simple wrapper class on Carla's waypoint object.
@ -88,4 +94,4 @@ namespace cg = carla::geom;
};
}
} // namespace traffic_manager

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

@ -1,5 +1,12 @@
// 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 <iostream>
#include "TrafficLightStage.h"
#include "iostream"
namespace traffic_manager {
@ -36,31 +43,29 @@ namespace traffic_manager {
void TrafficLightStage::Action() {
// Selecting the output frame based on the selection key.
auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;
const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;
// Looping over registered actors.
for (uint i = 0u; i < number_of_vehicles; ++i) {
bool traffic_light_hazard = false;
LocalizationToTrafficLightData &data = localization_frame->at(i);
Actor ego_actor = data.actor;
ActorId ego_actor_id = ego_actor->GetId();
SimpleWaypointPtr closest_waypoint = data.closest_waypoint;
SimpleWaypointPtr look_ahead_point = data.junction_look_ahead_waypoint;
const LocalizationToTrafficLightData &data = localization_frame->at(i);
const Actor ego_actor = data.actor;
const ActorId ego_actor_id = ego_actor->GetId();
const SimpleWaypointPtr closest_waypoint = data.closest_waypoint;
const SimpleWaypointPtr look_ahead_point = data.junction_look_ahead_waypoint;
JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId();
TimeInstance current_time = chr::system_clock::now();
const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId();
const TimeInstance current_time = chr::system_clock::now();
auto ego_vehicle = boost::static_pointer_cast<cc::Vehicle>(ego_actor);
const auto ego_vehicle = boost::static_pointer_cast<cc::Vehicle>(ego_actor);
TLS traffic_light_state = ego_vehicle->GetTrafficLightState();
// Generate number between 0 and 100
int r = rand() % 101;
const int r = rand() % 101;
// Set to green if random number is lower than percentage, default is 0
if (parameters.GetPercentageRunningLight(boost::shared_ptr<cc::Actor>(ego_actor)) > r)
traffic_light_state = TLS::Green;
//DrawLight(traffic_light_state, ego_actor);
// 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
@ -96,8 +101,8 @@ namespace traffic_manager {
need_to_issue_new_ticket = true;
} else {
TimeInstance &previous_ticket = vehicle_last_ticket.at(ego_actor_id);
chr::duration<double> diff = current_time - previous_ticket;
const TimeInstance &previous_ticket = vehicle_last_ticket.at(ego_actor_id);
const chr::duration<double> diff = current_time - previous_ticket;
if (diff.count() > NO_SIGNAL_PASSTHROUGH_INTERVAL) {
need_to_issue_new_ticket = true;
}
@ -110,8 +115,8 @@ namespace traffic_manager {
if (junction_last_ticket.find(junction_id) != junction_last_ticket.end()) {
TimeInstance &last_ticket = junction_last_ticket.at(junction_id);
chr::duration<double> diff = current_time - last_ticket;
if (diff.count() > 0) {
const chr::duration<double> diff = current_time - last_ticket;
if (diff.count() > 0.0) {
last_ticket = current_time + chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL);
} else {
last_ticket += chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL);
@ -131,9 +136,9 @@ namespace traffic_manager {
}
// If current time is behind ticket time, then do not enter junction.
TimeInstance &current_ticket = vehicle_last_ticket.at(ego_actor_id);
chr::duration<double> diff = current_ticket - current_time;
if (diff.count() > 0) {
const TimeInstance &current_ticket = vehicle_last_ticket.at(ego_actor_id);
const chr::duration<double> diff = current_ticket - current_time;
if (diff.count() > 0.0) {
traffic_light_hazard = true;
}
}
@ -145,7 +150,7 @@ namespace traffic_manager {
}
void TrafficLightStage::DataReceiver() {
auto packet = localization_messenger->ReceiveData(localization_messenger_state);
const auto packet = localization_messenger->ReceiveData(localization_messenger_state);
localization_frame = packet.data;
localization_messenger_state = packet.id;
@ -162,10 +167,10 @@ namespace traffic_manager {
void TrafficLightStage::DataSender() {
DataPacket<std::shared_ptr<TrafficLightToPlannerFrame>> packet{
planner_messenger_state,
frame_selector ? planner_frame_a : planner_frame_b
};
const DataPacket<std::shared_ptr<TrafficLightToPlannerFrame>> packet{
planner_messenger_state,
frame_selector ? planner_frame_a : planner_frame_b
};
frame_selector = !frame_selector;
planner_messenger_state = planner_messenger->SendData(packet);
}
@ -175,28 +180,29 @@ namespace traffic_manager {
if (traffic_light_state == TLS::Green) {
str="Green";
debug_helper.DrawString(
cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1),
str,
false,
{0u, 255u, 0u}, 0.1f, true);
cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f),
str,
false,
{0u, 255u, 0u}, 0.1f, true);
}
else if (traffic_light_state == TLS::Yellow) {
str="Yellow";
debug_helper.DrawString(
cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1),
str,
false,
{255u, 255u, 0u}, 0.1f, true);
cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f),
str,
false,
{255u, 255u, 0u}, 0.1f, true);
}
else {
str="Red";
debug_helper.DrawString(
cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1),
str,
false,
{255u, 0u, 0u}, 0.1f, true);
cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1.0f),
str,
false,
{255u, 0u, 0u}, 0.1f, true);
}
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <chrono>
@ -5,23 +11,23 @@
#include <mutex>
#include <unordered_map>
#include "carla/client/Vehicle.h"
#include "carla/client/TrafficLight.h"
#include "carla/client/World.h"
#include "carla/client/ActorList.h"
#include "carla/client/TrafficLight.h"
#include "carla/client/Vehicle.h"
#include "carla/client/World.h"
#include "carla/Memory.h"
#include "carla/rpc/TrafficLightState.h"
#include "carla/trafficmanager/MessengerAndDataTypes.h"
#include "carla/trafficmanager/PipelineStage.h"
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PipelineStage.h"
namespace traffic_manager {
namespace chr = std::chrono;
namespace cc = carla::client;
namespace cg = carla::geom;
namespace chr = std::chrono;
namespace cc = carla::client;
namespace cg = carla::geom;
using ActorId = carla::ActorId;
using Actor = carla::SharedPtr<cc::Actor>;
@ -71,11 +77,11 @@ namespace cg = carla::geom;
public:
TrafficLightStage(
std::string stage_name,
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger,
Parameters &parameters,
cc::DebugHelper &debug_helper);
std::string stage_name,
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger,
Parameters &parameters,
cc::DebugHelper &debug_helper);
~TrafficLightStage();
void DataReceiver() override;
@ -86,4 +92,4 @@ namespace cg = carla::geom;
};
}
} // namespace traffic_manager

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

@ -1,4 +1,11 @@
// 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 "TrafficManager.h"
#include "carla/client/TrafficLight.h"
namespace traffic_manager {
@ -19,10 +26,10 @@ namespace traffic_manager {
debug_helper(client_connection.GetWorld().MakeDebugHelper()) {
using WorldMap = carla::SharedPtr<cc::Map>;
WorldMap world_map = world.GetMap();
auto dao = CarlaDataAccessLayer(world_map);
const WorldMap world_map = world.GetMap();
const auto dao = CarlaDataAccessLayer(world_map);
using Topology = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
Topology topology = dao.GetTopology();
const Topology topology = dao.GetTopology();
local_map = std::make_shared<traffic_manager::InMemoryMap>(topology);
local_map->SetUp(0.1f);
@ -36,38 +43,38 @@ namespace traffic_manager {
planner_control_messenger = std::make_shared<PlannerToControlMessenger>();
localization_stage = std::make_unique<LocalizationStage>(
"Localization stage",
localization_planner_messenger, localization_collision_messenger,
localization_traffic_light_messenger,
registered_actors, *local_map.get(),
parameters, debug_helper);
"Localization stage",
localization_planner_messenger, localization_collision_messenger,
localization_traffic_light_messenger,
registered_actors, *local_map.get(),
parameters, debug_helper);
collision_stage = std::make_unique<CollisionStage>(
"Collision stage",
localization_collision_messenger, collision_planner_messenger,
world, parameters, debug_helper);
"Collision stage",
localization_collision_messenger, collision_planner_messenger,
world, parameters, debug_helper);
traffic_light_stage = std::make_unique<TrafficLightStage>(
"Traffic light stage",
localization_traffic_light_messenger, traffic_light_planner_messenger,
parameters, debug_helper);
"Traffic light stage",
localization_traffic_light_messenger, traffic_light_planner_messenger,
parameters, debug_helper);
planner_stage = std::make_unique<MotionPlannerStage>(
"Motion planner stage",
localization_planner_messenger,
collision_planner_messenger,
traffic_light_planner_messenger,
planner_control_messenger,
parameters,
longitudinal_PID_parameters,
longitudinal_highway_PID_parameters,
lateral_PID_parameters,
lateral_highway_PID_parameters,
debug_helper);
"Motion planner stage",
localization_planner_messenger,
collision_planner_messenger,
traffic_light_planner_messenger,
planner_control_messenger,
parameters,
longitudinal_PID_parameters,
longitudinal_highway_PID_parameters,
lateral_PID_parameters,
lateral_highway_PID_parameters,
debug_helper);
control_stage = std::make_unique<BatchControlStage>(
"Batch control stage",
planner_control_messenger, client_connection);
"Batch control stage",
planner_control_messenger, client_connection);
Start();
}
@ -79,15 +86,14 @@ namespace traffic_manager {
std::unique_ptr<TrafficManager> TrafficManager::singleton_pointer = nullptr;
TrafficManager& TrafficManager::GetInstance(cc::Client &client_connection) {
// std::lock_guard<std::mutex> lock(singleton_mutex);
if (singleton_pointer == nullptr) {
std::vector<float> longitudinal_param = {2.0f, 0.15f, 0.01f};
std::vector<float> longitudinal_highway_param = {4.0f, 0.15f, 0.01f};
std::vector<float> lateral_param = {10.0f, 0.0f, 0.1f};
std::vector<float> lateral_highway_param = {6.0f, 0.0f, 0.3f};
float perc_difference_from_limit = 30.0f;
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 float perc_difference_from_limit = 30.0f;
TrafficManager* tm_ptr = new TrafficManager(
longitudinal_param, longitudinal_highway_param, lateral_param, lateral_highway_param,
@ -157,11 +163,11 @@ namespace traffic_manager {
}
void TrafficManager::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
parameters.SetPercentageSpeedDifference(actor, percentage);
parameters.SetPercentageSpeedDifference(actor, percentage);
}
void TrafficManager::SetGlobalPercentageSpeedDifference(const float percentage) {
parameters.SetGlobalPercentageSpeedDifference(percentage);
parameters.SetGlobalPercentageSpeedDifference(percentage);
}
void TrafficManager::SetCollisionDetection(
@ -208,35 +214,37 @@ namespace traffic_manager {
}
void TrafficManager::ResetAllTrafficLights() {
auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*");
const auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*");
std::vector<TLGroup> list_of_all_groups;
TLGroup tl_to_freeze;
std::vector<carla::ActorId> list_of_ids;
for (auto tl : *world_traffic_lights.get()) {
if (!(std::find(list_of_ids.begin(), list_of_ids.end(), tl->GetId()) != list_of_ids.end())) {
TLGroup tl_group = boost::static_pointer_cast<cc::TrafficLight>(tl)->GetGroupTrafficLights();
list_of_all_groups.push_back(tl_group);
for (unsigned i=0; i<tl_group.size(); i++) {
list_of_ids.push_back(tl_group.at(i).get()->GetId());
if(i!=0) {
tl_to_freeze.push_back(tl_group.at(i));
}
}
const TLGroup tl_group = boost::static_pointer_cast<cc::TrafficLight>(tl)->GetGroupTrafficLights();
list_of_all_groups.push_back(tl_group);
for (uint i=0u; i<tl_group.size(); i++) {
list_of_ids.push_back(tl_group.at(i).get()->GetId());
if(i!=0u) {
tl_to_freeze.push_back(tl_group.at(i));
}
}
}
}
for (TLGroup& tl_group : list_of_all_groups) {
tl_group.front()->SetState(TLS::Green);
std::for_each(tl_group.begin()+1, tl_group.end(),
[] (auto& tl) {tl->SetState(TLS::Red);});
std::for_each(
tl_group.begin()+1, tl_group.end(),
[] (auto& tl) {tl->SetState(TLS::Red);});
}
while (!CheckAllFrozen(tl_to_freeze)) {
for (auto& tln : tl_to_freeze) {
tln->SetState(TLS::Red);
tln->Freeze(true);
tln->SetState(TLS::Red);
tln->Freeze(true);
}
}
}
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <algorithm>
@ -27,7 +33,7 @@
namespace traffic_manager {
namespace cc = carla::client;
namespace cc = carla::client;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using TLS = carla::rpc::TrafficLightState;
@ -113,9 +119,10 @@ namespace cc = carla::client;
void SetGlobalPercentageSpeedDifference(float const percentage);
/// Set collision detection rules between vehicles.
void SetCollisionDetection(const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision);
void SetCollisionDetection(
const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision);
/// Method to force lane change on a vehicle.
/// Direction flag can be set to true for left and false for right.
@ -145,4 +152,4 @@ namespace cc = carla::client;
};
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 traffic_manager {
@ -14,17 +20,17 @@ namespace traffic_manager {
std::pair<int, int> VicinityGrid::UpdateGrid(Actor actor) {
ActorId actor_id = actor->GetId();
cg::Location location = actor->GetLocation();
int first = static_cast<int>(std::floor(location.x / GRID_SIZE));
int second = static_cast<int>(std::floor(location.y / GRID_SIZE));
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));
std::string new_grid_id = MakeKey({first, second});
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()) {
std::string old_grid_id = actor_to_grid_id.at(actor_id);
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) {
@ -60,7 +66,7 @@ namespace traffic_manager {
std::unordered_set<carla::ActorId> VicinityGrid::GetActors(Actor actor) {
std::pair<int, int> grid_ids = UpdateGrid(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;
@ -69,9 +75,9 @@ namespace traffic_manager {
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
std::string grid_key = MakeKey({grid_ids.first + i, grid_ids.second + 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()) {
auto &grid_actor_set = grid_to_actor_id.at(grid_key);
const auto &grid_actor_set = grid_to_actor_id.at(grid_key);
actors.insert(grid_actor_set.begin(), grid_actor_set.end());
}
}
@ -81,9 +87,9 @@ namespace traffic_manager {
}
void VicinityGrid::EraseActor(ActorId actor_id) {
std::string grid_key = actor_to_grid_id.at(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

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

@ -1,3 +1,9 @@
// 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>
@ -56,4 +62,4 @@ namespace traffic_manager {
};
}
} // namespace traffic_manager

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

@ -1,3 +1,9 @@
// 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 <chrono>
#include <memory>

2
PythonAPI/carla/source/libcarla/libcarla.cpp
View File

@ -1,4 +1,4 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// 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.