From a7068d2f4555a6f1417eb7b39357e564327ed85c Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo Date: Mon, 18 Nov 2019 19:35:32 +0100 Subject: [PATCH] traffic light debugger, no junctions, lane marking --- .../carla/trafficmanager/CollisionStage.cpp | 4 +- .../trafficmanager/TrafficDistributor.cpp | 13 +++++- .../trafficmanager/TrafficLightStage.cpp | 41 ++++++++++++++++--- .../carla/trafficmanager/TrafficLightStage.h | 8 +++- .../carla/trafficmanager/TrafficManager.cpp | 2 +- PythonAPI/examples/tm_demo.py | 5 +++ 6 files changed, 62 insertions(+), 11 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index bf326271e..73c9e9813 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -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 actor_id_list = vicinity_grid.GetActors(ego_actor); bool collision_hazard = false; diff --git a/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp b/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp index 314ddcd1e..bf9efb72d 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp @@ -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, diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index 4d9f36eac..ca2759647 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -6,9 +6,11 @@ namespace traffic_manager { TrafficLightStage::TrafficLightStage( std::shared_ptr localization_messenger, - std::shared_ptr planner_messenger) + std::shared_ptr 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(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 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); + } + } } diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h index bef552f63..dc71fabcd 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h @@ -46,6 +46,8 @@ namespace cg = carla::geom; /// Pointers to messenger objects. std::shared_ptr localization_messenger; std::shared_ptr planner_messenger; + /// Reference to Carla's debug helper object. + cc::DebugHelper &debug_helper; /// Map containing the time ticket issued for vehicles. std::unordered_map 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 localization_messenger, - std::shared_ptr planner_messenger); + std::shared_ptr planner_messenger, + cc::DebugHelper &debug_helper); ~TrafficLightStage(); void DataReceiver() override; diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp index 98a086537..16d1954b6 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp @@ -43,7 +43,7 @@ namespace traffic_manager { world, parameters, debug_helper); traffic_light_stage = std::make_unique( - localization_traffic_light_messenger, traffic_light_planner_messenger); + localization_traffic_light_messenger, traffic_light_planner_messenger, debug_helper); planner_stage = std::make_unique( localization_planner_messenger, diff --git a/PythonAPI/examples/tm_demo.py b/PythonAPI/examples/tm_demo.py index 78eee7040..b0c0eaf72 100644 --- a/PythonAPI/examples/tm_demo.py +++ b/PythonAPI/examples/tm_demo.py @@ -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)