enableLowBatteryHandling issue

qgroundcontrol.pro

@@ -32,6 +32,7 @@ DebugBuild {
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
+    DEFINES += DEBUG
     DEFINES += NDEBUG
 }
 

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"
+}
 ]

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)

src/Settings/WimaSettings.h

@@ -13,4 +13,5 @@ public:
     DEFINE_SETTINGFACT(lowBatteryThreshold)
     DEFINE_SETTINGFACT(enableLowBatteryHandling)
     DEFINE_SETTINGFACT(minimalRemainingMissionTime)
+    DEFINE_SETTINGFACT(rosbridgeConnectionString)
 };

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;
-
     });
 }

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<std::atomic_bool>(false))
-  , stopped(std::make_shared<std::atomic_bool>(false))
+  , stopped(std::make_shared<std::atomic_bool>(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<std::thread> ([this] {
         std::list<Task> 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<WsClient::Connection> connection,
                                           std::shared_ptr<WsClient::InMessage> in_message){
-                            std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
+                            //std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
                             std::unique_lock<std::mutex> 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<WsClient::Connection> connection,
                                           std::shared_ptr<WsClient::InMessage> in_message){
-                            std::cout << "/rosapi/services: " << in_message->string() << std::endl;
+                            //std::cout << "/rosapi/services: " << in_message->string() << std::endl;
                             std::unique_lock<std::mutex> 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<std::mutex> 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<integral(EntryEnum::ServiceTopicName)>(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<WsClient>(server_port_path);
 #ifdef DEBUG
     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> 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<std::atomic_bool> stop)
+void RosbridgeWsClient::waitForService(const std::string &service, const std::function<bool(void)> 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<std::atomic_bool>(false);
-    waitForTopic(topic, stop);
+    waitForTopic(topic, []{
+        return false; // never stop
+    });
 }
 
-void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::shared_ptr<std::atomic_bool> stop)
+void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::function<bool(void)> 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

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<std::atomic_bool> stop);
+  void waitForService(const std::string& service, const std::function<bool(void)> 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<std::atomic_bool> stop);
+  void waitForTopic(const std::string& topic, const std::function<bool(void)> stop);
 };

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.

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);

src/comm/ros_bridge/src/ROSBridge.cpp

@@ -3,12 +3,11 @@
 ROSBridge::ROSBridge::ROSBridge() :
     _stopped(std::make_shared<std::atomic_bool>(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<std::chrono::milliseconds>(end-start).count();
     std::cout << "ROSBridge reset duration: " << delta << " ms" << std::endl;

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
                             }
                         }
                     }