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

traffic light debugger, no junctions, lane marking

This commit is contained in:
Jacopo Bartiromo 2019-11-18 19:35:32 +01:00 committed by bernat
parent 65f84442e0
commit a7068d2f45
6 changed files with 62 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,7 +6,7 @@ namespace CollisionStageConstants {
static const float VERTICAL_OVERLAP_THRESHOLD = 2.0f;
static const float ZERO_AREA = 0.0001f;
static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f;
static const float EXTENSION_SQUARE_POINT = 7.0f;
static const float EXTENSION_SQUARE_POINT = 7.5f;
static const float TIME_HORIZON = 0.5f;
static const float HIGHWAY_SPEED = 50.0f / 3.6f;
static const float HIGHWAY_TIME_HORIZON = 5.0f;
@ -93,6 +93,8 @@ namespace CollisionStageConstants {
Actor ego_actor = data.actor;
ActorId ego_actor_id = ego_actor->GetId();
//DrawBoundary(GetGeodesicBoundary(ego_actor));
// Retrieve actors around ego actor.
std::unordered_set<ActorId> actor_id_list = vicinity_grid.GetActors(ego_actor);
bool collision_hazard = false;

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

@ -101,6 +101,12 @@ namespace TrafficDistributorConstants {
auto left_waypoint = current_waypoint->GetLeftWaypoint();
auto right_waypoint = current_waypoint->GetRightWaypoint();
auto lane_change = current_waypoint->GetWaypoint()->GetLaneChange();
auto change_right = carla::road::element::LaneMarking::LaneChange::Right;
auto change_left = carla::road::element::LaneMarking::LaneChange::Left;
//auto change_both = carla::road::element::LaneMarking::LaneChange::Both;
// Don't try to change lane if the current lane has less than two vehicles.
if (co_lane_vehicles.size() >= 2 && !force) {
@ -134,7 +140,10 @@ namespace TrafficDistributorConstants {
// If lane change connections are available,
// pick a direction (preferring left) and
// announce the need for a lane change.
if (left_waypoint != nullptr) {
//if (lane_change == change_both) {
// Method to assign either left or right, based on road occupancy.
//}
if (left_waypoint != nullptr && lane_change == change_left) {
traffic_manager::ActorIDSet left_lane_vehicles = GetVehicleIds({
current_road_ids.road_id,
current_road_ids.section_id,
@ -144,7 +153,7 @@ namespace TrafficDistributorConstants {
need_to_change_lane = true;
lane_change_direction = true;
}
} else if (right_waypoint != nullptr) {
} else if (right_waypoint != nullptr && lane_change == change_right) {
traffic_manager::ActorIDSet right_lane_vehicles = GetVehicleIds({
current_road_ids.road_id,
current_road_ids.section_id,

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

@ -6,9 +6,11 @@ namespace traffic_manager {
TrafficLightStage::TrafficLightStage(
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger)
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger,
cc::DebugHelper &debug_helper)
: localization_messenger(localization_messenger),
planner_messenger(planner_messenger) {
planner_messenger(planner_messenger),
debug_helper(debug_helper) {
// Initializing output frame selector.
frame_selector = true;
@ -46,22 +48,19 @@ namespace traffic_manager {
auto ego_vehicle = boost::static_pointer_cast<cc::Vehicle>(ego_actor);
TLS traffic_light_state = ego_vehicle->GetTrafficLightState();
//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
// is inside the junction and there is a red or yellow light.
if (ego_vehicle->IsAtTrafficLight() &&
!closest_waypoint->CheckJunction() &&
look_ahead_point->CheckJunction() &&
traffic_light_state != TLS::Green) {
traffic_light_hazard = true;
}
// Handle entry negotiation at non-signalised junction.
else if (!ego_vehicle->IsAtTrafficLight() &&
!closest_waypoint->CheckJunction() &&
look_ahead_point->CheckJunction() &&
traffic_light_state != TLS::Green) {
std::lock_guard<std::mutex> lock(no_signal_negotiation_mutex);
@ -155,4 +154,34 @@ namespace traffic_manager {
frame_selector = !frame_selector;
planner_messenger_state = planner_messenger->SendData(packet);
}
void TrafficLightStage::DrawLight(TLS traffic_light_state, const Actor &ego_actor) const {
std::string str;
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);
}
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);
}
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);
}
}
}

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

@ -46,6 +46,8 @@ namespace cg = carla::geom;
/// Pointers to messenger objects.
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger;
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger;
/// Reference to Carla's debug helper object.
cc::DebugHelper &debug_helper;
/// Map containing the time ticket issued for vehicles.
std::unordered_map<ActorId, TimeInstance> vehicle_last_ticket;
/// Map containing the previous time ticket issued for junctions.
@ -57,11 +59,15 @@ namespace cg = carla::geom;
/// Number of vehicles registered with the traffic manager.
uint number_of_vehicles;
void DrawLight(TLS traffic_light_state, const Actor &ego_actor) const;
public:
TrafficLightStage(
std::shared_ptr<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger);
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger,
cc::DebugHelper &debug_helper);
~TrafficLightStage();
void DataReceiver() override;

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

@ -43,7 +43,7 @@ namespace traffic_manager {
world, parameters, debug_helper);
traffic_light_stage = std::make_unique<TrafficLightStage>(
localization_traffic_light_messenger, traffic_light_planner_messenger);
localization_traffic_light_messenger, traffic_light_planner_messenger, debug_helper);
planner_stage = std::make_unique<MotionPlannerStage>(
localization_planner_messenger,

5
PythonAPI/examples/tm_demo.py
View File

@ -120,6 +120,11 @@ def main():
traffic_manager.set_distance_to_leading_vehicle(v, 6)
pass
time.sleep(10)
for v in vehicle_list:
traffic_manager.force_lane_change(v, True)
while True:
time.sleep(1)