Removed redundant environment variable from cmake file

Added start method to messenger template
Moved vehicle lifecycle management functions to PipelineExecutable.cpp

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(${CARLA_LOCATION}/TrafficManager/source/pipeline/)
-include_directories(${LIBCARLA_LOCATION}/source/)
+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})

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);
+    }
+
   };
 
 }

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<ActorPtr> spawn_traffic(
-      cc::Client &client,
-      cc::World &world,
-      uint core_count,
-      uint target_amount = 0) {
-
-    std::vector<ActorPtr> actor_list;
-    carla::SharedPtr<cc::Map> world_map = world.GetMap();
-
-    auto max_random = [] (uint limit) {return rand()%limit;};
-
-    // Get a random selection of spawn points from the map.
-    std::vector<cg::Transform> 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<cc::BlueprintLibrary>;
-    BlueprintLibraryPtr blueprint_library = world.GetBlueprintLibrary()->Filter("vehicle.*");
-
-    // Removing unsafe vehicles from the blueprint library.
-    std::vector<cc::ActorBlueprint> 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<cr::Command> 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<cc::ActorList> 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<cc::Vehicle>(world_actor);
-      std::vector<cc::ActorAttributeValue> 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<ActorPtr> &actor_list, cc::Client &client) {
-
-    std::vector<cr::Command> 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<float> longitudinal_PID_parameters,
       std::vector<float> longitudinal_highway_PID_parameters,
@@ -125,21 +9,26 @@ namespace PipelineConstants {
       float urban_target_velocity,
       float highway_target_velocity,
       std::vector<ActorPtr> &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),
+      pipeline_width(pipeline_width),
-      debug_helper(debug_helper),
+      world(client_connection.GetWorld()),
-      pipeline_width(pipeline_width) {
+      debug_helper(client_connection.GetWorld().MakeDebugHelper()) {
+
+    using WorldMap = carla::SharedPtr<cc::Map>;
+    WorldMap world_map = world.GetMap();
+    auto dao = CarlaDataAccessLayer(world_map);
+    using Topology = std::vector<std::pair<WaypointPtr, WaypointPtr>>;
+    Topology topology = dao.GetTopology();
+    local_map = std::make_shared<traffic_manager::InMemoryMap>(topology);
+    local_map->SetUp(1.0);
+
 
     localization_collision_messenger = std::make_shared<LocalizationToCollisionMessenger>();
     localization_traffic_light_messenger = std::make_shared<LocalizationToTrafficLightMessenger>();
@@ -151,7 +40,7 @@ namespace PipelineConstants {
     localization_stage = std::make_unique<LocalizationStage>(
         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<CollisionStage>(
@@ -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();

TrafficManager/source/pipeline/Pipeline.h

@@ -5,7 +5,6 @@
 #include <random>
 #include <vector>
 
-
 #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<cc::Actor>;
 
-  /// Function to read hardware concurrency.
-  uint read_core_count();
-
-  /// Function to spawn a specified number of vehicles.
-  std::vector<ActorPtr> spawn_traffic(
-      cc::Client &client,
-      cc::World &world,
-      uint core_count,
-      uint target_amount);
-
-  /// Destroy actors.
-  void destroy_traffic(
-      std::vector<ActorPtr> &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.
+    /// List of all actors registered with traffic manager.
-    std::vector<ActorPtr> &actor_list;
+    std::vector<ActorPtr> actor_list;
-    /// Reference to local map cache.
+    /// Pointer to local map cache.
-    InMemoryMap &local_map;
+    std::shared_ptr<InMemoryMap> local_map;
-    /// Reference to Carla's debug helper object.
+    /// Carla's debug helper object.
-    cc::DebugHelper &debug_helper;
+    cc::DebugHelper debug_helper;
-    /// Reference to Carla's client connection object.
+    /// Carla's client connection object.
-    cc::Client &client_connection;
+    cc::Client client_connection;
-    /// Reference to Carla's world object.
+    /// Carla's world object.
-    cc::World &world;
+    cc::World world;
     /// Pointers to messenger objects connecting stage pairs.
     std::shared_ptr<CollisionToPlannerMessenger> collision_planner_messenger;
     std::shared_ptr<LocalizationToCollisionMessenger> localization_collision_messenger;
@@ -94,10 +75,7 @@ namespace cr = carla::rpc;
         float urban_target_velocity,
         float highway_target_velocity,
         std::vector<ActorPtr> &actor_list,
-        InMemoryMap &local_map,
         cc::Client &client_connection,
-        cc::World &world,
-        cc::DebugHelper &debug_helper,
         uint pipeline_width);
 
     /// To start the pipeline.

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;
+static uint MINIMUM_NUMBER_OF_VEHICLES = 100u;
-using Actor = carla::SharedPtr<cc::Actor>;
 
-void run_pipeline(cc::World &world, cc::Client &client_conn,
+namespace cc = carla::client;
-                  uint target_traffic_amount, uint randomization_seed);
+namespace cg = carla::geom;
+namespace cr = carla::rpc;
+using Actor = carla::SharedPtr<cc::Actor>;
 
 std::atomic<bool> quit(false);
 void got_signal(int) {
@@ -47,6 +47,160 @@ void handler() {
   }
 }
 
+std::vector<Actor> spawn_traffic(
+    cc::Client &client,
+    cc::World &world,
+    uint target_amount = 0) {
+
+  std::vector<Actor> actor_list;
+  carla::SharedPtr<cc::Map> world_map = world.GetMap();
+
+  auto max_random = [] (uint limit) {return rand()%limit;};
+
+  // Get a random selection of spawn points from the map.
+  std::vector<cg::Transform> 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<cc::BlueprintLibrary>;
+  BlueprintLibraryPtr blueprint_library = world.GetBlueprintLibrary()->Filter("vehicle.*");
+
+  // Removing unsafe vehicles from the blueprint library.
+  std::vector<cc::ActorBlueprint> 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<cr::Command> 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<cc::ActorList> 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<cc::Vehicle>(world_actor);
+    std::vector<cc::ActorAttributeValue> 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> &actor_list, cc::Client &client) {
+
+  std::vector<cr::Command> 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<Actor> registered_actors = spawn_traffic(
+    client_conn, world, target_traffic_amount);
+  global_actor_list = &registered_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<cc::Map>;
-  WorldMap world_map = world.GetMap();
-  cc::DebugHelper debug_helper = client_conn.GetWorld().MakeDebugHelper();
-  auto dao = traffic_manager::CarlaDataAccessLayer(world_map);
-  using Topology = std::vector<std::pair<traffic_manager::WaypointPtr, traffic_manager::WaypointPtr>>;
-  Topology topology = dao.GetTopology();
-  auto local_map = std::make_shared<traffic_manager::InMemoryMap>(topology);
-  local_map->SetUp(1.0);
-
-  uint core_count = traffic_manager::read_core_count();
-  std::vector<Actor> registered_actors = traffic_manager::spawn_traffic(
-    client_conn, world, core_count, target_traffic_amount);
-  global_actor_list = &registered_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");
-}