diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 5ede3adcd72f05ee4acd6b9759d12a15dfa2b3ee..2b11cb8b0e56a43351d472b682eef694aad79c08 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -27,6 +27,8 @@ QGCROOT = $$PWD DebugBuild { DESTDIR = $${OUT_PWD}/debug + #DEFINES += DEBUG + #DEFINES += ROS_BRIDGE_CLIENT_DEBUG } else { DESTDIR = $${OUT_PWD}/release diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 2179b30db460cc0f2649ce9dd487d0d549832e0d..a213b2ff1993d41a21bfd69fc14141086ffe5b21 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -75,7 +75,8 @@ WimaController::WimaController(QObject *parent) , _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName]) , _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName]) , _nemoHeartbeat (0 /*status: not connected*/) - , _fallbackStatus (0 /*status: not connected*/) + , _fallbackStatus (0 /*status: not connected*/) + , _bridgeSetupDone (false) , _pRosBridge (new ROSBridge::ROSBridge()) { // Set up facts. @@ -692,8 +693,7 @@ void WimaController::_checkBatteryLevel() void WimaController::_eventTimerHandler() { static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL); - static EventTicker snakeEventLoopTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL); - static EventTicker rosBridgeTicker(EVENT_TIMER_INTERVAL, 1000); + static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL); static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000); // Battery level check necessary? @@ -713,37 +713,83 @@ void WimaController::_eventTimerHandler() emit WimaController::nemoStatusStringChanged(); } - if (_enableSnake.rawValue().toBool() && rosBridgeTicker.ready()) { + if ( snakeTicker.ready() ) { + if ( _enableSnake.rawValue().toBool() + && _pRosBridge->connected() ) { + if ( !_bridgeSetupDone ) { + qWarning() << "ROS Bridge setup. "; + auto start = std::chrono::high_resolution_clock::now(); + _pRosBridge->start(); + auto end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->start() time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->publish(_snakeOrigin, "/snake/origin"); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->publish(_snakeOrigin, \"/snake/origin\") time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->publish(_snakeTilesLocal, \"/snake/tiles\") time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + + + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->subscribe("/nemo/progress", + /* callback */ [this](JsonDocUPtr pDoc){ + int requiredSize = this->_snakeTilesLocal.polygons().size(); + auto& progress = this->_nemoProgress; + if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress) + || progress.progress().size() != requiredSize ) { + progress.progress().fill(0, requiredSize); + } + + emit WimaController::nemoProgressChanged(); + }); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->subscribe(\"/nemo/progress\" time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->subscribe("/nemo/heartbeat", + /* callback */ [this, &nemoStatusTicker](JsonDocUPtr pDoc){ + if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) { + if ( this->_nemoHeartbeat.status() == this->_fallbackStatus ) + return; + this->_nemoHeartbeat.setStatus(this->_fallbackStatus); + } + + nemoStatusTicker.reset(); + this->_fallbackStatus = -1; /*Timeout*/ + emit WimaController::nemoStatusChanged(); + emit WimaController::nemoStatusStringChanged(); + }); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->subscribe(\"/nemo/heartbeat\" time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + _bridgeSetupDone = true; + } + } else if ( _bridgeSetupDone) { + _pRosBridge->reset(); + _bridgeSetupDone = false; + } - qWarning() << "Connectable: " << _pRosBridge->connected() << "\n"; + //qWarning() << "Connectable: " << _pRosBridge->connected() << "\n"; - auto start = std::chrono::high_resolution_clock::now(); - _pRosBridge->publish(_snakeOrigin, "/snake/origin"); - _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); + //auto start = std::chrono::high_resolution_clock::now(); -// using namespace std::placeholders; -// auto progressCallBack = std::bind(&WimaController::_progressFromJson, -// this, -// _1, -// std::ref(_nemoProgress)); -// _pRosBridge->subscribe("/nemo/progress", progressCallBack); -// _pRosBridge->subscribe("/nemo/heartbeat", [this, &nemoStatusTicker](JsonDocUPtr pDoc){ -// if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) { -// if ( this->_nemoHeartbeat.status() == this->_fallbackStatus ) -// return; -// this->_nemoHeartbeat.setStatus(this->_fallbackStatus); -// } -// nemoStatusTicker.reset(); -// this->_fallbackStatus = -1; /*Timeout*/ -// emit WimaController::nemoStatusChanged(); -// emit WimaController::nemoStatusStringChanged(); -// }); - auto end = std::chrono::high_resolution_clock::now(); - qWarning() << "Duration: " << std::chrono::duration_cast(end-start).count() - << " ms\n"; + //auto end = std::chrono::high_resolution_clock::now(); + //qWarning() << "Duration: " << std::chrono::duration_cast(end-start).count() + //<< " ms\n"; } } @@ -937,16 +983,3 @@ void WimaController::_switchSnakeManager(QVariant variant) _switchWaypointManager(_defaultManager); } } - -void WimaController::_progressFromJson(JsonDocUPtr pDoc, - QNemoProgress &progress) -{ - int requiredSize = _snakeTilesLocal.polygons().size(); - if ( !_pRosBridge->casePacker()->unpack(pDoc, progress) - || progress.progress().size() != requiredSize ) { - progress.progress().fill(0, requiredSize); - } - - emit WimaController::nemoProgressChanged(); -} - diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 60536124a5fde5005ee32ea598581741e2b5fda8..6fd11ca0fdb1bc95dccb8f1cda268162d478cc9d 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -46,7 +46,7 @@ #define SMART_RTL_MAX_ATTEMPTS 3 // times #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms #define EVENT_TIMER_INTERVAL 50 // ms -#define SNAKE_EVENT_LOOP_INTERVAL 5000 // ms +#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms using namespace snake; @@ -351,9 +351,6 @@ private slots: private: using StatusMap = std::map; - // Snake. - void _progressFromJson (JsonDocUPtr pDoc, - QNemoProgress &progress); // Controllers. PlanMasterController *_masterController; @@ -418,6 +415,7 @@ private: QNemoHeartbeat _nemoHeartbeat; // measurement progress int _fallbackStatus; ROSBridgePtr _pRosBridge; + bool _bridgeSetupDone; static StatusMap _nemoStatusMap; // Periodic tasks. diff --git a/src/comm/ros_bridge/include/ComPrivateInclude.h b/src/comm/ros_bridge/include/ComPrivateInclude.h index c83cdb97406f4214051835cada29afd392ff07d1..d6285509917704fb5434b70ab38b13c5d58abc82 100644 --- a/src/comm/ros_bridge/include/ComPrivateInclude.h +++ b/src/comm/ros_bridge/include/ComPrivateInclude.h @@ -5,7 +5,7 @@ #include #include -#include +#include namespace ROSBridge { namespace ComPrivate { @@ -15,7 +15,8 @@ typedef rapidjson::Document JsonDoc; typedef std::unique_ptr JsonDocUPtr; typedef std::deque JsonQueue; typedef std::size_t HashType; -typedef std::set HashSet; + +using ClientMap = std::unordered_map; static const char* _topicAdvertiserKey = "topic_advertiser"; static const char* _topicPublisherKey = "topic_publisher"; diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index 003167d598d4af5a10af913a784f27b3d3f68d46..039e60f5ec76221a834fd99bfabbdbb2507d7a9a 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -31,13 +31,13 @@ class RosbridgeWsClient { if (!client->on_open) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG client->on_open = [client_name, message](std::shared_ptr connection) { #else client->on_open = [message](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; #endif @@ -45,7 +45,7 @@ class RosbridgeWsClient }; } -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG if (!client->on_message) { client->on_message = [client_name](std::shared_ptr /*connection*/, std::shared_ptr in_message) { @@ -69,14 +69,14 @@ class RosbridgeWsClient } #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::thread client_thread([client_name, client]() { #else std::thread client_thread([client]() { #endif client->start(); -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::cout << client_name << ": Terminated" << std::endl; #endif client->on_open = NULL; @@ -141,7 +141,7 @@ public: { client_map[client_name] = std::make_shared(server_port_path); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG else { std::cerr << client_name << " has already been created" << std::endl; @@ -169,11 +169,11 @@ public: it->second->on_message = NULL; it->second->on_close = NULL; it->second->on_error = NULL; -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::cout << client_name << " has been stopped" << std::endl; #endif } -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG else { std::cerr << client_name << " has not been created" << std::endl; @@ -189,11 +189,11 @@ public: it->second->stop(); it->second.reset(); client_map.erase(it); -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::cout << client_name << " has been removed" << std::endl; #endif } -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG else { std::cerr << client_name << " has not been created" << std::endl; @@ -202,6 +202,7 @@ public: } // Gets the client from client_map and starts it. Advertising is essentially sending a message. + // One client per topic! void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "") { std::unordered_map>::iterator it = client_map.find(client_name); @@ -217,7 +218,7 @@ public: start(client_name, it->second, message); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG else { std::cerr << client_name << "has not been created" << std::endl; @@ -242,7 +243,7 @@ public: std::shared_ptr publish_client = std::make_shared(server_port_path); publish_client->on_open = [message](std::shared_ptr connection) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::cout << "publish_client: Opened connection" << std::endl; std::cout << "publish_client: Sending message: " << message << std::endl; #endif @@ -291,7 +292,7 @@ public: it->second->on_message = callback; start(client_name, it->second, message); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG else { std::cerr << client_name << "has not been created" << std::endl; @@ -309,7 +310,7 @@ public: it->second->on_message = callback; start(client_name, it->second, message); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG else { std::cerr << client_name << "has not been created" << std::endl; @@ -336,7 +337,7 @@ public: std::shared_ptr service_response_client = std::make_shared(server_port_path); service_response_client->on_open = [message](std::shared_ptr connection) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::cout << "service_response_client: Opened connection" << std::endl; std::cout << "service_response_client: Sending message: " << message << std::endl; #endif @@ -383,7 +384,7 @@ public: else { call_service_client->on_message = [](std::shared_ptr connection, std::shared_ptr in_message) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_CLIENT_DEBUG std::cout << "call_service_client: Message received: " << in_message->string() << std::endl; std::cout << "call_service_client: Sending close connection" << std::endl; #endif diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp index 7636f2b361c2c424c280451c9edfe68da0c02949..18cda8de9e550c4cfc72db886f15f65a10bf8d0a 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ b/src/comm/ros_bridge/include/TopicPublisher.cpp @@ -11,7 +11,6 @@ struct ROSBridge::ComPrivate::ThreadData std::mutex &rbcMutex; ROSBridge::ComPrivate::JsonQueue &queue; std::mutex &queueMutex; - ROSBridge::ComPrivate::HashSet &advertisedTopicsHashList; const std::atomic &running; std::condition_variable &cv; }; @@ -43,7 +42,6 @@ void ROSBridge::ComPrivate::TopicPublisher::start() *_rbcMutex, _queue, _queueMutex, - _advertisedTopicsHashList, _running, _cv }; @@ -60,17 +58,17 @@ void ROSBridge::ComPrivate::TopicPublisher::reset() return; _pThread->join(); _queue.clear(); - _advertisedTopicsHashList.clear(); } void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data) { // Init. - { - std::lock_guard lk(data.rbcMutex); - data.rbc.addClient(ROSBridge::ComPrivate::_topicAdvertiserKey); - data.rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey); - } + ClientMap clientMap; +// { +// std::lock_guard lk(data.rbcMutex); +// data.rbc.addClient(ROSBridge::ComPrivate::_topicAdvertiserKey); +// data.rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey); +// } // Main Loop. while(data.running.load()){ std::unique_lock lk(data.queueMutex); @@ -94,15 +92,19 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data // Check if topic must be advertised. // Advertised topics are stored in advertisedTopicsHashList as // a hash. - HashType hash = ROSBridge::ComPrivate::getHash(tag.topic()); - if ( data.advertisedTopicsHashList.count(hash) == 0) { - data.advertisedTopicsHashList.insert(hash); + std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey + + tag.topic(); + HashType hash = ROSBridge::ComPrivate::getHash(clientName); + auto it = clientMap.find(hash); + if ( it == clientMap.end()) { // Need to advertise topic? + clientMap.insert(std::make_pair(hash, clientName)); + std::cout << clientName << ";" + << tag.topic() << ";" + << tag.messageType() << ";" << std::endl; { - std::cout << ROSBridge::ComPrivate::_topicAdvertiserKey << ";" - << tag.topic() << ";" - << tag.messageType() << ";" << std::endl; std::lock_guard lk(data.rbcMutex); - data.rbc.advertise(ROSBridge::ComPrivate::_topicAdvertiserKey, + data.rbc.addClient(clientName); + data.rbc.advertise(clientName, tag.topic(), tag.messageType() ); } @@ -118,8 +120,8 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data // Tidy up. { std::lock_guard lk(data.rbcMutex); - data.rbc.removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey); - data.rbc.removeClient(ROSBridge::ComPrivate::_topicPublisherKey); + for (auto& it : clientMap) + data.rbc.removeClient(it.second); } } diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/TopicPublisher.h index 89527026ef8c575544f1d560ec999cd08e43ab8e..ce7e142856300a6b89fc7be6a15ebb5381413a35 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.h +++ b/src/comm/ros_bridge/include/TopicPublisher.h @@ -57,8 +57,6 @@ private: CasePacker *_casePacker; RosbridgeWsClient *_rbc; std::mutex *_rbcMutex; - HashSet _advertisedTopicsHashList; // Not thread save! This container - // is manipulated by transmittLoop only! CondVar _cv; ThreadPtr _pThread; };