diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 2a319e15ceab52a63d7a4f0867575a6460ed2c76..bb4b87d035b9bb5951570457996396ac5fdef0f3 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -16,6 +16,10 @@ #include +#define SMART_RTL_MAX_ATTEMPTS 3 // times +#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms +#define EVENT_TIMER_INTERVAL 50 // ms + // const char* WimaController::wimaFileExtension = "wima"; @@ -70,14 +74,14 @@ WimaController::WimaController(QObject *parent) , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) , _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName]) , _altitude (settingsGroup, _metaDataMap[altitudeName]) - , _measurementPathLength (-1) - , _lowBatteryHandlingTriggered (false) - , _snakeCalcInProgress (false) , _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName]) , _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName]) , _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName]) , _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName]) , _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName]) + , _measurementPathLength (-1) + , _lowBatteryHandlingTriggered (false) + , _snakeCalcInProgress (false) , _nemoHeartbeat (0 /*status: not connected*/) , _fallbackStatus (0 /*status: not connected*/) , _pRosBridge (new ROSBridge::ROSBridge()) diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 3c40cfd6d7a1a5cd3a98fa3643abd3157929b0c5..73b4e0fb1dc78fd38ae8679a85832edd3825698a 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -43,10 +43,6 @@ #include -#define SMART_RTL_MAX_ATTEMPTS 3 // times -#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms -#define EVENT_TIMER_INTERVAL 50 // ms - using namespace snake; @@ -400,8 +396,6 @@ private: // Snake bool _snakeCalcInProgress; - bool _snakeRecalcNecessary; - bool _scenarioDefinedBool; SnakeWorker _snakeWorker; Scenario _scenario; ::GeoPoint3D _snakeOrigin; diff --git a/src/comm/ros_bridge/include/GenericMessages.h b/src/comm/ros_bridge/include/GenericMessages.h index 7e19254cd27b243f9574cddb217849a034a070d9..8acb15a33d4e61b6548b85276ad340b7123e5744 100644 --- a/src/comm/ros_bridge/include/GenericMessages.h +++ b/src/comm/ros_bridge/include/GenericMessages.h @@ -328,9 +328,9 @@ template class ContainterType class GenericProgress : public MessageBaseClass{ public: typedef MessageGroups::ProgressGroup Group; - GenericProgress(){} - GenericProgress(const ContainterType &progress) :_progress(progress){} - GenericProgress(const GenericProgress &p) :_progress(p.progress()){} + GenericProgress() : MessageBaseClass() {} + GenericProgress(const ContainterType &progress) : MessageBaseClass(), _progress(progress){} + GenericProgress(const GenericProgress &p) :MessageBaseClass(), _progress(p.progress()){} ~GenericProgress() {} virtual GenericProgress *Clone() const override { return new GenericProgress(*this); } diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp index 5489e6c8e7d9b934e85aac5a6b694b63f4ecf284..d237a1ce8f63d8e67aaeb8c24f8ce622a6f62fec 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.cpp +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -53,7 +53,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar #ifdef DEBUG std::thread client_thread([client_name, client]() { #else - std::thread client_thread([client, client_name]() { + std::thread client_thread([client]() { #endif client->start(); @@ -64,8 +64,10 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar client->on_message = NULL; client->on_close = NULL; client->on_error = NULL; +#ifdef DEBUG std::cout << "start" << client_name << " client reference count:" << client.use_count() << std::endl; std::cout << client_name << " thread end" << std::endl; +#endif }); client_thread.detach(); @@ -89,28 +91,28 @@ RosbridgeWsClient::~RosbridgeWsClient() bool RosbridgeWsClient::connected(){ - // auto p = std::make_shared>(); - // auto future = p->get_future(); - - // auto status_client = std::make_shared(server_port_path); - // status_client->on_open = [p](std::shared_ptr) { - // p->set_value(); - // }; - - // std::thread t([status_client]{ - // status_client->start(); - // status_client->on_open = NULL; - // status_client->on_message = NULL; - // status_client->on_close = NULL; - // status_client->on_error = NULL; - // }); - - // auto status = future.wait_for(std::chrono::milliseconds(20)); - // status_client->stop(); - // t.join(); - // bool connected = status == std::future_status::ready; - // //std::cout << "connected(): " << connected << std::endl; - // return connected; + auto p = std::make_shared>(); + auto future = p->get_future(); + + auto status_client = std::make_shared(server_port_path); + status_client->on_open = [p](std::shared_ptr) { + p->set_value(); + }; + + std::thread t([status_client]{ + status_client->start(); + status_client->on_open = NULL; + status_client->on_message = NULL; + status_client->on_close = NULL; + status_client->on_error = NULL; + }); + + auto status = future.wait_for(std::chrono::milliseconds(20)); + status_client->stop(); + t.join(); + bool connected = status == std::future_status::ready; + //std::cout << "connected(): " << connected << std::endl; + return connected; return true; } @@ -142,29 +144,6 @@ std::shared_ptr RosbridgeWsClient::getClient(const std::string &client } void RosbridgeWsClient::stopClient(const std::string &client_name) -{ - std::lock_guard lk(mutex); - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - it->second->stop(); - it->second->on_open = NULL; - it->second->on_message = NULL; - it->second->on_close = NULL; - it->second->on_error = NULL; -#ifdef DEBUG - std::cout << client_name << " has been stopped" << std::endl; -#endif - } -#ifdef DEBUG - else - { - std::cerr << client_name << " has not been created" << std::endl; - } -#endif -} - -void RosbridgeWsClient::removeClient(const std::string &client_name) { std::lock_guard lk(mutex); std::unordered_map>::iterator it = client_map.find(client_name); @@ -176,23 +155,21 @@ void RosbridgeWsClient::removeClient(const std::string &client_name) #ifdef DEBUG std::thread t([client, client_name](){ #else - std::thread t([client, client_name](){ + std::thread t([client](){ #endif std::this_thread::sleep_for(std::chrono::milliseconds(100)); client->stop(); - client->on_open = NULL; - client->on_message = NULL; - client->on_close = NULL; - client->on_error = NULL; - std::cout << "removeClient thread: " << client_name << " reference count: " << client.use_count() << std::endl; - std::cout << client_name << " has been removed" << std::endl; + // The next lines of code seem to cause a double free or corruption error, why? +// client->on_open = NULL; +// client->on_message = NULL; +// client->on_close = NULL; +// client->on_error = NULL; #ifdef DEBUG + std::cout << "removeClient thread: " << client_name << " reference count: " << client.use_count() << std::endl; std::cout << client_name << " has been removed" << std::endl; #endif }); - client_map.erase(it); t.detach(); - std::cout << "removeClient: " << client_name << " reference count: " << client.use_count() << std::endl; } #ifdef DEBUG else @@ -202,6 +179,28 @@ void RosbridgeWsClient::removeClient(const std::string &client_name) #endif } +void RosbridgeWsClient::removeClient(const std::string &client_name) +{ + stopClient(client_name); + { + std::lock_guard lk(mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + client_map.erase(it); +#ifdef DEBUG + std::cout << "removeClient: " << client_name << " reference count: " << client.use_count() << std::endl; +#endif + } +#ifdef DEBUG + else + { + std::cerr << client_name << " has not been created" << std::endl; + } +#endif + } +} + std::string RosbridgeWsClient::getAdvertisedTopics(){ auto pPromise(std::make_shared>()); auto future = pPromise->get_future(); @@ -211,7 +210,7 @@ std::string RosbridgeWsClient::getAdvertisedTopics(){ pPromise->set_value(in_message->string()); connection->send_close(1000); }); - auto status = future.wait_for(std::chrono::milliseconds(25)); // enought? + auto status = future.wait_for(std::chrono::milliseconds(20)); // enought? return status == std::future_status::ready ? future.get() : std::string(); } @@ -691,7 +690,7 @@ void RosbridgeWsClient::waitForService(const std::string &service, const std::sh if ( serviceAvailable(service) ){ break; } - std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Avoid excessive polling. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Avoid excessive polling. }; #ifdef DEBUG auto e = std::chrono::high_resolution_clock::now(); @@ -723,7 +722,7 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::shared if ( topicAvailable(topic) ){ break; } - std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Avoid excessive polling. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Avoid excessive polling. }; #ifdef DEBUG auto e = std::chrono::high_resolution_clock::now(); diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp index 29f8cac2ab42945835f5e2a24894400d01ba7903..7842e827041fdd02df1b66f1f97b2756101cef64 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ b/src/comm/ros_bridge/include/TopicPublisher.cpp @@ -25,17 +25,21 @@ void ROSBridge::ComPrivate::TopicPublisher::start() // Init. std::unordered_map topicMap; // Main Loop. - while( !this->_stopped->load()){ - std::unique_lock lk(this->_mutex); - // Check if new data available, wait if not. - if (this->_queue.empty()){ - this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case. - continue; + while( !this->_stopped->load() ){ + JsonDocUPtr pJsonDoc; + { + std::unique_lock lk(this->_mutex); + // Check if new data available, wait if not. + if (this->_queue.empty()){ + if ( _stopped->load() ) // Check condition again while holding the lock. + break; + this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case. + continue; + } + // Pop message from queue. + pJsonDoc = std::move(this->_queue.front()); + this->_queue.pop_front(); } - // Pop message from queue. - JsonDocUPtr pJsonDoc(std::move(this->_queue.front())); - this->_queue.pop_front(); - lk.unlock(); // Get tag from Json message and remove it. Tag tag; @@ -78,15 +82,20 @@ void ROSBridge::ComPrivate::TopicPublisher::reset() { if ( _stopped->load() ) // stop called while thread not running. return; - std::cout << "TopicPublisher: _stopped->store(true)." << std::endl; - _stopped->store(true); - std::cout << "TopicPublisher: ask publisher thread to stop." << std::endl; - _cv.notify_one(); // Wake publisher thread. + { + std::lock_guard lk(_mutex); + std::cout << "TopicPublisher: _stopped->store(true)." << std::endl; + _stopped->store(true); + std::cout << "TopicPublisher: ask publisher thread to stop." << std::endl; + _cv.notify_one(); // Wake publisher thread. + } if ( !_pThread ) return; _pThread->join(); std::cout << "TopicPublisher: publisher thread joined." << std::endl; - _queue.clear(); - std::cout << "TopicPublisher: queue cleard." << std::endl; + { + _queue.clear(); + std::cout << "TopicPublisher: queue cleard." << std::endl; + } } diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp index e79e534cd3898986306ef13186dafecb12eb755c..94035e01fc38efb9a6cdc56548672067ead7bba6 100644 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ b/src/comm/ros_bridge/src/ROSBridge.cpp @@ -46,16 +46,16 @@ void ROSBridge::ROSBridge::reset() auto start = std::chrono::high_resolution_clock::now(); std::cout << "ROSBridge::reset: reset publisher" << std::endl; _topicPublisher.reset(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); std::cout << "ROSBridge::reset: reset subscriber" << std::endl; _topicSubscriber.reset(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); std::cout << "ROSBridge::reset: reset server" << std::endl; _server.reset(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); std::cout << "ROSBridge::reset: _stopped->store(true)" << std::endl; _stopped->store(true); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); auto end = std::chrono::high_resolution_clock::now(); auto delta = std::chrono::duration_cast(end-start).count(); std::cout << "ROSBridge reset duration: " << delta << " ms" << std::endl;