From d47dc9a9a795beb69200af2091b44039fad4d0aa Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Fri, 31 Jul 2020 08:46:35 +0200 Subject: [PATCH] bug appearded again --- src/Wima/WimaController.cc | 38 +++++++++---------- .../ros_bridge/include/TopicPublisher.cpp | 38 +++++++++---------- src/comm/ros_bridge/include/TopicPublisher.h | 11 +++--- .../ros_bridge/include/TopicSubscriber.cpp | 2 +- 4 files changed, 44 insertions(+), 45 deletions(-) diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 6ad642c1a..2179b30db 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -719,27 +719,27 @@ void WimaController::_eventTimerHandler() auto start = std::chrono::high_resolution_clock::now(); - _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); _pRosBridge->publish(_snakeOrigin, "/snake/origin"); + _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); - 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(); - }); +// 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() diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp index 5e36c2e68..7636f2b36 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ b/src/comm/ros_bridge/include/TopicPublisher.cpp @@ -37,11 +37,6 @@ void ROSBridge::ComPrivate::TopicPublisher::start() if ( _running.load() ) // start called while thread running. return; _running.store(true); - { - std::lock_guard lk(*_rbcMutex); - _rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey); - _rbc->addClient(ROSBridge::ComPrivate::_topicPublisherKey); - } ROSBridge::ComPrivate::ThreadData data{ *_casePacker, *_rbc, @@ -64,17 +59,19 @@ void ROSBridge::ComPrivate::TopicPublisher::reset() if ( !_pThread ) return; _pThread->join(); - { - std::lock_guard lk(*_rbcMutex); - _rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey); - _rbc->removeClient(ROSBridge::ComPrivate::_topicPublisherKey); - } _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); + } + // Main Loop. while(data.running.load()){ std::unique_lock lk(data.queueMutex); // Check if new data available, wait if not. @@ -87,13 +84,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data data.queue.pop_front(); lk.unlock(); - // Debug output. -// std::cout << "Transmitter loop json document:" << std::endl; -// rapidjson::OStreamWrapper out(std::cout); -// rapidjson::Writer writer(out); -// pJsonDoc->Accept(writer); -// std::cout << std::endl << std::endl; - // Get tag from Json message and remove it. Tag tag; bool ret = data.casePacker.getTag(pJsonDoc, tag); @@ -108,20 +98,28 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data if ( data.advertisedTopicsHashList.count(hash) == 0) { data.advertisedTopicsHashList.insert(hash); { + std::cout << ROSBridge::ComPrivate::_topicAdvertiserKey << ";" + << tag.topic() << ";" + << tag.messageType() << ";" << std::endl; std::lock_guard lk(data.rbcMutex); data.rbc.advertise(ROSBridge::ComPrivate::_topicAdvertiserKey, tag.topic(), tag.messageType() ); } } - // Debug output. - //std::cout << "Hash Set size: " << advertisedTopicsHashList.size() << std::endl; - // Send Json message. + // Publish Json message. { std::lock_guard lk(data.rbcMutex); data.rbc.publish(tag.topic(), *pJsonDoc.get()); } } // while loop + + // Tidy up. + { + std::lock_guard lk(data.rbcMutex); + data.rbc.removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey); + data.rbc.removeClient(ROSBridge::ComPrivate::_topicPublisherKey); + } } diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/TopicPublisher.h index 87ede08a3..89527026e 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.h +++ b/src/comm/ros_bridge/include/TopicPublisher.h @@ -36,11 +36,6 @@ public: //! @brief Resets the publisher. void reset(); - template - void publish(const T &msg, const std::string &topic){ - JsonDocUPtr docPtr(_casePacker->pack(msg, topic)); - publish(std::move(docPtr)); - } void publish(JsonDocUPtr docPtr){ { std::lock_guard lock(_queueMutex); @@ -49,6 +44,12 @@ public: _cv.notify_one(); // Wake publisher thread. } + template + void publish(const T &msg, const std::string &topic){ + JsonDocUPtr docPtr(_casePacker->pack(msg, topic)); + publish(std::move(docPtr)); + } + private: JsonQueue _queue; std::mutex _queueMutex; diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/TopicSubscriber.cpp index f58c10885..dce6371ec 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.cpp +++ b/src/comm/ros_bridge/include/TopicSubscriber.cpp @@ -24,13 +24,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::start() void ROSBridge::ComPrivate::TopicSubscriber::reset() { if ( _running ){ + _running = false; { std::lock_guard lk(*_rbcMutex); for (std::string clientName : _clientList){ _rbc->removeClient(clientName); } } - _running = false; { std::lock_guard lk(_callbackMap.mutex); _callbackMap.map.clear(); -- 2.22.0