From fd49a04a5f617eef045f9ee54a09c3a29f39c34c Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Thu, 20 Aug 2020 15:00:52 +0200 Subject: [PATCH] enableLowBatteryHandling issue --- qgroundcontrol.pro | 1 + src/Settings/Wima.SettingsGroup.json | 6 ++ src/Settings/WimaSettings.cc | 1 + src/Settings/WimaSettings.h | 1 + src/Wima/WimaController.cc | 59 +++++++---- .../ros_bridge/include/RosBridgeClient.cpp | 100 +++++++++++------- src/comm/ros_bridge/include/RosBridgeClient.h | 9 +- .../ros_bridge/include/TopicPublisher.cpp | 4 +- .../ros_bridge/include/TopicSubscriber.cpp | 4 +- src/comm/ros_bridge/src/ROSBridge.cpp | 9 +- src/ui/preferences/GeneralSettings.qml | 25 ++++- 11 files changed, 152 insertions(+), 67 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0fb910521..214c83c0d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -32,6 +32,7 @@ DebugBuild { } else { DESTDIR = $${OUT_PWD}/release + DEFINES += DEBUG DEFINES += NDEBUG } diff --git a/src/Settings/Wima.SettingsGroup.json b/src/Settings/Wima.SettingsGroup.json index 9f9275c4f..01573eab5 100644 --- a/src/Settings/Wima.SettingsGroup.json +++ b/src/Settings/Wima.SettingsGroup.json @@ -19,4 +19,10 @@ "units": "s", "defaultValue": 15 } +{ + "name": "rosbridgeConnectionString", + "shortDescription": "Ros Bridge Connection String (host:port).", + "type": "string", + "defaultValue": "localhost:9090" +} ] diff --git a/src/Settings/WimaSettings.cc b/src/Settings/WimaSettings.cc index 8aa5a8c57..46a04fc79 100644 --- a/src/Settings/WimaSettings.cc +++ b/src/Settings/WimaSettings.cc @@ -11,3 +11,4 @@ DECLARE_SETTINGGROUP(Wima, "Wima") DECLARE_SETTINGSFACT(WimaSettings, lowBatteryThreshold) DECLARE_SETTINGSFACT(WimaSettings, enableLowBatteryHandling) DECLARE_SETTINGSFACT(WimaSettings, minimalRemainingMissionTime) +DECLARE_SETTINGSFACT(WimaSettings, rosbridgeConnectionString) diff --git a/src/Settings/WimaSettings.h b/src/Settings/WimaSettings.h index 3f2ab2e0c..30b747d67 100644 --- a/src/Settings/WimaSettings.h +++ b/src/Settings/WimaSettings.h @@ -13,4 +13,5 @@ public: DEFINE_SETTINGFACT(lowBatteryThreshold) DEFINE_SETTINGFACT(enableLowBatteryHandling) DEFINE_SETTINGFACT(minimalRemainingMissionTime) + DEFINE_SETTINGFACT(rosbridgeConnectionString) }; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 9daf5ee3d..54f7425ae 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -368,6 +368,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) // reset visual items _areas.clear(); _defaultManager.clear(); + _snakeManager.clear(); _snakeTiles.polygons().clear(); _snakeTilesLocal.polygons().clear(); _snakeTileCenterPoints.clear(); @@ -533,6 +534,15 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) emit snakeTilesChanged(); emit snakeTileCenterPointsChanged(); + if ( _enableSnake.rawValue().toBool() + && _pRosBridge->isRunning() + && _pRosBridge->connected() ){ + if ( _snakeTilesLocal.polygons().size() > 0 ){ + _pRosBridge->publish(_snakeOrigin, "/snake/origin"); + _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); + } + } + _localPlanDataValid = true; return true; } @@ -723,14 +733,21 @@ void WimaController::_eventTimerHandler() } if ( _snakeTicker.ready() ) { + static bool setupDone = false; if ( _enableSnake.rawValue().toBool() ) { - if ( !_pRosBridge->isRunning() && _pRosBridge->connected() ) { + if ( !_pRosBridge->isRunning()) { + _pRosBridge->start(); + } else if ( _pRosBridge->isRunning() && _pRosBridge->connected() && !setupDone) { + _pRosBridge->reset(); + _pRosBridge->start(); _setupTopicService(); + setupDone = true; } else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){ - _pRosBridge->reset(); + setupDone = false; } } else if ( _pRosBridge->isRunning() ) { _pRosBridge->reset(); + setupDone = false; } } } @@ -908,16 +925,23 @@ void WimaController::_switchSnakeManager(QVariant variant) void WimaController::_setupTopicService() { - _pRosBridge->start(); - _pRosBridge->publish(_snakeOrigin, "/snake/origin"); - _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); + if ( _snakeTilesLocal.polygons().size() > 0){ + _pRosBridge->publish(_snakeOrigin, "/snake/origin"); + _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); + } _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().size() != requiredSize ) { // Some error occured. + // Set progress to default. progress.progress().fill(0, requiredSize); + // Publish origin and tiles again. + if ( this->_snakeTilesLocal.polygons().size() > 0){ + this->_pRosBridge->publish(this->_snakeOrigin, "/snake/origin"); + this->_pRosBridge->publish(this->_snakeTilesLocal, "/snake/tiles"); + } } emit WimaController::nemoProgressChanged(); @@ -936,32 +960,31 @@ void WimaController::_setupTopicService() emit WimaController::nemoStatusStringChanged(); }); - auto pOrigin = &_snakeOrigin; - auto casePacker = _pCasePacker; _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin", - [casePacker, pOrigin](JsonDocUPtr) -> JsonDocUPtr { - JsonDocUPtr pDoc = casePacker->pack(*pOrigin, ""); - casePacker->removeTag(pDoc); + [this](JsonDocUPtr) -> JsonDocUPtr { + JsonDocUPtr pDoc; + if ( this->_snakeTilesLocal.polygons().size() > 0){ + pDoc = this->_pCasePacker->pack(this->_snakeOrigin, ""); + } else { + pDoc = this->_pCasePacker->pack(::GeoPoint3D(0,0,0), ""); + } + this->_pCasePacker->removeTag(pDoc); rapidjson::Document value(rapidjson::kObjectType); JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType)); value.CopyFrom(*pDoc, pReturn->GetAllocator()); pReturn->AddMember("origin", value, pReturn->GetAllocator()); return pReturn; - }); - auto pTiles = &_snakeTilesLocal; _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles", - [casePacker, pTiles](JsonDocUPtr) -> JsonDocUPtr { - - JsonDocUPtr pDoc = casePacker->pack(*pTiles, ""); - casePacker->removeTag(pDoc); + [this](JsonDocUPtr) -> JsonDocUPtr { + JsonDocUPtr pDoc = this->_pCasePacker->pack(this->_snakeTilesLocal, ""); + this->_pCasePacker->removeTag(pDoc); rapidjson::Document value(rapidjson::kObjectType); JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType)); value.CopyFrom(*pDoc, pReturn->GetAllocator()); pReturn->AddMember("tiles", value, pReturn->GetAllocator()); return pReturn; - }); } diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp index 84de696fe..4509ae113 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.cpp +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -74,7 +74,6 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar 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 }); @@ -82,11 +81,34 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar client_thread.detach(); } -RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_path) : +RosbridgeWsClient::RosbridgeWsClient(const std::string &server_port_path, bool run) : server_port_path(server_port_path) , isConnected(std::make_shared(false)) - , stopped(std::make_shared(false)) + , stopped(std::make_shared(true)) +{ + if ( run ) + this->run(); +} + +RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_path) : + RosbridgeWsClient::RosbridgeWsClient(server_port_path, true) +{ +} + +RosbridgeWsClient::~RosbridgeWsClient() +{ + reset(); +} + +bool RosbridgeWsClient::connected(){ + return isConnected->load(); +} + +void RosbridgeWsClient::run() { + if ( !stopped->load() ) + return; + stopped->store(false); // Start periodic thread to monitor connection status, advertised topics etc. periodic_thread = std::make_shared ([this] { std::list task_list; @@ -165,7 +187,7 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat this->callService("/rosapi/topics", [topics_set, this]( std::shared_ptr connection, std::shared_ptr in_message){ - std::cout << "/rosapi/topics: " << in_message->string() << std::endl; + //std::cout << "/rosapi/topics: " << in_message->string() << std::endl; std::unique_lock lk(this->mutex); this->available_topics = in_message->string(); lk.unlock(); @@ -212,7 +234,7 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat this->callService("/rosapi/services", [this, services_set]( std::shared_ptr connection, std::shared_ptr in_message){ - std::cout << "/rosapi/services: " << in_message->string() << std::endl; + //std::cout << "/rosapi/services: " << in_message->string() << std::endl; std::unique_lock lk(this->mutex); this->available_services = in_message->string(); lk.unlock(); @@ -257,26 +279,24 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat // Process tasks. // ==================================================================================== for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){ - std::cout << "processing task: " << task_it->name << std::endl; + //std::cout << "processing task: " << task_it->name << std::endl; if ( !task_it->expired() ){ if ( task_it->ready() ){ - std::cout << "executing task: " << task_it->name << std::endl; + //std::cout << "executing task: " << task_it->name << std::endl; task_it->execute(); task_it = task_list.erase(task_it); } else { - std::cout << "noting to do for task: " << task_it->name << std::endl; + //std::cout << "noting to do for task: " << task_it->name << std::endl; ++task_it; } } else { - std::cout << "task expired: " << task_it->name << std::endl; + //std::cout << "task expired: " << task_it->name << std::endl; task_it->clear_up(); task_it = task_list.erase(task_it); } - std::cout << std::endl; + //std::cout << std::endl; } - std::cout << "task list size: " << task_list.size() << std::endl; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); } @@ -287,11 +307,20 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat task_list.clear(); std::cout << "periodic thread end" << std::endl; }); + } -RosbridgeWsClient::~RosbridgeWsClient() +void RosbridgeWsClient::stop() { + if ( stopped->load() ) + return; stopped->store(true); + periodic_thread->join(); +} + +void RosbridgeWsClient::reset() +{ + stop(); unsubscribeAll(); unadvertiseAll(); unadvertiseAllServices(); @@ -299,11 +328,6 @@ RosbridgeWsClient::~RosbridgeWsClient() { removeClient(client.first); } - periodic_thread->join(); -} - -bool RosbridgeWsClient::connected(){ - return isConnected->load(); } void RosbridgeWsClient::addClient(const std::string &client_name) @@ -378,9 +402,6 @@ void RosbridgeWsClient::removeClient(const std::string &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 @@ -468,7 +489,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string std::lock_guard lk(mutex); auto it_ser_top = std::find_if(service_topic_list.begin(), service_topic_list.end(), - [topic](const EntryData &td){ + [&topic](const EntryData &td){ return topic == std::get(td); }); if ( it_ser_top == service_topic_list.end()){ @@ -723,7 +744,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){ message += ", \"service\":\"" + service + "\""; message = "{" + message + "}"; - std::string client_name = "topic_unsubscriber" + service; + std::string client_name = "service_unadvertiser" + service; auto client = std::make_shared(server_port_path); #ifdef DEBUG client->on_open = [client_name, message](std::shared_ptr connection) { @@ -851,30 +872,32 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service) void RosbridgeWsClient::waitForService(const std::string &service) { - waitForService(service, stopped); + waitForService(service, []{ + return false; // never stop + }); } -void RosbridgeWsClient::waitForService(const std::string &service, const std::shared_ptr stop) +void RosbridgeWsClient::waitForService(const std::string &service, const std::function stop) { #ifdef DEBUG auto s = std::chrono::high_resolution_clock::now(); long counter = 0; #endif const auto poll_interval = std::chrono::milliseconds(1000); - auto end = std::chrono::high_resolution_clock::now() + poll_interval; - while( !stop->load() ) + auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; + while( !stop() ) { - if ( std::chrono::high_resolution_clock::now() > end ){ + if ( std::chrono::high_resolution_clock::now() > poll_time_point ){ #ifdef DEBUG ++counter; #endif if ( serviceAvailable(service) ){ break; } else { - end = std::chrono::high_resolution_clock::now() + poll_interval; + poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; } } else { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } }; #ifdef DEBUG @@ -889,31 +912,32 @@ void RosbridgeWsClient::waitForService(const std::string &service, const std::sh void RosbridgeWsClient::waitForTopic(const std::string &topic) { - auto stop = std::make_shared(false); - waitForTopic(topic, stop); + waitForTopic(topic, []{ + return false; // never stop + }); } -void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::shared_ptr stop) +void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::function stop) { #ifdef DEBUG auto s = std::chrono::high_resolution_clock::now(); long counter = 0; #endif const auto poll_interval = std::chrono::milliseconds(1000); - auto end = std::chrono::high_resolution_clock::now() + poll_interval; - while( !stop->load() ) + auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; + while( !stop() ) { - if ( std::chrono::high_resolution_clock::now() > end ){ + if ( std::chrono::high_resolution_clock::now() > poll_time_point ){ #ifdef DEBUG ++counter; #endif if ( topicAvailable(topic) ){ break; } else { - end = std::chrono::high_resolution_clock::now() + poll_interval; + poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; } } else { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } }; #ifdef DEBUG diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index 82dc4862f..0c6bc6e63 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -63,11 +63,16 @@ class RosbridgeWsClient public: RosbridgeWsClient(const std::string& server_port_path); + RosbridgeWsClient(const std::string& server_port_path, bool run); ~RosbridgeWsClient(); bool connected(); + void run(); + void stop(); + void reset(); + // Adds a client to the client_map. void addClient(const std::string& client_name); @@ -173,7 +178,7 @@ public: //! \param stop Flag to stop waiting. //! @note This function will block as long as the service is not available or \p stop is false. //! - void waitForService(const std::string& service, const std::shared_ptr stop); + void waitForService(const std::string& service, const std::function stop); //! //! \brief Waits until the topic with the name \p topic is available. @@ -187,5 +192,5 @@ public: //! \param stop Flag to stop waiting. //! @note This function will block as long as the topic is not available or \p stop is false. //! - void waitForTopic(const std::string& topic, const std::shared_ptr stop); + void waitForTopic(const std::string& topic, const std::function stop); }; diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp index 7842e8270..2f78a3cf2 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ b/src/comm/ros_bridge/include/TopicPublisher.cpp @@ -60,7 +60,9 @@ void ROSBridge::ComPrivate::TopicPublisher::start() this->_rbc.advertise(clientName, tag.topic(), tag.messageType() ); - this->_rbc.waitForTopic(tag.topic(), this->_stopped); // Wait until topic is advertised. + this->_rbc.waitForTopic(tag.topic(), [this]{ + return this->_stopped->load(); + }); // Wait until topic is advertised. } // Publish Json message. diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/TopicSubscriber.cpp index 2314c1126..d6896400c 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.cpp +++ b/src/comm/ros_bridge/include/TopicSubscriber.cpp @@ -85,7 +85,9 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe( // Wait until topic is available. std::cout << "TopicSubscriber: Starting wait thread, " << clientName << std::endl; std::thread t([this, clientName, topic, callbackWrapper]{ - this->_rbc.waitForTopic(topic, this->_stopped); + this->_rbc.waitForTopic(topic, [this]{ + return this->_stopped->load(); + }); if ( !this->_stopped->load() ){ this->_rbc.addClient(clientName); this->_rbc.subscribe(clientName, topic, callbackWrapper); diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp index 6924c3ed0..bf54bad1c 100644 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ b/src/comm/ros_bridge/src/ROSBridge.cpp @@ -3,12 +3,11 @@ ROSBridge::ROSBridge::ROSBridge() : _stopped(std::make_shared(true)) , _casePacker(&_typeFactory, &_jsonFactory) - , _rbc("localhost:9090") + , _rbc("localhost:9090", false /*run*/) , _topicPublisher(_casePacker, _rbc) , _topicSubscriber(_casePacker, _rbc) , _server(_casePacker, _rbc) { - } void ROSBridge::ROSBridge::publish(ROSBridge::ROSBridge::JsonDocUPtr doc) @@ -35,19 +34,21 @@ const ROSBridge::CasePacker *ROSBridge::ROSBridge::casePacker() const void ROSBridge::ROSBridge::start() { + _stopped->store(false); + _rbc.run(); _topicPublisher.start(); _topicSubscriber.start(); _server.start(); - _stopped->store(false); } void ROSBridge::ROSBridge::reset() { auto start = std::chrono::high_resolution_clock::now(); + _stopped->store(true); _topicPublisher.reset(); _topicSubscriber.reset(); _server.reset(); - _stopped->store(true); + _rbc.reset(); 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; diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index e896225cf..75114650f 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -950,18 +950,18 @@ QGCView { columns: 4 QGCLabel { - text: qsTr("Low Battery Threshold") + text: qsTr("Battery Threshold") visible: _userBrandImageIndoor.visible } FactTextField { Layout.preferredWidth: _valueFieldWidth fact: QGroundControl.settingsManager.wimaSettings.lowBatteryThreshold + Layout.columnSpan: 2 } FactCheckBox { - text: "Enable low Battery handling" + text: "Enable Smart RTL" fact: QGroundControl.settingsManager.wimaSettings.enableLowBatteryHandling - Layout.columnSpan: 2 } QGCLabel { @@ -971,6 +971,25 @@ QGCView { FactTextField { Layout.preferredWidth: _valueFieldWidth fact: QGroundControl.settingsManager.wimaSettings.minimalRemainingMissionTime + Layout.columnSpan: 2 + } + Item{ + // dummy + width: 1 + } + + QGCLabel { + text: qsTr("ROS Bridge Connection String") + visible: _userBrandImageIndoor.visible + } + FactTextField { + Layout.preferredWidth: _valueFieldWidth + fact: QGroundControl.settingsManager.wimaSettings.rosbridgeConnectionString + Layout.columnSpan: 2 + } + Item{ + // dummy + width: 1 } } } -- 2.22.0