From 571dc451f0a8f74b94410995cf62037ee5390d09 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Fri, 21 Aug 2020 10:18:40 +0200 Subject: [PATCH] Ros Bridge tested with remote connection. --- qgroundcontrol.pro | 2 +- src/Settings/Wima.SettingsGroup.json | 2 +- src/Wima/WimaController.cc | 36 +++++++++----- src/Wima/WimaController.h | 1 - src/comm/ros_bridge/include/ROSBridge.h | 3 ++ .../ros_bridge/include/RosBridgeClient.cpp | 49 ++++++++++++++++--- src/comm/ros_bridge/include/RosBridgeClient.h | 2 + src/comm/ros_bridge/src/ROSBridge.cpp | 14 +++++- src/ui/preferences/GeneralSettings.qml | 7 +-- 9 files changed, 86 insertions(+), 30 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 214c83c0d..3d4c5dfd6 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -32,7 +32,7 @@ DebugBuild { } else { DESTDIR = $${OUT_PWD}/release - DEFINES += DEBUG + #DEFINES += DEBUG DEFINES += NDEBUG } diff --git a/src/Settings/Wima.SettingsGroup.json b/src/Settings/Wima.SettingsGroup.json index 01573eab5..661453d29 100644 --- a/src/Settings/Wima.SettingsGroup.json +++ b/src/Settings/Wima.SettingsGroup.json @@ -18,7 +18,7 @@ "type": "uint64", "units": "s", "defaultValue": 15 -} +}, { "name": "rosbridgeConnectionString", "shortDescription": "Ros Bridge Connection String (host:port).", diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 54f7425ae..b0424af3c 100644 --- a/src/Wima/WimaController.cc +++ b/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()); diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 73b4e0fb1..7996f361c 100644 --- a/src/Wima/WimaController.h +++ b/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. diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h index 10787566a..59faeeb88 100644 --- a/src/comm/ros_bridge/include/ROSBridge.h +++ b/src/comm/ros_bridge/include/ROSBridge.h @@ -20,6 +20,7 @@ public: typedef std::unique_ptr JsonDocUPtr; explicit ROSBridge(); + explicit ROSBridge(const char* connectionString); template void publish(T &msg, const std::string &topic){ @@ -55,4 +56,6 @@ private: ComPrivate::Server _server; }; + +bool isValidConnectionString(const char* connectionString); } diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp index 4509ae113..5b79aff28 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.cpp +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -1,9 +1,11 @@ #include "RosBridgeClient.h" + #include #include #include #include +#include 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(false); auto status_client = std::make_shared(this->server_port_path); status_client->on_open = [status_set, this](std::shared_ptr) { +#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(false); 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; +#ifdef DEBUG + std::cout << "/rosapi/topics: " << in_message->string() << std::endl; +#endif std::unique_lock 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(false); 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; +#ifdef DEBUG + std::cout << "/rosapi/services: " << in_message->string() << std::endl; +#endif std::unique_lock 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); +} diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index 0c6bc6e63..251922941 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -17,6 +17,8 @@ #include #include +bool is_valid_port_path(std::string server_port_path); + using WsClient = SimpleWeb::SocketClient; using InMessage = std::function, std::shared_ptr)>; diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp index bf54bad1c..40983a1c0 100644 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ b/src/comm/ros_bridge/src/ROSBridge.cpp @@ -1,15 +1,20 @@ #include "ros_bridge/include/ROSBridge.h" -ROSBridge::ROSBridge::ROSBridge() : +ROSBridge::ROSBridge::ROSBridge(const char *connectionString) : _stopped(std::make_shared(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); +} diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index 75114650f..a0efe586c 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/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 } } } -- 2.22.0