Merge pull request #274 from vimalrajliya/traffic_manager

Traffic Manager API

traffic_manager/source/include/ActorStateCallable.hpp

@@ -15,7 +15,7 @@ class ActorStateCallable: public PipelineCallable
     ActorStateMessage* actorstate_msg;
 
 public:
-    ActorStateCallable(ActorStateMessage* actorstate_msg,
+    ActorStateCallable(
         std::queue<PipelineMessage>* input_queue,
         std::queue<PipelineMessage>* output_queue,
         std::mutex& read_mutex,

traffic_manager/source/include/ActorStateStage.hpp

@@ -18,7 +18,7 @@ class ActorStateStage: public PipelineStage
     ActorStateMessage* actorstate_msg;
 
 public:
-    ActorStateStage(ActorStateMessage* actorstate_msg, int output_buffer_size,
+    ActorStateStage(int output_buffer_size,
         std::queue<PipelineMessage>* input_queue,
         std::queue<PipelineMessage>* output_queue);
     ~ActorStateStage();

traffic_manager/source/include/CarlaDataAccessLayer.hpp

@@ -1,6 +1,4 @@
 // Data access layer to carla server
-#pragma once
-
 #include <vector>
 #include <array>
 

traffic_manager/source/include/FeederMessage.hpp

@@ -1,6 +1,4 @@
 //Declaration of FeederMessage class members
-#pragma once
-
 #include "PipelineMessage.hpp"
 
 namespace traffic_manager{

traffic_manager/source/include/InMemoryMap.hpp

@@ -1,6 +1,4 @@
 // Class for in memory representation of descrete map
-#pragma once
-
 #include <map>
 #include <cmath>
 #include "carla/Memory.h"

traffic_manager/source/include/PipelineCallable.hpp

@@ -1,7 +1,5 @@
 // Declaration for base class of all pipeline threads
-
 #pragma once
-
 #include <queue>
 #include <mutex>
 #include "PipelineMessage.hpp"

traffic_manager/source/include/PipelineMessage.hpp

@@ -1,6 +1,5 @@
 // Declaration for a common base class to all messages between pipeline stages
 #pragma once
-
 #include "carla/client/Actor.h"
 
 namespace traffic_manager {

traffic_manager/source/include/RegisteredActorMessage.hpp

@@ -1,5 +1,5 @@
 // Declaration of RegisteredActorMessage class
-#pragma once
+# pragma once
 #include "PipelineMessage.hpp"
 
 namespace traffic_manager
@@ -13,4 +13,4 @@ namespace traffic_manager
         void addActor(carla::SharedPtr<carla::client::Actor> actor);
         void removeActor(carla::SharedPtr<carla::client::Actor> actor);
     };
-}
+}   

traffic_manager/source/include/SimpleWaypoint.hpp

@@ -1,5 +1,4 @@
 // A simple waypoint class
-#pragma once
 #include "carla/Memory.h"
 #include "carla/geom/Vector3D.h"
 #include "carla/geom/Location.h"

traffic_manager/source/src/ActorStateCallable.cpp

@@ -4,12 +4,12 @@
 
 namespace traffic_manager {
 
-    ActorStateCallable::ActorStateCallable(ActorStateMessage* actorstate_msg,
+    ActorStateCallable::ActorStateCallable(
         std::queue<PipelineMessage>* input_queue,
         std::queue<PipelineMessage>* output_queue,
         std::mutex& read_mutex,
         std::mutex& write_mutex,
-        int output_buffer_size):actorstate_msg(actorstate_msg),
+        int output_buffer_size):
         PipelineCallable(input_queue,output_queue,read_mutex,write_mutex,output_buffer_size){}
 
     ActorStateCallable::~ActorStateCallable(){}

traffic_manager/source/src/ActorStateStage.cpp

@@ -3,11 +3,11 @@
 
 namespace traffic_manager {
 
-    ActorStateStage::ActorStateStage(ActorStateMessage* actorstate_msg, int output_buffer_size,
+    ActorStateStage::ActorStateStage(int output_buffer_size,
         std::queue<PipelineMessage>* input_queue,
-        std::queue<PipelineMessage>* output_queue):actorstate_msg(actorstate_msg),
+        std::queue<PipelineMessage>* output_queue):
         PipelineStage(1, output_buffer_size,
-        input_queue,output_queue,actorstate_msg){}
+        input_queue,output_queue,NULL){}
 
     ActorStateStage::~ActorStateStage(){}
 
@@ -25,10 +25,10 @@ namespace traffic_manager {
 
     void ActorStateStage::createPipelineCallables()
     {
-        ActorStateCallable* actorstate_callable = new ActorStateCallable(actorstate_msg, 
+        ActorStateCallable* actorstate_callable = new ActorStateCallable( 
         input_queue, output_queue, 
         read_mutex, write_mutex, 
         output_buffer_size);
-       threadCallables.push_back(actorstate_callable);
+        threadCallables.push_back(actorstate_callable);
     }
 }

traffic_manager/source/src/PipelineCallable.cpp

@@ -33,7 +33,11 @@ void PipelineCallable::writeQueue(PipelineMessage message) {
 void PipelineCallable::run() {
     while(true)
     {
-        PipelineMessage in_message;
+        PipelineMessage in_message;        
+        if (input_queue!=NULL && input_queue->empty())
+        {
+            continue;
+        }
         if (input_queue!=NULL && !input_queue->empty())
             in_message = readQueue();
         auto out_message = action(in_message);

traffic_manager/source/test/main.cpp

@@ -5,7 +5,6 @@
 #include "CarlaDataAccessLayer.hpp"
 #include "InMemoryMap.hpp"
 #include "carla/client/Waypoint.h"
-//#include "ActorReadStage.hpp"
 #include "carla/client/ActorList.h"
 #include "carla/client/Actor.h"
 #include "RegisteredActorMessage.hpp"
@@ -15,15 +14,10 @@
 #include "ActorStateMessage.hpp"
 #include "ActorStateStage.hpp"
 
-void test(traffic_manager::RegisteredActorMessage registerActorObj,
-    int pull, int buffer_size,
-    std::queue <traffic_manager::PipelineMessage> input_queue,
-    std::queue <traffic_manager::PipelineMessage> out_queue);
+void test_get_topology(carla::SharedPtr<carla::client::Map> world_map);
+void test_feeder_stage(carla::SharedPtr<carla::client::ActorList> actor_list);
 
-void test2(traffic_manager::ActorStateMessage actor_state_obj,
-    int pull, int buffer_size,
-    std::queue <traffic_manager::PipelineMessage> input_queue,
-    std::queue <traffic_manager::PipelineMessage> out_queue);
+void test_actor_state_stage(carla::SharedPtr<carla::client::ActorList> actor_list);
 
 int main()
 {   
@@ -33,33 +27,47 @@ int main()
     auto world_map = world.GetMap();
     auto actorList = world.GetActors();
     auto vehicle_list = actorList->Filter("vehicle.*");
-    traffic_manager::ActorStateMessage actor_state_obj;
-    traffic_manager::RegisteredActorMessage registerActorObj;
-    //auto registerActorObj.shared_actor_list;
-    for(auto it = vehicle_list->begin(); it != vehicle_list->end(); it++)
-    {
-        registerActorObj.shared_actor_list.push_back(*it);
-    }
 
+    test_get_topology(world_map);
+    // test_feeder_stage(vehicle_list);
+    test_actor_state_stage(vehicle_list);
 
-    std::queue <traffic_manager::PipelineMessage> input_queue;
-    std::queue <traffic_manager::PipelineMessage> out_queue;
+    return 0;
+}
 
-    //test(registerActorObj, 1, 20, input_queue, out_queue);
-    test2(actor_state_obj, 1, 20,input_queue,out_queue);
-    //traffic_manager::FeederStage feeder_stage(&registerActorObj, 20, &input_queue, &out_queue);
-
+void test_get_topology(carla::SharedPtr<carla::client::Map> world_map) {
+
+    auto dao = traffic_manager::CarlaDataAccessLayer(world_map);
+    auto topology = dao.getTopology();
+
+    typedef std::vector<
+        std::pair<
+            carla::SharedPtr<carla::client::Waypoint>,
+            carla::SharedPtr<carla::client::Waypoint>
+        >
+    > toplist;
+    for(toplist::iterator it = topology.begin(); it != topology.end(); it++) {
+        auto wp1 = it->first;
+        auto wp2 = it->second;
+        std::cout << "Segment end road IDs : " << wp1->GetRoadId() << " -- " << wp2->GetRoadId() << std::endl;
+    }
 }
 
-void test(traffic_manager::RegisteredActorMessage registerActorObj,
-    int pull, int buffer_size,
-    std::queue <traffic_manager::PipelineMessage> input_queue,
-    std::queue <traffic_manager::PipelineMessage> out_queue)
+void test_feeder_stage(carla::SharedPtr<carla::client::ActorList> actor_list)
 {
-    traffic_manager::FeederStage feeder_stage(&registerActorObj, buffer_size, &input_queue, &out_queue);
+    std::queue <traffic_manager::PipelineMessage> input_queue;
+    std::queue <traffic_manager::PipelineMessage> out_queue;
+
+    traffic_manager::RegisteredActorMessage registerActorObj;
+    for(auto it = actor_list->begin(); it != actor_list->end(); it++)
+    {
+        registerActorObj.shared_actor_list.push_back(*it);
+    }
+
+    traffic_manager::FeederStage feeder_stage(&registerActorObj, 20, &input_queue, &out_queue);
     feeder_stage.start();
     sleep(1);
-    //std::cout <<"out_queue size : " <<out_queue.size()<< std::endl;
+
     int count = 20;
     while(!out_queue.empty() && count > 0)
     {
@@ -73,50 +81,46 @@ void test(traffic_manager::RegisteredActorMessage registerActorObj,
     while(true);
 }
 
-void test2(traffic_manager::ActorStateMessage actor_state_obj,
-    int pull, int buffer_size,
-    std::queue <traffic_manager::PipelineMessage> input_queue,
-    std::queue <traffic_manager::PipelineMessage> out_queue)
+void test_actor_state_stage(carla::SharedPtr<carla::client::ActorList> actor_list)
 {
-    traffic_manager::ActorStateStage actorstage_obj(&actor_state_obj, buffer_size, &input_queue, &out_queue);
-    actorstage_obj.start();
-    sleep(1);
+    std::queue <traffic_manager::PipelineMessage> input_queue;
+    std::queue <traffic_manager::PipelineMessage> out_queue;
+    std::queue <traffic_manager::PipelineMessage> in_queue_to_actorstage;
+    std::queue <traffic_manager::PipelineMessage> out_queue_to_actorstage;
 
-     int count = 20;
-    while(!out_queue.empty() && count > 0)
+    traffic_manager::RegisteredActorMessage registerActorObj;
+    for(auto it = actor_list->begin(); it != actor_list->end(); it++)
     {
-        auto out = out_queue.front();
-        out_queue.pop();
-
-        std::cout << "Actor_Type_id " << out_queue.size()<< std::endl;
-        //std::cout << "Actor_id " << out.getActor()->GetId() << std::endl;
-        count--;
+        registerActorObj.shared_actor_list.push_back(*it);
     }
-    while(true);
-
-}
 
+    traffic_manager::FeederStage feeder_stage(&registerActorObj, 20, &input_queue, &out_queue);
+    feeder_stage.start();
+    sleep(1);
 
+    //call_feeder_stage(&registerActorObj, &input_queue, &out_queue);
+    // int count = 20;
+    // while(!in_queue_to_actorstage.empty() && count > 0)
+    // {
+    //     auto out = in_queue_to_actorstage.front();
+    //     in_queue_to_actorstage.pop();
 
+    //     std::cout << "Actor_id " << out.getActor()->GetId() << std::endl;
+    //     count--;
+    // }
 
+    traffic_manager::ActorStateStage actorstage_obj(20, &out_queue, &out_queue_to_actorstage);
+    actorstage_obj.start();
+    sleep(1);
 
+    // int count1 = 20;
+    // while(!out_queue_to_actorstage.empty() && count1 > 0)
+    // {
+    //     auto out = out_queue_to_actorstage.front();
+    //     out_queue_to_actorstage.pop();
 
-// //void test_closest_waypoint(carla::SharedPtr<carla::client::ActorList> vehicle_list, carla::SharedPtr<carla::client::Map> world_map);
-// std::vector<int> getActors(carla::SharedPtr<carla::client::ActorList> _actor_list);
-// int main(){
-//     auto client_conn = carla::client::Client("localhost", 2000);
-//     std::cout<<"Connected with client object : "<<client_conn.GetClientVersion()<<std::endl;
-//     auto world = client_conn.GetWorld();
-//     auto world_map = world.GetMap();
-//     auto actorList = world.GetActors();
-//     auto vehicle_list = actorList->Filter("vehicle.*");
-//     auto topology = world_map->GetTopology();
-//     auto newtopo = traffic_manager::InMemoryMap(topology);
-//     auto all_actors = getActors(vehicle_list);
-//     traffic_manager::RegisteredActorMessage actorobj;
-//     int actorId = actorobj.getActorID();
-//     std::cout << actorId << std::endl;
-//     // auto location = getLocation(vehicle_list);
-//     // auto allwayp = newtopo.getWaypoint(location);
-//     return 0;
-// }
+    //     std::cout << "actor state actor_id " << out.getActor()->GetId() << std::endl;
+    //     count1--;
+    // }
+    while(true);
+}