diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index a7b9a79ea..9536190b4 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -11,7 +11,7 @@ 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 MINIMUM_GRID_EXTENSION = 10.0f; static const float MAX_COLLISION_RADIUS = 100.0f; } using namespace CollisionStageConstants; diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index ca86175c5..f62f325fe 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -463,19 +463,19 @@ namespace LocalizationConstants { bool need_to_change_lane = false; auto left_waypoint = current_waypoint->GetLeftWaypoint(); auto right_waypoint = current_waypoint->GetRightWaypoint(); - auto lane_change = current_waypoint->GetWaypoint()->GetLaneChange(); + //auto lane_change = current_waypoint->GetWaypoint()->GetLaneChange(); if (!force) { auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b; - uint vehicle_index = vehicle_id_to_index.at(actor_id); + //uint vehicle_index = vehicle_id_to_index.at(actor_id); - ActorIdSet overlapping_vehicles = GetOverlappingVehicles(actor_id); + auto blocking_vehicles = GetOverlappingVehicles(actor_id); // Check if any vehicle in the current lane is blocking us. bool abort_lane_change = false; - for (auto i = overlapping_vehicles.begin(); - i != overlapping_vehicles.end() && !abort_lane_change; + for (auto i = blocking_vehicles.begin(); + i != blocking_vehicles.end() && !abort_lane_change; ++i) { const ActorId &other_vehicle_id = *i; diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index d039c8271..1f7c810b8 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -4,18 +4,16 @@ namespace traffic_manager { static const uint NO_SIGNAL_PASSTHROUGH_INTERVAL = 5u; - static bool initialized = false; + TrafficLightStage::TrafficLightStage( std::string stage_name, std::shared_ptr localization_messenger, std::shared_ptr planner_messenger, - cc::DebugHelper &debug_helper, - cc::World &world) + cc::DebugHelper &debug_helper) : PipelineStage(stage_name), localization_messenger(localization_messenger), planner_messenger(planner_messenger), - debug_helper(debug_helper), - world(world) { + debug_helper(debug_helper){ // Initializing output frame selector. frame_selector = true; @@ -33,20 +31,6 @@ namespace traffic_manager { TrafficLightStage::~TrafficLightStage() {} - void TrafficLightStage::ResetAllTrafficLightGroups() { - // TO BE FINISHED - if (!initialized) { - initialized = true; - auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*"); - for (auto tl : *world_traffic_lights.get()) { - auto group = boost::static_pointer_cast(tl)->GetGroupTrafficLights(); - for (auto g : group) { - std::cout << g->GetId() << std::endl; - } - } - } - } - void TrafficLightStage::Action() { // Selecting the output frame based on the selection key. @@ -67,7 +51,7 @@ 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); - ResetAllTrafficLightGroups(); + // 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 diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h index fda11687a..d59a6aaae 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h @@ -60,12 +60,9 @@ namespace cg = carla::geom; std::mutex no_signal_negotiation_mutex; /// Number of vehicles registered with the traffic manager. uint number_of_vehicles; - /// Reference to Carla's world object. - cc::World &world; void DrawLight(TLS traffic_light_state, const Actor &ego_actor) const; - void ResetAllTrafficLightGroups(); public: @@ -73,8 +70,7 @@ namespace cg = carla::geom; std::string stage_name, std::shared_ptr localization_messenger, std::shared_ptr planner_messenger, - cc::DebugHelper &debug_helper, - cc::World &world); + 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 7173e0ef8..d1fffdabc 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp @@ -48,7 +48,7 @@ namespace traffic_manager { traffic_light_stage = std::make_unique( "Traffic light stage", localization_traffic_light_messenger, traffic_light_planner_messenger, - debug_helper, world); + debug_helper); planner_stage = std::make_unique( "Motion planner stage", @@ -179,7 +179,7 @@ namespace traffic_manager { bool TrafficManager::CheckAllFrozen(TLGroup tl_to_freeze) { for (auto& elem : tl_to_freeze) { - if (!elem->IsFrozen() or elem->GetState() != TLS::Red) { + if (!elem->IsFrozen() || elem->GetState() != TLS::Red) { return false; } } @@ -188,18 +188,16 @@ namespace traffic_manager { void TrafficManager::ResetAllTrafficLights() { auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*"); + std::cout << "hi "; + std::vector list_of_all_groups; TLGroup tl_to_freeze; std::vector 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()) { + if (!(std::find(list_of_ids.begin(), list_of_ids.end(), tl->GetId()) != list_of_ids.end())) { TLGroup tl_group = boost::static_pointer_cast(tl)->GetGroupTrafficLights(); list_of_all_groups.push_back(tl_group); - // tl_group is a std::vector for (unsigned i=0; i - // tlg->Id is a carla::ActorId list_of_ids.push_back(tl_group.at(i).get()->GetId()); if(i!=0) { tl_to_freeze.push_back(tl_group.at(i)); @@ -208,9 +206,13 @@ namespace traffic_manager { } } + std::cout << "hello "; + for (auto i = list_of_ids.begin(); i != list_of_ids.end(); ++i) + std::cout << *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(), + std::for_each(tl_group.begin()+1, tl_group.end(), [] (auto& tl) {tl->SetState(TLS::Red);}); } diff --git a/PythonAPI/carla/source/libcarla/TrafficManager.cpp b/PythonAPI/carla/source/libcarla/TrafficManager.cpp index ff07e9bae..987337172 100644 --- a/PythonAPI/carla/source/libcarla/TrafficManager.cpp +++ b/PythonAPI/carla/source/libcarla/TrafficManager.cpp @@ -23,7 +23,8 @@ void export_trafficmanager() { .def("set_collision_detection", &traffic_manager::TrafficManager::SetCollisionDetection) .def("force_lane_change", &traffic_manager::TrafficManager::SetForceLaneChange) .def("set_auto_lane_change", &traffic_manager::TrafficManager::SetAutoLaneChange) - .def("set_distance_to_leading_vehicle", &traffic_manager::TrafficManager::SetDistanceToLeadingVehicle); + .def("set_distance_to_leading_vehicle", &traffic_manager::TrafficManager::SetDistanceToLeadingVehicle) + .def("reset_traffic_lights", &traffic_manager::TrafficManager::ResetAllTrafficLights); def("GetTrafficManager", &traffic_manager::TrafficManager::GetInstance, return_value_policy()); diff --git a/PythonAPI/examples/tm_demo.py b/PythonAPI/examples/tm_demo.py index b0c0eaf72..e34bc2016 100644 --- a/PythonAPI/examples/tm_demo.py +++ b/PythonAPI/examples/tm_demo.py @@ -24,7 +24,6 @@ except IndexError: pass import carla - def main(): argparser = argparse.ArgumentParser( description=__doc__) @@ -66,6 +65,35 @@ def main(): traffic_manager = None world = client.get_world() blueprints = world.get_blueprint_library().filter(args.filter) + debug = world.debug + traffic_lights = world.get_actors().filter('*traffic_light*') + list_of_all_groups = [] + all_groups_without_first_elem = [] + list_of_ids = [] + + for tl in traffic_lights: + if tl.id not in list_of_ids: + tl_group = tl.get_group_traffic_lights() + for tl_g in tl_group: + list_of_ids.append(tl_g.id) + if tl_group not in list_of_all_groups: + list_of_all_groups.append(tl_group) + all_groups_without_first_elem.extend(tl_group[1:len(tl_group)]) + + def show_tl_count_down(duration=30, indefinite=False): + end_time = time.time() + duration + color_dict = { + 'Red':carla.Color(255, 0, 0), + 'Yellow':carla.Color(255, 255, 0), + 'Green':carla.Color(0, 255, 0) + } + while time.time() < end_time or indefinite: + for tl_group in list_of_all_groups: + for tl in tl_group: + debug.draw_string(tl.get_location() + carla.Location(0, 0, 5), + str(tl.get_elapsed_time())[:4], False, + color_dict[tl.get_state().name], 0.01) + time.sleep(0.002) if args.safe: blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] @@ -121,9 +149,17 @@ def main(): pass time.sleep(10) + print("Time to lane change!") + for v in vehicle_list: traffic_manager.force_lane_change(v, True) + show_tl_count_down(10) + print("Time to reset the lights!") + start = time.time() + traffic_manager.reset_traffic_lights() + print("Total time needed: " + str(time.time() - start)) + show_tl_count_down(15) while True: time.sleep(1)