diff --git a/Source/CarlaServer/source/carla/CarlaServer.cpp b/Source/CarlaServer/source/carla/CarlaServer.cpp index 7360cbc0b..b35f4f1de 100644 --- a/Source/CarlaServer/source/carla/CarlaServer.cpp +++ b/Source/CarlaServer/source/carla/CarlaServer.cpp @@ -1,9 +1,13 @@ #include "CarlaServer.h" -#include +#include namespace carla { + Image::Image() {} + + Image::~Image() {} + Reward_Values::Reward_Values() {} Reward_Values::~Reward_Values() {} @@ -12,10 +16,13 @@ namespace carla { Scene_Values::~Scene_Values() {} - class CarlaServer::Pimpl : public carla::server::Server { + class CarlaServer::Pimpl { public: - Pimpl(uint32_t worldPort, uint32_t writePort, uint32_t readPort) : - carla::server::Server(worldPort, writePort, readPort) {} + + template + Pimpl(Args&&... args) : communication(std::forward(args)...) {} + + carla::server::CarlaCommunication communication; }; CarlaServer::CarlaServer(uint32_t writePort, uint32_t readPort, uint32_t worldPort) : @@ -23,69 +30,77 @@ namespace carla { CarlaServer::~CarlaServer() {} - bool CarlaServer::init(uint32_t LevelCount) { - if (!worldConnected() && !clientConnected() && !serverConnected()) return false; - _pimpl->sendWorld(static_cast(Mode::NUMBER_OF_MODES), LevelCount); + bool CarlaServer::init(uint32_t levelCount) { + if (!worldConnected() && !clientConnected() && !serverConnected()) + return false; + _pimpl->communication.sendWorld(levelCount); return true; } - bool CarlaServer::tryReadSceneInit(Mode &mode, uint32_t &scene, bool &readed) { - if (!worldConnected()) return false; - readed = _pimpl->tryReadSceneInit(mode, scene); + bool CarlaServer::tryReadSceneInit(uint32_t &scene, bool &readed) { + if (!worldConnected()) + return false; + readed = _pimpl->communication.tryReadSceneInit(scene); return true; } bool CarlaServer::tryReadEpisodeStart(uint32_t &startIndex, uint32_t &endIndex, bool &readed) { - if (!worldConnected()) return false; - readed = _pimpl->tryReadEpisodeStart(startIndex, endIndex); + if (!worldConnected()) + return false; + readed = _pimpl->communication.tryReadEpisodeStart(startIndex, endIndex); return true; } bool CarlaServer::tryReadControl(float &steer, float &throttle, bool &readed) { - if (!clientConnected()) return false; - readed = _pimpl->tryReadControl(steer, throttle); + if (!clientConnected()) + return false; + readed = _pimpl->communication.tryReadControl(steer, throttle); return true; } bool CarlaServer::newEpisodeRequested(bool &newEpisode) { - if (!worldConnected()) return false; - newEpisode = _pimpl->tryReadRequestNewEpisode(); + if (!worldConnected()) + return false; + newEpisode = _pimpl->communication.tryReadRequestNewEpisode(); return true; } bool CarlaServer::sendReward(Reward_Values *values) { std::unique_ptr ptr(values); - if (!serverConnected()) return false; - _pimpl->sendReward(std::move(ptr)); + if (!serverConnected()) + return false; + _pimpl->communication.sendReward(std::move(ptr)); return true; } bool CarlaServer::sendSceneValues(const Scene_Values &values) { - if (!worldConnected()) return false; - _pimpl->sendSceneValues(values); + if (!worldConnected()) + return false; + _pimpl->communication.sendScene(values); return true; } bool CarlaServer::sendEndReset() { - if (!worldConnected()) return false; - _pimpl->sendEndReset(); + if (!worldConnected()) + return false; + _pimpl->communication.sendReset(); return true; } bool CarlaServer::worldConnected(){ - return _pimpl->worldConnected() && !_pimpl->needsRestart(); + return _pimpl->communication.worldConnected() && !_pimpl->communication.needsRestart(); } bool CarlaServer::clientConnected(){ - return _pimpl->clientConnected() && !_pimpl->needsRestart(); + return _pimpl->communication.clientConnected() && !_pimpl->communication.needsRestart(); } bool CarlaServer::serverConnected(){ - return _pimpl->serverConnected() && !_pimpl->needsRestart(); + return _pimpl->communication.serverConnected() && !_pimpl->communication.needsRestart(); } bool CarlaServer::needsRestart(){ - return _pimpl->needsRestart(); + return _pimpl->communication.needsRestart(); } } // namespace carla diff --git a/Source/CarlaServer/source/carla/CarlaServer.h b/Source/CarlaServer/source/carla/CarlaServer.h index fc307cabd..6b9fef1ee 100644 --- a/Source/CarlaServer/source/carla/CarlaServer.h +++ b/Source/CarlaServer/source/carla/CarlaServer.h @@ -27,12 +27,14 @@ namespace carla { uint8_t A; }; - enum ImageType{ + enum ImageType { IMAGE, DEPTH, }; struct Image { + Image(); + ~Image(); std::vector image; ImageType type; uint32_t width, height; @@ -61,14 +63,8 @@ namespace carla { float intersect_other_lane; /// Percentage of the car off-road. float intersect_offroad; - /// Width and height of the images. - //uint32_t image_width, image_height; - /// RGB images. + /// Captured images. std::vector images; - //std::vector image_rgb_1; - /// Depth images. - //std::vector image_depth_0; - //std::vector image_depth_1; }; struct Scene_Values { @@ -76,18 +72,10 @@ namespace carla { ~Scene_Values(); /// Possible world positions to spawn the player. std::vector possible_positions; - /// Projection matrices of the cameras (1 in mode mono, 2 in stereo). + /// Projection matrices of the cameras. std::vector> projection_matrices; }; - enum class Mode : int8_t { - MONO, - STEREO, - - NUMBER_OF_MODES, - INVALID = -1 - }; - /// Asynchronous TCP server. Uses three ports, one for sending messages /// (write), one for receiving messages (read), and one for world level /// commands (world). @@ -109,13 +97,11 @@ namespace carla { /// Initialize the server. /// /// @param LevelCount Number of levels available. - bool init(uint32_t LevelCount); + bool init(uint32_t levelCount); /// Try to read if the client has selected an scene and mode. Return false /// if the queue is empty. - /// - /// If returned mode INVALID, ignore scene. - bool tryReadSceneInit(Mode &mode, uint32_t &scene, bool &readed); + bool tryReadSceneInit(uint32_t &scene, bool &readed); /// Try to read if the client has selected an end & start point. Return /// false if the queue is empty. diff --git a/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp b/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp index a3d486bdf..7bc863632 100644 --- a/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp +++ b/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp @@ -10,16 +10,6 @@ namespace server { // -- Static methods --------------------------------------------------------- - std::mutex _generalMutex; - - static Mode getMode(int modeInt) { - switch (modeInt) { - case 0: return Mode::MONO; - case 1: return Mode::STEREO; - default: return Mode::INVALID; - } - } - template static void logTCPError(const std::string &text, const ERROR_CODE &errorCode) { std::cerr << "CarlaConnection - TCP Server: " << text << ": " << errorCode.message() << std::endl; @@ -141,9 +131,11 @@ namespace server { } static void ReconnectAll(CarlaCommunication &communication) { - std::lock_guard lock(_generalMutex); + /// @todo This blocks every CarlaCommunication with a single mutex. + static std::mutex MUTEX; + std::lock_guard lock(MUTEX); - if (!communication.NeedsRestart()) { + if (!communication.needsRestart()) { std::cout << " ---- RECONNECT ALL ...." << std::endl; @@ -165,7 +157,7 @@ namespace server { communication.getClientThread().restart(); } - communication.Restart(); + communication.restart(); } } @@ -174,7 +166,6 @@ namespace server { _server(writePort), _client(readPort), _needsRestart(false), - _mode(Mode::MONO), _proto(std::make_unique(this)), _worldThread{ [this]() { return worldReceiveThread(this->_world, this->_worldThread); }, @@ -213,13 +204,14 @@ namespace server { return true; } - void CarlaCommunication::sendWorld(const uint32_t modes, const uint32_t scenes) { + void CarlaCommunication::sendWorld(const uint32_t scenes) { //ClearThreads _worldThread.clear(); _clientThread.clear(); _serverThread.clear(); World world; + constexpr int modes = 0; /// @todo #18 _proto->LoadWorld(world, modes, scenes); auto message = std::make_unique(); @@ -253,8 +245,7 @@ namespace server { } } - bool CarlaCommunication::tryReadSceneInit(Mode &mode, uint32_t &scene) { - mode = Mode::INVALID; + bool CarlaCommunication::tryReadSceneInit(uint32_t &scene) { scene = 0u; std::unique_ptr info = _worldThread.tryPop(); @@ -264,11 +255,7 @@ namespace server { if (!sceneInit.ParseFromString(*info)) { return false; } - mode = getMode(sceneInit.mode()); scene = sceneInit.scene(); - - _mode = mode; - return true; } @@ -303,17 +290,14 @@ namespace server { void CarlaCommunication::restartServer() { _server.close(); - //_server = TCPServer(_serverPort); } void CarlaCommunication::restartWorld() { _world.close(); - //_world = TCPServer(_worldPort); } void CarlaCommunication::restartClient() { _client.close(); - //_client = TCPServer(_clientPort); } void CarlaCommunication::checkRestart() { @@ -347,15 +331,11 @@ namespace server { return _server.Connected(); } - Mode CarlaCommunication::GetMode() { - return _mode; - } - - bool CarlaCommunication::NeedsRestart() { + bool CarlaCommunication::needsRestart() { return _needsRestart; } - void CarlaCommunication::Restart() { + void CarlaCommunication::restart() { _needsRestart = true; } diff --git a/Source/CarlaServer/source/carla/server/CarlaCommunication.h b/Source/CarlaServer/source/carla/server/CarlaCommunication.h index 5a3c71ffb..d8111cee9 100644 --- a/Source/CarlaServer/source/carla/server/CarlaCommunication.h +++ b/Source/CarlaServer/source/carla/server/CarlaCommunication.h @@ -36,10 +36,9 @@ namespace server { void sendReset(); - //void sendWorld(const World &world); - void sendWorld(const uint32_t modes, const uint32_t scenes); + void sendWorld(const uint32_t scenes); - bool tryReadSceneInit(Mode &mode, uint32_t &scene); + bool tryReadSceneInit(uint32_t &scene); bool tryReadEpisodeStart(uint32_t &start_index, uint32_t &end_index); @@ -63,10 +62,9 @@ namespace server { std::mutex getGeneralMutex(); - Mode GetMode(); + bool needsRestart(); - bool NeedsRestart(); - void Restart(); + void restart(); private: @@ -78,8 +76,6 @@ namespace server { std::atomic_bool _needsRestart; - std::atomic _mode; - const std::unique_ptr _proto; thread::AsyncReadWriteJobQueue< std::string, std::string> _worldThread; diff --git a/Source/CarlaServer/source/carla/server/Server.cpp b/Source/CarlaServer/source/carla/server/Server.cpp deleted file mode 100644 index eda16c97f..000000000 --- a/Source/CarlaServer/source/carla/server/Server.cpp +++ /dev/null @@ -1,88 +0,0 @@ -// CARLA, Copyright (C) 2017 Computer Vision Center (CVC) - -#include "Server.h" - -#include "CarlaCommunication.h" -#include "carla_protocol.pb.h" - -#include -#include - -namespace carla { -namespace server { - - - - // -- CarlaServer ------------------------------------------------------------ - - Server::Server(uint32_t worldPort, uint32_t writePort, uint32_t readPort) : - _communication(std::make_unique(worldPort, writePort, readPort)){} - - Server::~Server() { - } - - void Server::sendReward( std::unique_ptr values) { - //Reward reward; - //_proto->LoadReward(reward, values); - //_communication->sendReward(reward); - _communication->sendReward(std::move(values)); - } - - void Server::sendSceneValues(const Scene_Values &values) { - _communication->sendScene(values); - } - - void Server::sendEndReset() { - - _communication->sendReset(); - } - - void Server::sendWorld(const uint32_t modes, const uint32_t scenes) { - //World world; - //_proto->LoadWorld(world, modes, scenes); - //_communication->sendWorld(world); - _communication->sendWorld(modes, scenes); - } - - bool Server::tryReadControl(float &steer, float &gas) { - return _communication->tryReadControl(steer, gas); - } - - bool Server::tryReadSceneInit(Mode &mode, uint32_t &scene) { - return _communication->tryReadSceneInit(mode, scene); - } - - bool Server::tryReadEpisodeStart(uint32_t &start_index, uint32_t &end_index) { - return _communication->tryReadEpisodeStart(start_index, end_index); - } - - bool Server::tryReadRequestNewEpisode(){ - return _communication->tryReadRequestNewEpisode(); - } - - Mode Server::GetMode() const { - return _communication->GetMode(); - } - - void Server::SetScene(int scene) { - _scene = scene; - } - - bool Server::worldConnected() const{ - return _communication->worldConnected(); - } - - bool Server::clientConnected() const{ - return _communication->clientConnected(); - } - - bool Server::serverConnected() const{ - return _communication->serverConnected(); - } - - bool Server::needsRestart() const { - return _communication->NeedsRestart(); - } - -} // namespace server -} // namespace carla diff --git a/Source/CarlaServer/source/carla/server/Server.h b/Source/CarlaServer/source/carla/server/Server.h deleted file mode 100644 index 64d63ed2a..000000000 --- a/Source/CarlaServer/source/carla/server/Server.h +++ /dev/null @@ -1,88 +0,0 @@ -// CARLA, Copyright (C) 2017 Computer Vision Center (CVC) - -#pragma once - -#include -#include - -#include -#include -#include -#include -#include - -namespace carla { -namespace server { - - class CarlaCommunication; - - /// Asynchronous TCP server. Uses two ports, one for sending messages (write) - /// and one for receiving messages (read). - /// - /// Writing and reading are executed in two different threads. Each thread has - /// its own queue of messages. - /// - /// Note that a new socket is created for every connection (every write and - /// read). - class Server : private NonCopyable { - public: - - /// Starts two threads for writing and reading. - explicit Server(uint32_t worldPort, uint32_t writePort, uint32_t readPort); - - ~Server(); - - ///// Send values of the current player status - void sendReward(std::unique_ptr values); - - //// Send the values of the generated scene - void sendSceneValues(const Scene_Values &values); - - //// Send a signal to the client to notify that the car is ready - void sendEndReset(); - - void sendWorld(uint32_t modes, uint32_t scenes); - - ///// Try to read the response of the client. Return false if the queue - ///// is empty. - bool tryReadControl(float &steer, float &gas); - - ////Try to read if the client has selected an scene and mode. Return false if the queue is empty - bool tryReadSceneInit(Mode &mode, uint32_t &scene); - - ////Try to read if the client has selected an end & start point. Return false if the queue is empty - bool tryReadEpisodeStart(uint32_t &start_index, uint32_t &end_index); - - - bool tryReadRequestNewEpisode(); - - void setMode(Mode mode); - - Mode GetMode() const; - - void SetScene(int scene); - - int GetScene() const; - - bool worldConnected() const; - - bool clientConnected() const; - - bool serverConnected() const; - - bool needsRestart() const; - - private: - - //std::mutex _mutex; - - - std::atomic_int _scene; - - const std::unique_ptr _communication; - - //const std::unique_ptr _proto; - }; - -} // namespace server -} // namespace carla diff --git a/Source/CarlaServer/source/test/async_server.cpp b/Source/CarlaServer/source/test/async_server.cpp index 2771aa0a4..baef9b46d 100644 --- a/Source/CarlaServer/source/test/async_server.cpp +++ b/Source/CarlaServer/source/test/async_server.cpp @@ -43,42 +43,41 @@ static std::vector makeImage(uint32_t width, uint32_t height) { return image; } -std::unique_ptr makeReward(){ - +std::unique_ptr makeReward() { auto reward = std::make_unique(); const uint32_t imageWidth = 512u; - const uint32_t imageHeight = 512u; + const uint32_t imageHeight = 512u; - reward->player_location = {1.0f, 1.0f}; - reward->player_orientation = {1.0f, 0.0f, 0.0f}; - reward->player_acceleration = {1.0f, 0.0f, 0.0f}; - reward->forward_speed = 100.0f; - reward->collision_general = 10.0f; - reward->collision_pedestrian = 10.0f; - reward->collision_car = 10.0f; - reward->intersect_other_lane = 0.5f; - reward->intersect_offroad = 0.5f; - - for (int i = 0; i < 4; ++i){ - carla::Image img; - img.image = makeImage(imageWidth, imageHeight); - img.width = imageWidth; - img.height = imageHeight; - if (i < 2) img.type = carla::IMAGE; - else img.type = carla::DEPTH; - reward->images.push_back(img); - } + reward->player_location = {1.0f, 1.0f}; + reward->player_orientation = {1.0f, 0.0f, 0.0f}; + reward->player_acceleration = {1.0f, 0.0f, 0.0f}; + reward->forward_speed = 100.0f; + reward->collision_general = 10.0f; + reward->collision_pedestrian = 10.0f; + reward->collision_car = 10.0f; + reward->intersect_other_lane = 0.5f; + reward->intersect_offroad = 0.5f; + + for (int i = 0; i < 4; ++i) { + carla::Image img; + img.image = makeImage(imageWidth, imageHeight); + img.width = imageWidth; + img.height = imageHeight; + if (i < 2) img.type = carla::IMAGE; + else img.type = carla::DEPTH; + reward->images.push_back(img); + } /* - reward->image_rgb_0 = makeImage(imageWidth, imageHeight); - reward->image_rgb_1 = makeImage(imageWidth, imageHeight); - reward->image_depth_0 = makeImage(imageWidth, imageHeight); - reward->image_depth_1 = makeImage(imageWidth, imageHeight); + reward->image_rgb_0 = makeImage(imageWidth, imageHeight); + reward->image_rgb_1 = makeImage(imageWidth, imageHeight); + reward->image_depth_0 = makeImage(imageWidth, imageHeight); + reward->image_depth_1 = makeImage(imageWidth, imageHeight); */ - static decltype(carla::Reward_Values::timestamp) timestamp = 0u; - reward->timestamp = timestamp++; + static decltype(carla::Reward_Values::timestamp) timestamp = 0u; + reward->timestamp = timestamp++; - return reward; + return reward; } @@ -91,67 +90,57 @@ int main(int argc, char *argv[]) { const uint32_t worldPort = toInt(argv[1u]); const uint32_t writePort = toInt(argv[2u]); const uint32_t readPort = toInt(argv[3u]); - // const uint32_t mode = toInt(argv[4u]); - // const uint32_t scene = toInt(argv[5u]); // This already starts the two threads. carla::CarlaServer server(writePort, readPort, worldPort); // Let's simulate the game loop. - - - - for (;;){ - if (server.init(1u)){ + for (;;) { + if (server.init(1u)) { { - carla::Mode mode; uint32_t scene; bool error = false, readed = false; - do{ - error = !server.tryReadSceneInit(mode, scene, readed); - }while(!readed && !error); + do { + error = !server.tryReadSceneInit(scene, readed); + } while (!readed && !error); - if (error) std::cerr << "ERROR while sending SceneValues" << std::endl; - else std::cout << "Received: mode = " << (mode == carla::Mode::MONO ? "MONO" : "STEREO") - << ", scene = " << scene << std::endl; + if (error) { + std::cerr << "ERROR while sending SceneValues" << std::endl; + } + else std::cout << "Received: scene = " << scene << std::endl; } + carla::Scene_Values sceneValues; - carla::Scene_Values sceneValues; + sceneValues.possible_positions.push_back({0.0f, 0.0f}); + sceneValues.possible_positions.push_back({1.0f, 2.0f}); + sceneValues.possible_positions.push_back({3.0f, 4.0f}); + const std::array pMatrix = {{ 10.0 }}; + sceneValues.projection_matrices.push_back(pMatrix); + if (!server.sendSceneValues(sceneValues)) { + std::cerr << "ERROR while sending SceneValues" << std::endl; + } - sceneValues.possible_positions.push_back({0.0f, 0.0f}); - sceneValues.possible_positions.push_back({1.0f, 2.0f}); - sceneValues.possible_positions.push_back({3.0f, 4.0f}); - const std::array pMatrix = {{ 10.0 }}; - sceneValues.projection_matrices.push_back(pMatrix); - - /* std::cout << "POSSIBLE POSITIONS "<< std::endl; - for (int i=0; i Start: " << startPoint << " End: " << endPoint << " <--" << std::endl; server.sendEndReset(); } - }else { + } else { bool readed = false; if (!server.tryReadControl(steer, gas, readed)){ std::cerr << "ERROR while reading Control" << std::endl; break; - } - else if (readed) + } else if (readed) { std::cout << "CONTROL --> gas: " << gas << " steer: " << steer << std::endl; - + } if (!server.sendReward(makeReward().release())) { std::cerr << "ERROR while sending Reward" << std::endl; break; } - } } - - std::cout << " ----- RESTARTING -----" << std::endl; + std::cout << " ----- RESTARTING -----" << std::endl; } } } catch (const std::exception &e) {