diff --git a/deploy/QGroundControl.AppImage b/deploy/QGroundControl.AppImage index c29953e1f8d17dd5b027aec45ce95c7abe9895fa..64927aecc4bc12465c4cb1b6fe7efdfc8be8e23b 100755 Binary files a/deploy/QGroundControl.AppImage and b/deploy/QGroundControl.AppImage differ diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index bb4b87d035b9bb5951570457996396ac5fdef0f3..6943e7b4c9dc2f5954431d08778d3c757b93010c 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -724,16 +724,12 @@ void WimaController::_eventTimerHandler() if ( _snakeTicker.ready() ) { if ( _enableSnake.rawValue().toBool() ) { - if ( !_pRosBridge->isRunning() && _pRosBridge->ping() ) { + if ( !_pRosBridge->isRunning() && _pRosBridge->connected() ) { _setupTopicService(); - } - - if ( !_pRosBridge->ping() ){ + } else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){ _pRosBridge->reset(); } - - } else { - if ( _pRosBridge->isRunning() ) + } else if ( _pRosBridge->isRunning() ) { _pRosBridge->reset(); } } diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h index c0d6af5c7c40693f0077e4656a9eb326f5b8715a..10787566a30503d1f3e84da426c997d203784984 100644 --- a/src/comm/ros_bridge/include/ROSBridge.h +++ b/src/comm/ros_bridge/include/ROSBridge.h @@ -40,7 +40,7 @@ public: void reset(); //! @return Returns true if connected to the rosbridge_server, false either. //! @note This function can block up to 100ms. However normal execution time is smaller. - bool ping(); + bool connected(); bool isRunning(); diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp index d237a1ce8f63d8e67aaeb8c24f8ce622a6f62fec..5977cec469c33b4ae2e68f89586cc7a85c448a5a 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.cpp +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -8,7 +8,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr client, const std::__cxx11::string &message) { #ifndef DEBUG - //(void)client_name; + (void)client_name; #endif if (!client->on_open) { @@ -73,13 +73,52 @@ 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::__cxx11::string &server_port_path) : + server_port_path(server_port_path) + , stopped(std::make_shared(false)) + , hasConnection(std::make_shared(false)) { - this->server_port_path = server_port_path; + // Start thread to monitor connection status. + // There might be a better way to do this. + std::thread t([this] () { + const auto t_max = std::chrono::milliseconds(500); + while ( !this->stopped->load() ){ + auto start = std::chrono::high_resolution_clock::now(); + auto pPromise = std::make_shared>(); + auto future = pPromise->get_future(); + + auto status_client = std::make_shared(this->server_port_path); + status_client->on_open = [pPromise](std::shared_ptr) { + pPromise->set_value(); + }; + + std::thread poll_thread([status_client]{ + status_client->start(); + status_client->on_open = NULL; + status_client->on_message = NULL; + status_client->on_close = NULL; + status_client->on_error = NULL; + }); + + + auto status = future.wait_for(std::chrono::milliseconds(t_max)); + status_client->stop(); + poll_thread.join(); + this->hasConnection->store(status == std::future_status::ready); + auto end = std::chrono::high_resolution_clock::now(); + auto t_sleep = std::chrono::milliseconds(t_max)-(end-start); + if ( t_sleep > std::chrono::milliseconds(0)){ + std::this_thread::sleep_for(t_sleep); + } + } + std::cout << "connection monitor thread end" << std::endl; + }); + t.detach(); } RosbridgeWsClient::~RosbridgeWsClient() { + stopped->store(true); unsubscribeAll(); unadvertiseAll(); unadvertiseAllServices(); @@ -91,29 +130,8 @@ RosbridgeWsClient::~RosbridgeWsClient() bool RosbridgeWsClient::connected(){ - auto p = std::make_shared>(); - auto future = p->get_future(); - - auto status_client = std::make_shared(server_port_path); - status_client->on_open = [p](std::shared_ptr) { - p->set_value(); - }; - - std::thread t([status_client]{ - status_client->start(); - status_client->on_open = NULL; - status_client->on_message = NULL; - status_client->on_close = NULL; - status_client->on_error = NULL; - }); - - auto status = future.wait_for(std::chrono::milliseconds(20)); - status_client->stop(); - t.join(); - bool connected = status == std::future_status::ready; - //std::cout << "connected(): " << connected << std::endl; - return connected; - return true; + std::cout << "connected " << hasConnection->load() << std::endl; + return hasConnection->load(); } void RosbridgeWsClient::addClient(const std::string &client_name) @@ -672,8 +690,7 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service) void RosbridgeWsClient::waitForService(const std::string &service) { - auto stop = std::make_shared(false); - waitForService(service, stop); + waitForService(service, stopped); } void RosbridgeWsClient::waitForService(const std::string &service, const std::shared_ptr stop) diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index 7e90f965c0ba32f224aad3720b70c0f4f948883b..e548d58c9ba236120046b7e9ee7646d1f4d1229f 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -45,11 +45,13 @@ class RosbridgeWsClient WPClient = 3 }; - std::string server_port_path; + const std::string server_port_path; std::unordered_map /*client*/> client_map; std::deque service_topic_list; std::mutex mutex; + std::shared_ptr hasConnection; + std::shared_ptr stopped; void start(const std::string& client_name, std::shared_ptr client, const std::string& message); @@ -58,7 +60,6 @@ public: ~RosbridgeWsClient(); - // The execution can take up to 100 ms! bool connected(); // Adds a client to the client_map. diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp index 94035e01fc38efb9a6cdc56548672067ead7bba6..6924c3ed002b79ef200b7f03920375b3fdbbc305 100644 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ b/src/comm/ros_bridge/src/ROSBridge.cpp @@ -44,24 +44,16 @@ void ROSBridge::ROSBridge::start() void ROSBridge::ROSBridge::reset() { auto start = std::chrono::high_resolution_clock::now(); - std::cout << "ROSBridge::reset: reset publisher" << std::endl; _topicPublisher.reset(); - //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - std::cout << "ROSBridge::reset: reset subscriber" << std::endl; _topicSubscriber.reset(); - //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - std::cout << "ROSBridge::reset: reset server" << std::endl; _server.reset(); - //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - std::cout << "ROSBridge::reset: _stopped->store(true)" << std::endl; _stopped->store(true); - //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); 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; } -bool ROSBridge::ROSBridge::ping() +bool ROSBridge::ROSBridge::connected() { return _rbc.connected(); }