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

Implemented TrafficManager::SetDistanceToLeadingVehicle

This commit is contained in:
Praveen Kumar 2019-10-23 16:51:17 +05:30 committed by bernat
parent 63523a38aa
commit 901745599e
6 changed files with 60 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,10 +6,10 @@ namespace CollisionStageConstants {
static const float SEARCH_RADIUS = 20.0f;
static const float VERTICAL_OVERLAP_THRESHOLD = 2.0f;
static const float ZERO_AREA = 0.0001f;
static const float BOUNDARY_EXTENSION_MINIMUM = 1.5f;
static const float BOUNDARY_EXTENSION_MINIMUM = 0.5f;
static const float EXTENSION_SQUARE_POINT = 7.0f;
static const float TIME_HORIZON = 0.5f;
static const float HIGHWAY_SPEED = 50 / 3.6f;
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
static const float HIGHWAY_TIME_HORIZON = 5.0f;
}
using namespace CollisionStageConstants;
@ -19,11 +19,13 @@ namespace CollisionStageConstants {
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
cc::World &world,
AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> &selective_collision,
AtomicMap<ActorId, float> &distance_to_leading_vehicle,
cc::DebugHelper &debug_helper)
: localization_messenger(localization_messenger),
planner_messenger(planner_messenger),
world(world),
selective_collision(selective_collision),
distance_to_leading_vehicle(distance_to_leading_vehicle),
debug_helper(debug_helper) {
// Initializing clock for checking unregistered actors periodically.
@ -248,27 +250,41 @@ namespace CollisionStageConstants {
if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) {
float velocity = actor->GetVelocity().Length();
cg::Location vehicle_location = actor->GetLocation();
float bbox_extension =
(std::max(std::sqrt(EXTENSION_SQUARE_POINT * velocity), BOUNDARY_EXTENSION_MINIMUM) +
std::max(velocity * TIME_HORIZON, BOUNDARY_EXTENSION_MINIMUM) +
BOUNDARY_EXTENSION_MINIMUM);
bbox_extension = (velocity > HIGHWAY_SPEED) ? (HIGHWAY_TIME_HORIZON * velocity) : bbox_extension;
if (distance_to_leading_vehicle.Contains(actor->GetId())) {
bbox_extension = std::max(distance_to_leading_vehicle.GetValue(actor->GetId()), bbox_extension);
}
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;
SimpleWaypointPtr boundary_start = waypoint_buffer->front();
SimpleWaypointPtr boundary_end = waypoint_buffer->front();
uint boundary_start_index = 0u;
while (boundary_start->DistanceSquared(vehicle_location) < std::pow(length, 2) &&
boundary_start_index < waypoint_buffer->size() -1) {
boundary_start = waypoint_buffer->at(boundary_start_index);
++boundary_start_index;
}
SimpleWaypointPtr boundary_end = waypoint_buffer->at(boundary_start_index);
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.
for (uint j = 0u;
for (uint j = boundary_start_index;
(boundary_start->DistanceSquared(boundary_end) < std::pow(bbox_extension, 2)) &&
(j < waypoint_buffer->size());
++j) {

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

@ -64,6 +64,8 @@ namespace bg = boost::geometry;
cc::World &world;
/// Map specifying selective collision avoidance rules between vehicles.
AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> &selective_collision;
/// Map specifying distance to leading vehicle.
AtomicMap<ActorId, float> &distance_to_leading_vehicle;
/// Reference to Carla's debug helper object.
cc::DebugHelper &debug_helper;
/// An object used for grid binning vehicles for faster proximity detection.
@ -110,7 +112,9 @@ namespace bg = boost::geometry;
std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
cc::World &world,
AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> &selective_collision,
AtomicMap<ActorId, float> &distance_to_leading_vehicle,
cc::DebugHelper &debug_helper);
~CollisionStage();
void DataReceiver() override;

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

@ -39,7 +39,7 @@ namespace traffic_manager {
collision_stage = std::make_unique<CollisionStage>(
localization_collision_messenger, collision_planner_messenger,
world, ignore_collision, debug_helper);
world, ignore_collision, distance_to_leading_vehicle, debug_helper);
traffic_light_stage = std::make_unique<TrafficLightStage>(
localization_traffic_light_messenger, traffic_light_planner_messenger);
@ -88,11 +88,11 @@ namespace traffic_manager {
return *singleton_pointer.get();
}
void TrafficManager::RegisterVehicles(std::vector<ActorPtr> actor_list) {
void TrafficManager::RegisterVehicles(const std::vector<ActorPtr> &actor_list) {
registered_actors.Insert(actor_list);
}
void TrafficManager::UnregisterVehicles(std::vector<ActorPtr> actor_list) {
void TrafficManager::UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
registered_actors.Remove(actor_list);
}
@ -128,15 +128,15 @@ namespace traffic_manager {
control_stage->Stop();
}
void TrafficManager::SetVehicleTargetVelocity(ActorId actor_id, float velocity) {
void TrafficManager::SetVehicleTargetVelocity(const ActorPtr &actor, const float velocity) {
vehicle_target_velocity.AddEntry({actor_id, velocity});
vehicle_target_velocity.AddEntry({actor->GetId(), velocity});
}
void TrafficManager::SetCollisionDetection(
ActorPtr reference_actor,
ActorPtr other_actor,
bool detect_collision) {
const ActorPtr &reference_actor,
const ActorPtr &other_actor,
const bool detect_collision) {
ActorId reference_id = reference_actor->GetId();
ActorId other_id = other_actor->GetId();
@ -165,15 +165,21 @@ namespace traffic_manager {
}
}
void TrafficManager::ForceLaneChange(ActorPtr actor, bool direction) {
void TrafficManager::ForceLaneChange(const ActorPtr &actor, const bool direction) {
auto entry = std::make_pair(actor->GetId(), direction);
force_lane_change.AddEntry(entry);
}
void TrafficManager::AutoLaneChange(ActorPtr actor, bool enable) {
void TrafficManager::AutoLaneChange(const ActorPtr &actor, const bool enable) {
auto entry = std::make_pair(actor->GetId(), enable);
auto_lane_change.AddEntry(entry);
}
void TrafficManager::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
auto entry = std::make_pair(actor->GetId(), distance);
distance_to_leading_vehicle.AddEntry(entry);
}
}

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

@ -75,6 +75,8 @@ namespace cc = carla::client;
static std::unique_ptr<TrafficManager> singleton_pointer;
/// Singleton lifecycle management mutex.
// static std::mutex singleton_mutex;
/// Map containing distance to leading vehicle command.
AtomicMap<ActorId, float> distance_to_leading_vehicle;
/// Private constructor for singleton lifecycle management.
TrafficManager(
@ -97,23 +99,29 @@ namespace cc = carla::client;
static TrafficManager& GetInstance(cc::Client &client_connection);
/// This method registers a vehicle with the traffic manager.
void RegisterVehicles(std::vector<ActorPtr> actor_list);
void RegisterVehicles(const std::vector<ActorPtr> &actor_list);
/// This method unregisters a vehicle from traffic manager.
void UnregisterVehicles(std::vector<ActorPtr> actor_list);
void UnregisterVehicles(const std::vector<ActorPtr> &actor_list);
/// Set target velocity specific to a vehicle.
void SetVehicleTargetVelocity(ActorId actor_id, float velocity);
void SetVehicleTargetVelocity(const ActorPtr &actor, const float velocity);
/// Set collision detection rules between vehicles.
void SetCollisionDetection(ActorPtr reference_actor, ActorPtr other_actor, 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.
void ForceLaneChange(ActorPtr actor, bool direction);
void ForceLaneChange(const ActorPtr &actor, const bool direction);
/// Enable / disable automatic lane change on a vehicle.
void AutoLaneChange(ActorPtr actor, bool enable);
void AutoLaneChange(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);
/// Destructor.
~TrafficManager();

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

@ -22,7 +22,8 @@ void export_trafficmanager() {
.def("set_vehicle_target_velocity", &traffic_manager::TrafficManager::SetVehicleTargetVelocity)
.def("set_collision_detection", &traffic_manager::TrafficManager::SetCollisionDetection)
.def("force_lane_change", &traffic_manager::TrafficManager::ForceLaneChange)
.def("set_auto_lane_change", &traffic_manager::TrafficManager::AutoLaneChange);
.def("set_auto_lane_change", &traffic_manager::TrafficManager::AutoLaneChange)
.def("set_distance_to_leading_vehicle", &traffic_manager::TrafficManager::SetDistanceToLeadingVehicle);
def("GetTrafficManager", &traffic_manager::TrafficManager::GetInstance, return_value_policy<reference_existing_object>());

5
PythonAPI/examples/tm_demo.py
View File

@ -114,7 +114,10 @@ def main():
time.sleep(1)
for v in vehicle_list:
if (v.id % 2 == 0):
traffic_manager.set_vehicle_target_velocity(v.id, 15.0/3.6)
traffic_manager.set_vehicle_target_velocity(v, 15.0/3.6)
traffic_manager.set_distance_to_leading_vehicle(v, 5)
else:
traffic_manager.set_distance_to_leading_vehicle(v, 10)
while True:
time.sleep(1)