Ros Bridge tested with remote connection.

qgroundcontrol.pro

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

src/Settings/Wima.SettingsGroup.json

@@ -18,7 +18,7 @@
     "type":             "uint64",
     "units":            "s",
     "defaultValue":     15
-}
+},
 {
     "name":             "rosbridgeConnectionString",
     "shortDescription": "Ros Bridge Connection String (host:port).",

src/Wima/WimaController.cc

@@ -22,7 +22,6 @@
 
 
 
-// const char* WimaController::wimaFileExtension           = "wima";
 const char* WimaController::areaItemsName               = "AreaItems";
 const char* WimaController::missionItemsName            = "MissionItems";
 const char* WimaController::settingsGroup               = "WimaController";
@@ -63,8 +62,9 @@ WimaController::WimaController(QObject *parent)
     , _rtlManager               (_managerSettings, _areaInterface)
     , _currentManager           (&_defaultManager)
     , _managerList              {&_defaultManager, &_snakeManager, &_rtlManager}
-    , _metaDataMap              (FactMetaData::createMapFromJsonFile(
-                                     QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
+    , _metaDataMap              (
+          FactMetaData::createMapFromJsonFile(
+              QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
     , _enableWimaController     (settingsGroup, _metaDataMap[enableWimaControllerName])
     , _overlapWaypoints         (settingsGroup, _metaDataMap[overlapWaypointsName])
     , _maxWaypointsPerPhase     (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
@@ -84,12 +84,26 @@ WimaController::WimaController(QObject *parent)
     , _snakeCalcInProgress      (false)
     , _nemoHeartbeat            (0 /*status: not connected*/)
     , _fallbackStatus           (0 /*status: not connected*/)
-    , _pRosBridge               (new ROSBridge::ROSBridge())
-    , _pCasePacker              (_pRosBridge->casePacker())
     , _batteryLevelTicker       (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
     , _snakeTicker              (EVENT_TIMER_INTERVAL, 200 /*ms*/)
     , _nemoTimeoutTicker        (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
-{
+{    
+
+    // ROS Bridge.
+    WimaSettings* wimaSettings  = qgcApp()->toolbox()->settingsManager()->wimaSettings();
+    auto connectionStringFact       = wimaSettings->rosbridgeConnectionString();
+    auto setConnectionString = [connectionStringFact, this]{
+        auto connectionString = connectionStringFact->rawValue().toString();
+        if ( ROSBridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
+            this->_pRosBridge.reset(new ROSBridge::ROSBridge(connectionString.toLocal8Bit().data()));
+        } else {
+            qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
+            this->_pRosBridge.reset(new ROSBridge::ROSBridge("localhost:9090"));
+        }
+    };
+    setConnectionString();
+    connect(wimaSettings->rosbridgeConnectionString(), &SettingsFact::rawValueChanged, setConnectionString);
+
     // Set up facts.
     _showAllMissionItems.setRawValue(true);
     _showCurrentMissionItems.setRawValue(true);
@@ -964,11 +978,11 @@ void WimaController::_setupTopicService()
                                   [this](JsonDocUPtr) -> JsonDocUPtr {
         JsonDocUPtr pDoc;
         if ( this->_snakeTilesLocal.polygons().size() > 0){
-            pDoc = this->_pCasePacker->pack(this->_snakeOrigin, "");
+            pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeOrigin, "");
         } else {
-            pDoc = this->_pCasePacker->pack(::GeoPoint3D(0,0,0), "");
+            pDoc = this->_pRosBridge->casePacker()->pack(::GeoPoint3D(0,0,0), "");
         }
-        this->_pCasePacker->removeTag(pDoc);
+        this->_pRosBridge->casePacker()->removeTag(pDoc);
         rapidjson::Document value(rapidjson::kObjectType);
         JsonDocUPtr         pReturn(new rapidjson::Document(rapidjson::kObjectType));
         value.CopyFrom(*pDoc, pReturn->GetAllocator());
@@ -979,8 +993,8 @@ void WimaController::_setupTopicService()
 
     _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
                                   [this](JsonDocUPtr) -> JsonDocUPtr {
-        JsonDocUPtr pDoc = this->_pCasePacker->pack(this->_snakeTilesLocal, "");
-        this->_pCasePacker->removeTag(pDoc);
+        JsonDocUPtr pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeTilesLocal, "");
+        this->_pRosBridge->casePacker()->removeTag(pDoc);
         rapidjson::Document value(rapidjson::kObjectType);
         JsonDocUPtr         pReturn(new rapidjson::Document(rapidjson::kObjectType));
         value.CopyFrom(*pDoc, pReturn->GetAllocator());

src/Wima/WimaController.h

@@ -406,7 +406,6 @@ private:
     QNemoHeartbeat          _nemoHeartbeat; // measurement progress
     int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
-    const CasePacker        *_pCasePacker;
     static StatusMap        _nemoStatusMap;
 
     // Periodic tasks.

src/comm/ros_bridge/include/ROSBridge.h

@@ -20,6 +20,7 @@ public:
     typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
 
     explicit ROSBridge();
+    explicit ROSBridge(const char* connectionString);
 
     template<class T>
     void publish(T &msg, const std::string &topic){
@@ -55,4 +56,6 @@ private:
     ComPrivate::Server             _server;
 
 };
+
+bool isValidConnectionString(const char* connectionString);
 }

src/comm/ros_bridge/include/RosBridgeClient.cpp

 #include "RosBridgeClient.h"
 
+
 #include <chrono>
 #include <functional>
 #include <thread>
 #include <future>
+#include <regex>
 
 
 struct Task{
@@ -120,8 +122,10 @@ void RosbridgeWsClient::run()
             // ====================================================================================
             if ( std::chrono::high_resolution_clock::now() > poll_time_point) {
                 poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
+#ifdef DEBUG
                 std::cout << "Starting new poll." << std::endl;
                 std::cout << "connected: " << this->isConnected->load() << std::endl;
+#endif
                 std::string reset_status_task_name = "reset_status_task";
                 // Add status task if necessary.
                 auto const it = std::find_if(task_list.begin(), task_list.end(),
@@ -129,12 +133,16 @@ void RosbridgeWsClient::run()
                     return t.name == reset_status_task_name;
                 });
                 if ( it == task_list.end() ){
+#ifdef DEBUG
                     std::cout << "Adding status_task" << std::endl;
+#endif
                     // Check connection status.
                     auto status_set = std::make_shared<std::atomic_bool>(false);
                     auto status_client = std::make_shared<WsClient>(this->server_port_path);
                     status_client->on_open = [status_set, this](std::shared_ptr<WsClient::Connection>) {
+#ifdef DEBUG
                             std::cout << "status_client opened" << std::endl;
+#endif
                             this->isConnected->store(true);
                             status_set->store(true);
                     };
@@ -182,12 +190,16 @@ void RosbridgeWsClient::run()
                     });
                     if ( topics_it == task_list.end() ){
                         // Call /rosapi/topics service.
+#ifdef DEBUG
                         std::cout << "Adding reset_topics_task" << std::endl;
+#endif
                         auto topics_set = std::make_shared<std::atomic_bool>(false);
                         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;
+#ifdef DEBUG
+                            std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
+#endif
                             std::unique_lock<std::mutex> lk(this->mutex);
                             this->available_topics = in_message->string();
                             lk.unlock();
@@ -229,12 +241,16 @@ void RosbridgeWsClient::run()
                     });
                     if ( services_it == task_list.end() ){
                         // Call /rosapi/services service.
+#ifdef DEBUG
                         std::cout << "Adding reset_services_task" << std::endl;
+#endif
                         auto services_set = std::make_shared<std::atomic_bool>(false);
                         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;
+#ifdef DEBUG
+                            std::cout << "/rosapi/services: " << in_message->string() << std::endl;
+#endif
                             std::unique_lock<std::mutex> lk(this->mutex);
                             this->available_services = in_message->string();
                             lk.unlock();
@@ -279,22 +295,29 @@ void RosbridgeWsClient::run()
             // Process tasks.
             // ====================================================================================
             for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){
-                //std::cout << "processing task: " << task_it->name << std::endl;
+#ifdef DEBUG
+                std::cout << "processing task: " << task_it->name << std::endl;
+#endif
                 if ( !task_it->expired() ){
                     if ( task_it->ready() ){
-                        //std::cout << "executing task: " << task_it->name << std::endl;
+#ifdef DEBUG
+                        std::cout << "executing task: " << task_it->name << std::endl;
+#endif
                         task_it->execute();
                         task_it = task_list.erase(task_it);
                     } else {
-                        //std::cout << "noting to do for task: " << task_it->name << std::endl;
+#ifdef DEBUG
+                        std::cout << "noting to do for task: " << task_it->name << std::endl;
+#endif
                         ++task_it;
                     }
                 } else {
-                    //std::cout << "task expired: " << task_it->name << std::endl;
+#ifdef DEBUG
+                    std::cout << "task expired: " << task_it->name << std::endl;
+#endif
                     task_it->clear_up();
                     task_it = task_list.erase(task_it);
                 }
-                //std::cout << std::endl;
             }
 
             std::this_thread::sleep_for(std::chrono::milliseconds(10));
@@ -305,7 +328,9 @@ void RosbridgeWsClient::run()
             task_it->clear_up();
         }
         task_list.clear();
+#ifdef DEBUG
         std::cout << "periodic thread end" << std::endl;
+#endif
     });
 
 }
@@ -571,7 +596,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
 #ifdef DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message." << std::endl;
-        //std::cout << client_name << ": Sending message: " << message << std::endl;
+        std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
         connection->send(message);
 
@@ -949,3 +974,11 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::functi
               << counter << std::endl;
 #endif
 }
+
+bool is_valid_port_path(std::string server_port_path)
+{
+    std::regex url_regex("^(((([a-z]|[A-z])+([0-9]|_)*\\.*([a-z]|[A-z])+([0-9]|_)*))"
+    "|(((1?[0-9]{1,2}|2[0-4][0-9]|25[0-5])\\.){3}(1?[0-9]{1,2}|2[0-4][0-9]|25[0-5]){1}))"
+    ":[0-9]+$");
+    return std::regex_match(server_port_path, url_regex);
+}

src/comm/ros_bridge/include/RosBridgeClient.h

@@ -17,6 +17,8 @@
 #include <deque>
 #include <thread>
 
+bool is_valid_port_path(std::string server_port_path);
+
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
 using InMessage =  std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
 

src/comm/ros_bridge/src/ROSBridge.cpp

 #include "ros_bridge/include/ROSBridge.h"
 
-ROSBridge::ROSBridge::ROSBridge() :
+ROSBridge::ROSBridge::ROSBridge(const char *connectionString) :
     _stopped(std::make_shared<std::atomic_bool>(true))
   ,  _casePacker(&_typeFactory, &_jsonFactory)
-  , _rbc("localhost:9090", false /*run*/)
+  , _rbc(connectionString, false /*run*/)
   , _topicPublisher(_casePacker, _rbc)
   , _topicSubscriber(_casePacker, _rbc)
   , _server(_casePacker, _rbc)
 {
 }
 
+ROSBridge::ROSBridge::ROSBridge() :
+    ROSBridge::ROSBridge::ROSBridge("localhost:9090")
+{
+}
+
 void ROSBridge::ROSBridge::publish(ROSBridge::ROSBridge::JsonDocUPtr doc)
 {
     _topicPublisher.publish(std::move(doc));
@@ -64,3 +69,8 @@ bool ROSBridge::ROSBridge::isRunning()
     return !_stopped->load();
 }
 
+
+bool ROSBridge::isValidConnectionString(const char *connectionString)
+{
+    return is_valid_port_path(connectionString);
+}

src/ui/preferences/GeneralSettings.qml

@@ -983,13 +983,8 @@ QGCView {
                                 visible:    _userBrandImageIndoor.visible
                             }
                             FactTextField {
-                                Layout.preferredWidth:  _valueFieldWidth
                                 fact:                   QGroundControl.settingsManager.wimaSettings.rosbridgeConnectionString
-                                Layout.columnSpan:      2
-                            }
-                            Item{
-                                // dummy
-                                width: 1
+                                Layout.columnSpan:      3
                             }
                         }
                     }