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

traffic light reset demo working + other stuff

This commit is contained in:
Jacopo Bartiromo 2019-12-03 20:39:52 +01:00 committed by bernat
parent f1c97fceb0
commit 2428615934
7 changed files with 60 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;

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

@ -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;

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

@ -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<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> 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<cc::TrafficLight>(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<cc::Vehicle>(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

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

@ -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<LocalizationToTrafficLightMessenger> localization_messenger,
std::shared_ptr<TrafficLightToPlannerMessenger> planner_messenger,
cc::DebugHelper &debug_helper,
cc::World &world);
cc::DebugHelper &debug_helper);
~TrafficLightStage();
void DataReceiver() override;

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

@ -48,7 +48,7 @@ namespace traffic_manager {
traffic_light_stage = std::make_unique<TrafficLightStage>(
"Traffic light stage",
localization_traffic_light_messenger, traffic_light_planner_messenger,
debug_helper, world);
debug_helper);
planner_stage = std::make_unique<MotionPlannerStage>(
"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<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()) {
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);
// tl_group is a std::vector<boost::shared_ptr<carla::client::TrafficLight>
for (unsigned i=0; i<tl_group.size(); i++) {
// tlg is a boost::shared_ptr<carla::client::TrafficLight>
// 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,6 +206,10 @@ 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(),

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

@ -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<reference_existing_object>());

38
PythonAPI/examples/tm_demo.py
View File

@ -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)