diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0fb910521d585e2191d90f6ee17966370d235730..214c83c0d9e4c48cb4b32f8aca821b941f399cf1 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 9f9275c4f08b18a24173a7b5596c2126a0707e87..01573eab5af8828e9de76f113a2a7410183ea418 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 8aa5a8c575eef37628c9fdb515c470d546c86ac0..46a04fc79c14405654e3346bc839c7407e96cc02 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 3f2ab2e0c39f71d1f75eab26e71312cfd1a17667..30b747d67ef2f312b0a9b5d4264e5e3d15722a41 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 9daf5ee3d974ea194947e92eebb0d6728783e5c4..54f7425ae419d2b7582fa182379be269d452a9d8 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 84de696fea36e8e497172f6d06e6e106690957c5..4509ae1139848de17078016b293f4453ac2b8e51 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 82dc4862fe728c1f339810d23362e1dcc373b495..0c6bc6e6337d4f92f767495980e6a88db1d8b59a 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 7842e827041fdd02df1b66f1f97b2756101cef64..2f78a3cf281498ffb8ef9e972ec5821661d37f8c 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 2314c11261b8fd87804a8dcd4bb5e09a3a23f24d..d6896400c8e9e114fbd32c45b82d231c7d3cf8f8 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 6924c3ed002b79ef200b7f03920375b3fdbbc305..bf54bad1c9031ac1749e0928baa3f95fa0556a08 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 e896225cfd84ef856cd03484ef980168fc529856..75114650f47e630cac51e8faeed7cd4033964b86 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 } } }