Fix issue agent servers connect too late

Util/CarlaServer/source/carla/server/CarlaServer.cpp

@@ -87,14 +87,15 @@ int32_t carla_write_episode_ready(
       CarlaServerPtr self,
       const carla_episode_ready &values,
       const uint32_t timeout) {
-  auto result = Cast(self)->Write(values);
   if (values.ready) {
     Cast(self)->StartAgentServer();
   } else {
     log_error("start agent server cancelled: episode_ready = false");
   }
   error_code ec = errc::timed_out();
+  auto result = Cast(self)->Write(values);
   future::wait_and_get(result, ec, timeout_t::milliseconds(timeout));
+  Cast(self)->ResetProtocol();
   return ec.value();
 }
 

Util/CarlaServer/source/carla/server/WorldServer.cpp

@@ -103,7 +103,6 @@ namespace server {
 
   void WorldServer::StartAgentServer() {
     _agent_server = std::make_unique<AgentServer>(_encoder, _port + 1u, _port + 2u, _timeout);
-    ResetProtocol();
   }
 
   void WorldServer::KillAgentServer() {

Util/CarlaServer/source/carla/server/WorldServer.h

@@ -36,7 +36,7 @@ namespace server {
     std::future<error_code> Write(const carla_episode_ready &episode_ready);
 
     /// This assumes you have entered the loop of write measurements, read
-    /// control. Automatically resets the protocol to start over.
+    /// control.
     void StartAgentServer();
 
     AgentServer *GetAgentServer() {
@@ -45,6 +45,8 @@ namespace server {
 
     void KillAgentServer();
 
+    void ResetProtocol();
+
   private:
 
     struct Protocol {
@@ -57,8 +59,6 @@ namespace server {
       WriteTask<carla_episode_ready> episode_ready;
     };
 
-    void ResetProtocol();
-
     void ExecuteProtocol(Protocol &&protocol);
 
     uint32_t _port;