From 53e11ccbede4299b9215713ada9ab3bbb263eb73 Mon Sep 17 00:00:00 2001
From: Xisco Bosch <fbosch@cvc.uab.es>
Date: Fri, 24 Mar 2017 11:31:28 +0100
Subject: [PATCH] Reset & restart

---
 .../carla/server/CarlaCommunication.cpp       | 19 +++++++++++--------
 .../carla/thread/AsyncReadWriteJobQueue.h     |  6 ++----
 .../source/carla/thread/AsyncReaderJobQueue.h | 12 ++++--------
 .../source/carla/thread/AsyncWriterJobQueue.h |  8 --------
 .../source/carla/thread/ThreadSafeQueue.h     |  2 +-
 .../CarlaServer/source/test/async_server.cpp  | 10 +++++-----
 6 files changed, 23 insertions(+), 34 deletions(-)

diff --git a/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp b/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp
index 26293fd68..6ff3b1f80 100644
--- a/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp
+++ b/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp
@@ -136,13 +136,13 @@ namespace server {
       //server.writeString("world", error);
 
        
-      //Scene demo_scene;
-      //if (demo_scene.ParseFromString(message)){
-        //std::cout << "POSSIBLE POSITIONS"<< std::endl;
-        //for (int i=0; i<demo_scene.position_size(); ++i){
-        //  std::cout << "   x: " << demo_scene.position(i).pos_x() << " y: " << demo_scene.position(i).pos_y() << std::endl;
-        //}
-      //}
+      Scene demo_scene;
+      if (demo_scene.ParseFromString(message)){
+        std::cout << "POSSIBLE POSITIONS THREAD"<< std::endl;
+        for (int i=0; i<demo_scene.position_size(); ++i){
+          std::cout << "   x: " << demo_scene.position(i).pos_x() << " y: " << demo_scene.position(i).pos_y() << std::endl;
+        }
+      }
 
       if (error) {
         logTCPError("Failed to send world", error);
@@ -266,8 +266,10 @@ namespace server {
     _proto -> LoadScene(scene, values);
 
     auto message = std::make_unique<std::string>();
-    if (scene.SerializeToString(message.get()))
+
+    if (scene.SerializeToString(message.get())){
       _worldThread.push(std::move(message));
+    }
 
   }
 
@@ -320,6 +322,7 @@ namespace server {
 
   bool CarlaCommunication::tryReadRequestNewEpisode(){
     std::unique_ptr <std::string> request = _worldThread.tryPop();
+    
     if (request == nullptr) return false;
 
     RequestNewEpisode reqEpisode;
diff --git a/Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h b/Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h
index 5baa28818..2b6c502e6 100644
--- a/Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h
+++ b/Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h
@@ -51,7 +51,8 @@ namespace thread {
     }
 
     void push(std::unique_ptr<R> item) {
-      _readQueue.push(std::move(item));
+      
+      if (item != nullptr) _readQueue.push(std::move(item));
     }
 
     void reconnect(){
@@ -78,9 +79,6 @@ namespace thread {
         _restart = false; 
         _readQueue.canWait(true);
         while (!_restart && !_done) {
-
-          std::cout << "1" << std::endl;
-
           auto value = _readQueue.wait_and_pop();
           if (value != nullptr) {
             _readJob(*value);
diff --git a/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h b/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h
index b79482c03..e151b568c 100644
--- a/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h
+++ b/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h
@@ -58,21 +58,17 @@ namespace thread {
   private:
     void workerThread() {
       while (!_done){
-        std::cout << "2.1" << std::endl;
     		_connectionJob();
         _restart = false;
         _queue.canWait(true);
         while (!_restart && !_done) {
-
-          std::cout << "2.2" << std::endl;
-
           auto value = _queue.wait_and_pop();
-          if (value != nullptr) _job(*value);
-    		  //Sleep(10);
+          if (value != nullptr) {
+            _job(*value);
+    		  }//Sleep(10);
         }
-      }
 
-          std::cout << "2.3" << std::endl;
+      }
     }
 
     std::atomic_bool _done;
diff --git a/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h b/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h
index eff25df74..d495a659a 100644
--- a/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h
+++ b/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h
@@ -57,22 +57,14 @@ namespace thread {
 
     void workerThread() {
       while (!_done){
-                  std::cout << "3.1" << std::endl;
-
 		    _connectJob();
         _restart = false;
         _queue.canWait(true);
         while (!_restart && !_done) {
-
-          std::cout << "3.2" << std::endl;
-
           _queue.push(std::move(_job()));
 		      //Sleep(10);
         }
       }
-
-      std::cout << "3.3" << std::endl;
-
     }
 
     std::atomic_bool _done;
diff --git a/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h b/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h
index 1bda8db3d..c1b881cb4 100644
--- a/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h
+++ b/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h
@@ -29,6 +29,7 @@ namespace thread {
       //_queue.push(new_value);
       //_condition.notify_one();
       _value = std::move(new_value);
+      _condition.notify_one();
     }
 
     void canWait(bool wait){
@@ -42,7 +43,6 @@ namespace thread {
       _condition.wait(lock, [this]() {
         return _value != nullptr || !_canWait;
       });
-
       return std::move(_value);
     }
 
diff --git a/Source/CarlaServer/source/test/async_server.cpp b/Source/CarlaServer/source/test/async_server.cpp
index c1088b64c..2c0ec8b67 100644
--- a/Source/CarlaServer/source/test/async_server.cpp
+++ b/Source/CarlaServer/source/test/async_server.cpp
@@ -125,10 +125,10 @@ int main(int argc, char *argv[]) {
           const std::array<float, 16u> pMatrix = {{ 10.0 }};
           sceneValues.projection_matrices.push_back(pMatrix);
 
-           std::cout << "POSSIBLE POSITIONS 5"<< std::endl;
+         /*  std::cout << "POSSIBLE POSITIONS "<< std::endl;
            for (int i=0; i<sceneValues.possible_positions.size(); ++i){
             std::cout << "   x: " << sceneValues.possible_positions[i].x << " y: " << sceneValues.possible_positions[i].y << std::endl;
-          }
+          }*/
 
 
           if (!server.sendSceneValues(sceneValues)) std::cerr << "ERROR while sending SceneValues" << std::endl;
@@ -153,7 +153,7 @@ int main(int argc, char *argv[]) {
 
         while (true) {
           float steer, gas;
-          /*bool newEpisode = false;
+          bool newEpisode = false;
           if (!server.newEpisodeRequested(newEpisode)){
             std::cerr << "ERROR while checking for newEpisode request" << std::endl;
             break;
@@ -184,7 +184,7 @@ int main(int argc, char *argv[]) {
             }
 
           }else {
-*/
+
             bool error = false, readed = false;
             if (!server.tryReadControl(steer, gas, readed)){
               std::cerr << "ERROR while reading Control" << std::endl;
@@ -201,7 +201,7 @@ int main(int argc, char *argv[]) {
               break;
             }
             
-          //}
+          }
         }
 
       std::cout << " -----  RESTARTING -----" <<  std::endl;