diff --git a/TrafficManager/CMakeLists.txt b/TrafficManager/CMakeLists.txt index e73c50040..febadea6e 100644 --- a/TrafficManager/CMakeLists.txt +++ b/TrafficManager/CMakeLists.txt @@ -9,26 +9,24 @@ set(CMAKE_CXX_COMPILER /usr/bin/clang++-7) ## Release options set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -DNDEBUG" CACHE STRING "" FORCE) -set(LIBCARLA_LOCATION $ENV{LIBCARLA_LOCATION}) set(CARLA_LOCATION $ENV{CARLA_LOCATION}) -include_directories(source/pipeline/) -include_directories(${LIBCARLA_LOCATION}/source/) +include_directories(${CARLA_LOCATION}/TrafficManager/source/pipeline/) +include_directories(${CARLA_LOCATION}/LibCarla/source/) include_directories(${CARLA_LOCATION}/Build/boost-1.69.0-c7-install/include) include_directories(${CARLA_LOCATION}/Build/recast-cdce4e-c7-install/include) include_directories(${CARLA_LOCATION}/Build/rpclib-v2.2.1_c2-c7-libstdcxx-install/include) -link_directories(${LIBCARLA_LOCATION}/lib/) link_directories(${CARLA_LOCATION}/Build/boost-1.69.0-c7-install/lib) link_directories(${CARLA_LOCATION}/Build/recast-cdce4e-c7-install/lib) link_directories(${CARLA_LOCATION}/Build/rpclib-v2.2.1_c2-c7-libstdcxx-install/lib) link_directories(${CARLA_LOCATION}/PythonAPI/carla/dependencies/lib) -file(GLOB SOURCES source/pipeline/*.cpp) +file(GLOB SOURCES ${CARLA_LOCATION}/TrafficManager/source/pipeline/*.cpp) ## Release executable -file(GLOB SOURCES_EXE source/pipeline/executable/PipelineExecutable.cpp) +file(GLOB SOURCES_EXE ${CARLA_LOCATION}/TrafficManager/source/pipeline/executable/PipelineExecutable.cpp) ## Debug executable -# file(GLOB SOURCES_EXE source/test/Test.cpp) +# file(GLOB SOURCES_EXE ${CARLA_LOCATION}/TrafficManager/source/test/Test.cpp) add_library(pipeline ${SOURCES}) add_executable(traffic_manager ${SOURCES_EXE}) diff --git a/TrafficManager/source/pipeline/Messenger.h b/TrafficManager/source/pipeline/Messenger.h index 5a10b189f..bca49c49d 100644 --- a/TrafficManager/source/pipeline/Messenger.h +++ b/TrafficManager/source/pipeline/Messenger.h @@ -43,7 +43,7 @@ namespace traffic_manager { public: Messenger() { - state_counter = 0; + state_counter.store(0); stop_messenger.store(false); } ~Messenger() {} @@ -88,6 +88,13 @@ namespace traffic_manager { stop_messenger.store(true); } + /// This method restores regular functionality of the messenger. + /// This method needs to be called if the messenger has to be + /// used again after a call to the Stop() method. + void Start() { + stop_messenger.store(false); + } + }; } diff --git a/TrafficManager/source/pipeline/Pipeline.cpp b/TrafficManager/source/pipeline/Pipeline.cpp index 7eff0e4c9..6a111017f 100644 --- a/TrafficManager/source/pipeline/Pipeline.cpp +++ b/TrafficManager/source/pipeline/Pipeline.cpp @@ -2,122 +2,6 @@ namespace traffic_manager { -namespace PipelineConstants { - uint MINIMUM_CORE_COUNT = 4u; - uint MINIMUM_NUMBER_OF_VEHICLES = 100u; -} - using namespace PipelineConstants; - - uint read_core_count() { - - // Assuming quad-core if core count not available. - uint core_count = std::thread::hardware_concurrency(); - return core_count > 0 ? core_count : MINIMUM_CORE_COUNT; - } - - std::vector spawn_traffic( - cc::Client &client, - cc::World &world, - uint core_count, - uint target_amount = 0) { - - std::vector actor_list; - carla::SharedPtr world_map = world.GetMap(); - - auto max_random = [] (uint limit) {return rand()%limit;}; - - // Get a random selection of spawn points from the map. - std::vector spawn_points = world_map->GetRecommendedSpawnPoints(); - std::random_shuffle(spawn_points.begin(), spawn_points.end(), max_random); - - // Blueprint library containing all vehicle types. - using BlueprintLibraryPtr = carla::SharedPtr; - BlueprintLibraryPtr blueprint_library = world.GetBlueprintLibrary()->Filter("vehicle.*"); - - // Removing unsafe vehicles from the blueprint library. - std::vector safe_blueprint_library; - for (auto &blueprint: *blueprint_library.get()) { - - if (blueprint.GetAttribute("number_of_wheels") == 4 && - blueprint.GetId() != "vehicle.carlamotors.carlacola" && - blueprint.GetId() != "vehicle.bmw.isetta") { - - safe_blueprint_library.push_back(blueprint); - } - } - - // Randomizing the order of the list. - std::random_shuffle(safe_blueprint_library.begin(), safe_blueprint_library.end(), max_random); - - uint number_of_vehicles; - if (target_amount == 0u) { - number_of_vehicles = MINIMUM_NUMBER_OF_VEHICLES; - } else { - number_of_vehicles = target_amount; - } - - if (number_of_vehicles > spawn_points.size()) { - carla::log_warning("Number of requested vehicles more than number available spawn points\n"); - carla::log_info("Spawning vehicle at every spawn point\n"); - number_of_vehicles = spawn_points.size(); - } - - carla::log_info("Spawning " + std::to_string(number_of_vehicles) + " vehicles\n"); - - // Creating spawn batch command. - std::vector batch_spawn_commands; - for (uint i = 0u; i < number_of_vehicles; ++i) { - - cg::Transform spawn_point = spawn_points.at(i); - uint blueprint_size = safe_blueprint_library.size(); - cc::ActorBlueprint blueprint = safe_blueprint_library.at(i % blueprint_size); - - blueprint.SetAttribute("role_name", "traffic_manager"); - - using spawn = cr::Command::SpawnActor; - batch_spawn_commands.push_back(spawn(blueprint.MakeActorDescription(), spawn_point)); - } - - client.ApplyBatch(std::move(batch_spawn_commands)); - // We need to wait till the simulator spawns all vehicles, - // tried to use World::WaitForTick but it also wasn't sufficient. - // We need to find a better way to do this. - std::this_thread::sleep_for(500ms); - - // Gathering actors spawned by the traffic manager. - carla::SharedPtr world_actors = world.GetActors(); - for (auto iter = world_actors->begin(); iter != world_actors->end(); ++iter) { - ActorPtr world_actor = *iter; - auto world_vehicle = boost::static_pointer_cast(world_actor); - std::vector actor_attributes = world_vehicle->GetAttributes(); - bool found_traffic_manager_vehicle = false; - for ( - auto iter = actor_attributes.begin(); - (iter != actor_attributes.end()) && !found_traffic_manager_vehicle; - ++iter - ) { - cc::ActorAttributeValue attribute = *iter; - if (attribute.GetValue() == "traffic_manager") { - found_traffic_manager_vehicle = true; - } - } - if (found_traffic_manager_vehicle) { - actor_list.push_back(world_actor); - } - } - - return actor_list; - } - - void destroy_traffic(std::vector &actor_list, cc::Client &client) { - - std::vector batch_spawn_commands; - for (auto &actor: actor_list) { - batch_spawn_commands.push_back(cr::Command::DestroyActor(actor->GetId())); - } - client.ApplyBatch(std::move(batch_spawn_commands)); - } - Pipeline::Pipeline( std::vector longitudinal_PID_parameters, std::vector longitudinal_highway_PID_parameters, @@ -125,21 +9,26 @@ namespace PipelineConstants { float urban_target_velocity, float highway_target_velocity, std::vector &actor_list, - InMemoryMap &local_map, cc::Client &client_connection, - cc::World &world, - cc::DebugHelper &debug_helper, uint pipeline_width) : longitudinal_PID_parameters(longitudinal_PID_parameters), longitudinal_highway_PID_parameters(longitudinal_highway_PID_parameters), lateral_PID_parameters(lateral_PID_parameters), urban_target_velocity(urban_target_velocity), actor_list(actor_list), - local_map(local_map), client_connection(client_connection), - world(world), - debug_helper(debug_helper), - pipeline_width(pipeline_width) { + pipeline_width(pipeline_width), + world(client_connection.GetWorld()), + debug_helper(client_connection.GetWorld().MakeDebugHelper()) { + + using WorldMap = carla::SharedPtr; + WorldMap world_map = world.GetMap(); + auto dao = CarlaDataAccessLayer(world_map); + using Topology = std::vector>; + Topology topology = dao.GetTopology(); + local_map = std::make_shared(topology); + local_map->SetUp(1.0); + localization_collision_messenger = std::make_shared(); localization_traffic_light_messenger = std::make_shared(); @@ -151,7 +40,7 @@ namespace PipelineConstants { localization_stage = std::make_unique( localization_planner_messenger, localization_collision_messenger, localization_traffic_light_messenger, actor_list.size(), pipeline_width, - actor_list, local_map, + actor_list, *local_map.get(), debug_helper); collision_stage = std::make_unique( @@ -184,6 +73,13 @@ namespace PipelineConstants { void Pipeline::Start() { + localization_collision_messenger->Start(); + localization_traffic_light_messenger->Start(); + localization_planner_messenger->Start(); + collision_planner_messenger->Start(); + traffic_light_planner_messenger->Start(); + planner_control_messenger->Start(); + localization_stage->Start(); collision_stage->Start(); traffic_light_stage->Start(); diff --git a/TrafficManager/source/pipeline/Pipeline.h b/TrafficManager/source/pipeline/Pipeline.h index b6330c7fb..0cf42e933 100644 --- a/TrafficManager/source/pipeline/Pipeline.h +++ b/TrafficManager/source/pipeline/Pipeline.h @@ -5,7 +5,6 @@ #include #include - #include "carla/client/Actor.h" #include "carla/client/BlueprintLibrary.h" #include "carla/client/Map.h" @@ -13,39 +12,21 @@ #include "carla/geom/Transform.h" #include "carla/Logging.h" #include "carla/Memory.h" -#include "carla/rpc/Command.h" #include "BatchControlStage.h" +#include "CarlaDataAccessLayer.h" #include "CollisionStage.h" #include "InMemoryMap.h" #include "LocalizationStage.h" #include "MotionPlannerStage.h" #include "TrafficLightStage.h" -#define EXPECT_TRUE(pred) if (!(pred)) { throw std::runtime_error(# pred); } - namespace traffic_manager { namespace cc = carla::client; -namespace cg = carla::geom; -namespace cr = carla::rpc; + using ActorPtr = carla::SharedPtr; - /// Function to read hardware concurrency. - uint read_core_count(); - - /// Function to spawn a specified number of vehicles. - std::vector spawn_traffic( - cc::Client &client, - cc::World &world, - uint core_count, - uint target_amount); - - /// Destroy actors. - void destroy_traffic( - std::vector &actor_list, - cc::Client &client); - /// The function of this class is to integrate all the various stages of /// the traffic manager appropriately using messengers. class Pipeline { @@ -61,16 +42,16 @@ namespace cr = carla::rpc; /// Target velocities. float highway_target_velocity; float urban_target_velocity; - /// Reference to list of all actors registered with traffic manager. - std::vector &actor_list; - /// Reference to local map cache. - InMemoryMap &local_map; - /// Reference to Carla's debug helper object. - cc::DebugHelper &debug_helper; - /// Reference to Carla's client connection object. - cc::Client &client_connection; - /// Reference to Carla's world object. - cc::World &world; + /// List of all actors registered with traffic manager. + std::vector actor_list; + /// Pointer to local map cache. + std::shared_ptr local_map; + /// Carla's debug helper object. + cc::DebugHelper debug_helper; + /// Carla's client connection object. + cc::Client client_connection; + /// Carla's world object. + cc::World world; /// Pointers to messenger objects connecting stage pairs. std::shared_ptr collision_planner_messenger; std::shared_ptr localization_collision_messenger; @@ -94,10 +75,7 @@ namespace cr = carla::rpc; float urban_target_velocity, float highway_target_velocity, std::vector &actor_list, - InMemoryMap &local_map, cc::Client &client_connection, - cc::World &world, - cc::DebugHelper &debug_helper, uint pipeline_width); /// To start the pipeline. diff --git a/TrafficManager/source/pipeline/executable/PipelineExecutable.cpp b/TrafficManager/source/pipeline/executable/PipelineExecutable.cpp index c26c7d719..5be9d7c9a 100644 --- a/TrafficManager/source/pipeline/executable/PipelineExecutable.cpp +++ b/TrafficManager/source/pipeline/executable/PipelineExecutable.cpp @@ -12,16 +12,16 @@ #include "carla/client/TimeoutException.h" #include "carla/Logging.h" #include "carla/Memory.h" +#include "carla/rpc/Command.h" -#include "CarlaDataAccessLayer.h" -#include "InMemoryMap.h" #include "Pipeline.h" -namespace cc = carla::client; -using Actor = carla::SharedPtr; +static uint MINIMUM_NUMBER_OF_VEHICLES = 100u; -void run_pipeline(cc::World &world, cc::Client &client_conn, - uint target_traffic_amount, uint randomization_seed); +namespace cc = carla::client; +namespace cg = carla::geom; +namespace cr = carla::rpc; +using Actor = carla::SharedPtr; std::atomic quit(false); void got_signal(int) { @@ -47,6 +47,160 @@ void handler() { } } +std::vector spawn_traffic( + cc::Client &client, + cc::World &world, + uint target_amount = 0) { + + std::vector actor_list; + carla::SharedPtr world_map = world.GetMap(); + + auto max_random = [] (uint limit) {return rand()%limit;}; + + // Get a random selection of spawn points from the map. + std::vector spawn_points = world_map->GetRecommendedSpawnPoints(); + std::random_shuffle(spawn_points.begin(), spawn_points.end(), max_random); + + // Blueprint library containing all vehicle types. + using BlueprintLibraryPtr = carla::SharedPtr; + BlueprintLibraryPtr blueprint_library = world.GetBlueprintLibrary()->Filter("vehicle.*"); + + // Removing unsafe vehicles from the blueprint library. + std::vector safe_blueprint_library; + for (auto &blueprint: *blueprint_library.get()) { + + if (blueprint.GetAttribute("number_of_wheels") == 4 && + blueprint.GetId() != "vehicle.carlamotors.carlacola" && + blueprint.GetId() != "vehicle.bmw.isetta") { + + safe_blueprint_library.push_back(blueprint); + } + } + + // Randomizing the order of the list. + std::random_shuffle(safe_blueprint_library.begin(), safe_blueprint_library.end(), max_random); + + uint number_of_vehicles; + if (target_amount == 0u) { + number_of_vehicles = MINIMUM_NUMBER_OF_VEHICLES; + } else { + number_of_vehicles = target_amount; + } + + if (number_of_vehicles > spawn_points.size()) { + carla::log_warning("Number of requested vehicles more than number available spawn points\n"); + carla::log_info("Spawning vehicle at every spawn point\n"); + number_of_vehicles = spawn_points.size(); + } + + carla::log_info("Spawning " + std::to_string(number_of_vehicles) + " vehicles\n"); + + // Creating spawn batch command. + std::vector batch_spawn_commands; + for (uint i = 0u; i < number_of_vehicles; ++i) { + + cg::Transform spawn_point = spawn_points.at(i); + uint blueprint_size = safe_blueprint_library.size(); + cc::ActorBlueprint blueprint = safe_blueprint_library.at(i % blueprint_size); + + blueprint.SetAttribute("role_name", "traffic_manager"); + + using spawn = cr::Command::SpawnActor; + batch_spawn_commands.push_back(spawn(blueprint.MakeActorDescription(), spawn_point)); + } + + client.ApplyBatch(std::move(batch_spawn_commands)); + // We need to wait till the simulator spawns all vehicles, + // tried to use World::WaitForTick but it also wasn't sufficient. + // We need to find a better way to do this. + std::this_thread::sleep_for(500ms); + + // Gathering actors spawned by the traffic manager. + carla::SharedPtr world_actors = world.GetActors(); + for (auto iter = world_actors->begin(); iter != world_actors->end(); ++iter) { + Actor world_actor = *iter; + auto world_vehicle = boost::static_pointer_cast(world_actor); + std::vector actor_attributes = world_vehicle->GetAttributes(); + bool found_traffic_manager_vehicle = false; + for ( + auto iter = actor_attributes.begin(); + (iter != actor_attributes.end()) && !found_traffic_manager_vehicle; + ++iter + ) { + cc::ActorAttributeValue attribute = *iter; + if (attribute.GetValue() == "traffic_manager") { + found_traffic_manager_vehicle = true; + } + } + if (found_traffic_manager_vehicle) { + actor_list.push_back(world_actor); + } + } + + return actor_list; +} + + +void destroy_traffic(std::vector &actor_list, cc::Client &client) { + + std::vector batch_spawn_commands; + for (auto &actor: actor_list) { + batch_spawn_commands.push_back(cr::Command::DestroyActor(actor->GetId())); + } + client.ApplyBatch(std::move(batch_spawn_commands)); +} + + +void run_pipeline(cc::World &world, cc::Client &client_conn, + uint target_traffic_amount, uint randomization_seed) { + + struct sigaction sa; + memset(&sa, 0, sizeof(sa)); + sa.sa_handler = got_signal; + sigfillset(&sa.sa_mask); + sigaction(SIGINT, &sa, NULL); + + std::vector registered_actors = spawn_traffic( + client_conn, world, target_traffic_amount); + global_actor_list = ®istered_actors; + + client_conn.SetTimeout(2s); + + traffic_manager::Pipeline pipeline( + {0.1f, 0.15f, 0.01f}, + {5.0f, 0.0f, 0.1f}, + {10.0f, 0.01f, 0.1f}, + 25 / 3.6, + 50 / 3.6, + registered_actors, + client_conn, + 1 + ); + + try + { + pipeline.Start(); + carla::log_info("TrafficManager started\n"); + + while (!quit.load()) { + sleep(1); + // Periodically polling to check if Carla is still running. + world.GetSettings(); + } + } + catch(const cc::TimeoutException& e) + { + carla::log_error("Carla has stopped running, stopping TrafficManager\n"); + } + + pipeline.Stop(); + + destroy_traffic(registered_actors, client_conn); + + carla::log_info("\nTrafficManager stopped by user\n"); +} + + int main(int argc, char *argv[]) { std::set_terminate(handler); @@ -54,6 +208,7 @@ int main(int argc, char *argv[]) { cc::World world = client_conn.GetWorld(); if (argc == 2 && std::string(argv[1]) == "-h") { + std::cout << "\nAvailable options\n"; std::cout << "[-n] \t\t Number of vehicles to be spawned\n"; std::cout << "[-s] \t\t System randomization seed integer\n"; @@ -89,65 +244,3 @@ int main(int argc, char *argv[]) { return 0; } - -void run_pipeline(cc::World &world, cc::Client &client_conn, - uint target_traffic_amount, uint randomization_seed) { - - struct sigaction sa; - memset(&sa, 0, sizeof(sa)); - sa.sa_handler = got_signal; - sigfillset(&sa.sa_mask); - sigaction(SIGINT, &sa, NULL); - - using WorldMap = carla::SharedPtr; - WorldMap world_map = world.GetMap(); - cc::DebugHelper debug_helper = client_conn.GetWorld().MakeDebugHelper(); - auto dao = traffic_manager::CarlaDataAccessLayer(world_map); - using Topology = std::vector>; - Topology topology = dao.GetTopology(); - auto local_map = std::make_shared(topology); - local_map->SetUp(1.0); - - uint core_count = traffic_manager::read_core_count(); - std::vector registered_actors = traffic_manager::spawn_traffic( - client_conn, world, core_count, target_traffic_amount); - global_actor_list = ®istered_actors; - - client_conn.SetTimeout(2s); - - traffic_manager::Pipeline pipeline( - {0.1f, 0.15f, 0.01f}, - {5.0f, 0.0f, 0.1f}, - {10.0f, 0.01f, 0.1f}, - 25 / 3.6, - 50 / 3.6, - registered_actors, - *local_map.get(), - client_conn, - world, - debug_helper, - 1 - ); - - try - { - pipeline.Start(); - carla::log_info("TrafficManager started\n"); - - while (!quit.load()) { - sleep(1); - // Periodically polling if Carla is still running - world.GetSettings(); - } - } - catch(const cc::TimeoutException& e) - { - carla::log_error("Carla has stopped running, stopping TrafficManager\n"); - } - - pipeline.Stop(); - - traffic_manager::destroy_traffic(registered_actors, client_conn); - - carla::log_info("\nTrafficManager stopped by user\n"); -}