Working on ros_bridge, code not working in this state.

qgroundcontrol.pro

@@ -367,6 +367,7 @@ INCLUDEPATH += \
     src/ViewWidgets \
     src/Audio \
     src/comm \
+    src/comm/ros_bridge \
     src/input \
     src/lib/qmapcontrol \
     src/uas \
@@ -463,6 +464,8 @@ HEADERS += \
     src/Wima/testplanimetrycalculus.h \
     src/Settings/WimaSettings.h \
     src/QmlControls/QmlObjectVectorModel.h \
+    src/comm/ros_bridge/include/jsongenerator.h \
+    src/comm/ros_bridge/include/messages.h \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
@@ -500,7 +503,8 @@ SOURCES += \
     src/Wima/TestPolygonCalculus.cpp \
     src/Wima/testplanimetrycalculus.cpp \
     src/Settings/WimaSettings.cc \
-    src/QmlControls/QmlObjectVectorModel.cc
+    src/QmlControls/QmlObjectVectorModel.cc \
+    src/comm/ros_bridge/src/messages.cpp
 
 #
 # Unit Test specific configuration goes here (requires full debug build with all plugins)

src/comm/ros_bridge/Simple-WebSocket-Server/.clang-format

+IndentWidth: 2
+AccessModifierOffset: -2
+UseTab: Never
+ColumnLimit: 0
+MaxEmptyLinesToKeep: 2
+SpaceBeforeParens: Never
+BreakBeforeBraces: Custom
+BraceWrapping: {BeforeElse: true, BeforeCatch: true}
+NamespaceIndentation: All

src/comm/ros_bridge/Simple-WebSocket-Server/.gitignore

+# https://github.com/github/gitignore/blob/master/CMake.gitignore
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Makefile
+cmake_install.cmake
+install_manifest.txt
+*.cmake
+#Additions to https://github.com/github/gitignore/blob/master/CMake.gitignore
+Testing
+compile_commands.json
+.usages_clang
+
+*.crt
+*.key
+
+# executables
+ws_examples
+wss_examples
+crypto_test
+io_test
+parse_test

src/comm/ros_bridge/Simple-WebSocket-Server/.gitlab-ci.yml

+before_script:
+  - mkdir build && cd build
+  - export CXXFLAGS=-Werror
+  - export CTEST_OUTPUT_ON_FAILURE=1
+
+.script: &compile_and_test
+  script:
+  - cmake -DCMAKE_BUILD_TYPE=Release .. && make && make test
+  - rm -r *
+  - cmake -DCMAKE_BUILD_TYPE=Release -DUSE_STANDALONE_ASIO=ON .. && make && make test
+
+arch:
+  image: "registry.gitlab.com/eidheim/docker-images:arch"
+  <<: *compile_and_test
+
+buster:
+  image: "registry.gitlab.com/eidheim/docker-images:buster"
+  <<: *compile_and_test
+
+jessie:
+  image: "registry.gitlab.com/eidheim/docker-images:jessie"
+  <<: *compile_and_test
+
+stretch:
+  image: "registry.gitlab.com/eidheim/docker-images:stretch"
+  <<: *compile_and_test
+
+thread-safety-analysis:
+  image: "registry.gitlab.com/eidheim/docker-images:arch"
+  script:
+  - CXX=clang++ cmake .. && make
+
+static-analysis:
+  image: "registry.gitlab.com/eidheim/docker-images:arch"
+  script:
+    - scan-build cmake .. && scan-build --status-bugs make

src/comm/ros_bridge/Simple-WebSocket-Server/CMakeLists.txt

+cmake_minimum_required (VERSION 3.0)
+
+project (Simple-WebSocket-Server)
+
+option(USE_STANDALONE_ASIO "set ON to use standalone Asio instead of Boost.Asio" OFF)
+option(BUILD_TESTING "set ON to build library tests" OFF)
+
+if(NOT MSVC)
+    add_compile_options(-std=c++11 -Wall -Wextra -Wsign-conversion)
+    if (CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+        add_compile_options(-Wthread-safety)
+    endif()
+else()
+    add_compile_options(/W1)
+endif()
+
+add_library(simple-websocket-server INTERFACE)
+
+target_include_directories(simple-websocket-server INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
+
+find_package(Threads REQUIRED)
+target_link_libraries(simple-websocket-server INTERFACE ${CMAKE_THREAD_LIBS_INIT})
+
+# TODO 2020 when Debian Jessie LTS ends:
+# Remove Boost system, thread, regex components; use Boost::<component> aliases; remove Boost target_include_directories
+if(USE_STANDALONE_ASIO)
+    target_compile_definitions(simple-websocket-server INTERFACE USE_STANDALONE_ASIO)
+    find_path(ASIO_PATH asio.hpp)
+    if(NOT ASIO_PATH)
+        message(FATAL_ERROR "Standalone Asio not found")
+    else()
+        target_include_directories(simple-websocket-server INTERFACE ${ASIO_PATH})
+    endif()
+else()
+    find_package(Boost 1.54.0 COMPONENTS system thread coroutine context REQUIRED)
+    target_link_libraries(simple-websocket-server INTERFACE ${Boost_LIBRARIES})
+    target_include_directories(simple-websocket-server INTERFACE ${Boost_INCLUDE_DIR})
+    if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9)
+        target_compile_definitions(simple-websocket-server INTERFACE USE_BOOST_REGEX)
+        find_package(Boost 1.54.0 COMPONENTS regex REQUIRED)
+        target_link_libraries(simple-websocket-server INTERFACE ${Boost_LIBRARIES})
+        target_include_directories(simple-websocket-server INTERFACE ${Boost_INCLUDE_DIR})
+    endif()
+endif()
+if(WIN32)
+    target_link_libraries(simple-websocket-server INTERFACE ws2_32 wsock32)
+endif()
+
+if(APPLE)
+    set(OPENSSL_ROOT_DIR "/usr/local/opt/openssl")
+endif()
+find_package(OpenSSL REQUIRED)
+target_link_libraries(simple-websocket-server INTERFACE ${OPENSSL_LIBRARIES})
+target_include_directories(simple-websocket-server INTERFACE ${OPENSSL_INCLUDE_DIR})
+
+# If Simple-WebSocket-Server is not a sub-project:
+if("${CMAKE_SOURCE_DIR}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}")
+    add_executable(ws_examples ws_examples.cpp)
+    target_link_libraries(ws_examples simple-websocket-server)
+    if(OPENSSL_FOUND)
+        add_executable(wss_examples wss_examples.cpp)
+        target_link_libraries(wss_examples simple-websocket-server)
+    endif()
+    
+    set(BUILD_TESTING ON)
+    
+    install(FILES asio_compatibility.hpp server_ws.hpp client_ws.hpp server_wss.hpp client_wss.hpp crypto.hpp utility.hpp status_code.hpp mutex.hpp DESTINATION include/simple-websocket-server)
+endif()
+
+if(BUILD_TESTING)
+    enable_testing()
+    add_subdirectory(tests)
+endif()

src/comm/ros_bridge/Simple-WebSocket-Server/LICENSE

+The MIT License (MIT)
+
+Copyright (c) 2014-2019 Ole Christian Eidheim
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/comm/ros_bridge/Simple-WebSocket-Server/README.md

+Simple-WebSocket-Server
+=================
+
+A very simple, fast, multithreaded, platform independent WebSocket (WS) and WebSocket Secure (WSS) server and client library implemented using C++11, Asio (both Boost.Asio and standalone Asio can be used) and OpenSSL. Created to be an easy way to make WebSocket endpoints in C++.
+
+See https://gitlab.com/eidheim/Simple-Web-Server for an easy way to make REST resources available from C++ applications. Also, feel free to check out the new C++ IDE supporting C++11/14/17: https://gitlab.com/cppit/jucipp. 
+
+### Features
+
+* RFC 6455 mostly supported: text/binary frames, fragmented messages, ping-pong, connection close with status and reason.
+* Asynchronous message handling
+* Thread pool if needed
+* Platform independent
+* WebSocket Secure support
+* Timeouts, if any of SocketServer::timeout_request and SocketServer::timeout_idle are >0 (default: SocketServer::timeout_request=5 seconds, and SocketServer::timeout_idle=0 seconds; no timeout on idle connections)
+* Simple way to add WebSocket endpoints using regex for path, and anonymous functions
+* An easy to use WebSocket and WebSocket Secure client library
+* C++ bindings to the following OpenSSL methods: Base64, MD5, SHA1, SHA256 and SHA512 (found in crypto.hpp)
+
+### Usage
+
+See [ws_examples.cpp](ws_examples.cpp) or [wss_examples.cpp](wss_examples.cpp) for example usage. 
+
+### Dependencies
+
+* Boost.Asio or standalone Asio
+* OpenSSL libraries
+
+### Compile
+
+Compile with a C++11 supported compiler:
+
+```sh
+mkdir build
+cd build
+cmake ..
+make
+cd ..
+```
+
+#### Run server and client examples
+
+### WS
+
+```sh
+./build/ws_examples
+```
+
+### WSS
+
+Before running the WSS-examples, an RSA private key (server.key) and an SSL certificate (server.crt) must be created.
+
+Then:
+```
+./build/wss_examples
+```

src/comm/ros_bridge/Simple-WebSocket-Server/asio_compatibility.hpp

+#ifndef SIMPLE_WEB_ASIO_COMPATIBILITY_HPP
+#define SIMPLE_WEB_ASIO_COMPATIBILITY_HPP
+
+#include <memory>
+
+#ifdef USE_STANDALONE_ASIO
+#include <asio.hpp>
+#include <asio/steady_timer.hpp>
+namespace SimpleWeb {
+  namespace error = asio::error;
+  using error_code = std::error_code;
+  using errc = std::errc;
+  using system_error = std::system_error;
+  namespace make_error_code = std;
+} // namespace SimpleWeb
+#else
+#include <boost/asio.hpp>
+#include <boost/asio/steady_timer.hpp>
+namespace SimpleWeb {
+  namespace asio = boost::asio;
+  namespace error = asio::error;
+  using error_code = boost::system::error_code;
+  namespace errc = boost::system::errc;
+  using system_error = boost::system::system_error;
+  namespace make_error_code = boost::system::errc;
+} // namespace SimpleWeb
+#endif
+
+namespace SimpleWeb {
+#if(USE_STANDALONE_ASIO && ASIO_VERSION >= 101300) || BOOST_ASIO_VERSION >= 101300
+  using io_context = asio::io_context;
+  using resolver_results = asio::ip::tcp::resolver::results_type;
+  using async_connect_endpoint = asio::ip::tcp::endpoint;
+
+  inline void restart(io_context &context) noexcept {
+    context.restart();
+  }
+  inline asio::ip::address make_address(const std::string &str) noexcept {
+    return asio::ip::make_address(str);
+  }
+  template <typename socket_type>
+  asio::executor get_socket_executor(socket_type &socket) {
+    return socket.get_executor();
+  }
+  template <typename handler_type>
+  void async_resolve(asio::ip::tcp::resolver &resolver, const std::pair<std::string, std::string> &host_port, handler_type &&handler) {
+    resolver.async_resolve(host_port.first, host_port.second, std::forward<handler_type>(handler));
+  }
+#else
+  using io_context = asio::io_service;
+  using resolver_results = asio::ip::tcp::resolver::iterator;
+  using async_connect_endpoint = asio::ip::tcp::resolver::iterator;
+
+  inline void restart(io_context &context) noexcept {
+    context.reset();
+  }
+  inline asio::ip::address make_address(const std::string &str) noexcept {
+    return asio::ip::address::from_string(str);
+  }
+  template <typename socket_type>
+  io_context &get_socket_executor(socket_type &socket) {
+    return socket.get_io_service();
+  }
+  template <typename handler_type>
+  void async_resolve(asio::ip::tcp::resolver &resolver, const std::pair<std::string, std::string> &host_port, handler_type &&handler) {
+    resolver.async_resolve(asio::ip::tcp::resolver::query(host_port.first, host_port.second), std::forward<handler_type>(handler));
+  }
+#endif
+} // namespace SimpleWeb
+
+#endif /* SIMPLE_WEB_ASIO_COMPATIBILITY_HPP */

src/comm/ros_bridge/Simple-WebSocket-Server/client_ws.hpp

+#ifndef SIMPLE_WEB_CLIENT_WS_HPP
+#define SIMPLE_WEB_CLIENT_WS_HPP
+
+#include "asio_compatibility.hpp"
+#include "crypto.hpp"
+#include "mutex.hpp"
+#include "utility.hpp"
+#include <array>
+#include <atomic>
+#include <iostream>
+#include <limits>
+#include <list>
+#include <random>
+
+namespace SimpleWeb {
+  template <class socket_type>
+  class SocketClient;
+
+  template <class socket_type>
+  class SocketClientBase {
+  public:
+    class InMessage : public std::istream {
+      friend class SocketClientBase<socket_type>;
+      friend class SocketClient<socket_type>;
+      friend class Connection;
+
+    public:
+      unsigned char fin_rsv_opcode;
+      std::size_t size() noexcept {
+        return length;
+      }
+
+      /// Convenience function to return std::string. The stream buffer is consumed.
+      std::string string() noexcept {
+        try {
+          std::string str;
+          auto size = streambuf.size();
+          str.resize(size);
+          read(&str[0], static_cast<std::streamsize>(size));
+          return str;
+        }
+        catch(...) {
+          return std::string();
+        }
+      }
+
+    private:
+      InMessage() noexcept : std::istream(&streambuf), length(0) {}
+      InMessage(unsigned char fin_rsv_opcode, std::size_t length) noexcept : std::istream(&streambuf), fin_rsv_opcode(fin_rsv_opcode), length(length) {}
+      std::size_t length;
+      asio::streambuf streambuf;
+    };
+
+    /// The buffer is consumed during send operations.
+    class OutMessage : public std::iostream {
+      friend class SocketClientBase<socket_type>;
+
+      asio::streambuf streambuf;
+
+    public:
+      OutMessage() noexcept : std::iostream(&streambuf) {}
+      OutMessage(std::size_t capacity) noexcept : std::iostream(&streambuf) {
+        streambuf.prepare(capacity);
+      }
+
+      /// Returns the size of the buffer
+      std::size_t size() const noexcept {
+        return streambuf.size();
+      }
+    };
+
+    class Connection : public std::enable_shared_from_this<Connection> {
+      friend class SocketClientBase<socket_type>;
+      friend class SocketClient<socket_type>;
+
+    public:
+      std::string http_version, status_code;
+      CaseInsensitiveMultimap header;
+
+      asio::ip::tcp::endpoint remote_endpoint;
+
+      std::string remote_endpoint_address() noexcept {
+        try {
+          return remote_endpoint.address().to_string();
+        }
+        catch(...) {
+          return std::string();
+        }
+      }
+
+      unsigned short remote_endpoint_port() noexcept {
+        return remote_endpoint.port();
+      }
+
+    private:
+      template <typename... Args>
+      Connection(std::shared_ptr<ScopeRunner> handler_runner_, long timeout_idle, Args &&... args) noexcept
+          : handler_runner(std::move(handler_runner_)), socket(new socket_type(std::forward<Args>(args)...)), timeout_idle(timeout_idle), closed(false) {}
+
+      std::shared_ptr<ScopeRunner> handler_runner;
+
+      std::unique_ptr<socket_type> socket; // Socket must be unique_ptr since asio::ssl::stream<asio::ip::tcp::socket> is not movable
+
+      std::shared_ptr<InMessage> in_message;
+      std::shared_ptr<InMessage> fragmented_in_message;
+
+      long timeout_idle;
+      Mutex timer_mutex;
+      std::unique_ptr<asio::steady_timer> timer GUARDED_BY(timer_mutex);
+
+      void close() noexcept {
+        error_code ec;
+        socket->lowest_layer().shutdown(asio::ip::tcp::socket::shutdown_both, ec);
+        socket->lowest_layer().cancel(ec);
+      }
+
+      void set_timeout(long seconds = -1) noexcept {
+        bool use_timeout_idle = false;
+        if(seconds == -1) {
+          use_timeout_idle = true;
+          seconds = timeout_idle;
+        }
+
+        LockGuard lock(timer_mutex);
+
+        if(seconds == 0) {
+          timer = nullptr;
+          return;
+        }
+
+        timer = std::unique_ptr<asio::steady_timer>(new asio::steady_timer(get_socket_executor(*socket), std::chrono::seconds(seconds)));
+        std::weak_ptr<Connection> connection_weak(this->shared_from_this()); // To avoid keeping Connection instance alive longer than needed
+        timer->async_wait([connection_weak, use_timeout_idle](const error_code &ec) {
+          if(!ec) {
+            if(auto connection = connection_weak.lock()) {
+              if(use_timeout_idle)
+                connection->send_close(1000, "idle timeout"); // 1000=normal closure
+              else
+                connection->close();
+            }
+          }
+        });
+      }
+
+      void cancel_timeout() noexcept {
+        LockGuard lock(timer_mutex);
+        if(timer) {
+          try {
+            timer->cancel();
+          }
+          catch(...) {
+          }
+        }
+      }
+
+      class OutData {
+      public:
+        OutData(std::shared_ptr<OutMessage> out_message_, std::function<void(const error_code)> &&callback_) noexcept
+            : out_message(std::move(out_message_)), callback(std::move(callback_)) {}
+        std::shared_ptr<OutMessage> out_message;
+        std::function<void(const error_code)> callback;
+      };
+
+      Mutex send_queue_mutex;
+      std::list<OutData> send_queue GUARDED_BY(send_queue_mutex);
+
+      void send_from_queue() REQUIRES(send_queue_mutex) {
+        auto self = this->shared_from_this();
+        asio::async_write(*self->socket, send_queue.begin()->out_message->streambuf, [self](const error_code &ec, std::size_t /*bytes_transferred*/) {
+          auto lock = self->handler_runner->continue_lock();
+          if(!lock)
+            return;
+          {
+            LockGuard lock(self->send_queue_mutex);
+            if(!ec) {
+              auto it = self->send_queue.begin();
+              auto callback = std::move(it->callback);
+              self->send_queue.erase(it);
+              if(self->send_queue.size() > 0)
+                self->send_from_queue();
+
+              lock.unlock();
+              if(callback)
+                callback(ec);
+            }
+            else {
+              // All handlers in the queue is called with ec:
+              std::vector<std::function<void(const error_code &)>> callbacks;
+              for(auto &out_data : self->send_queue) {
+                if(out_data.callback)
+                  callbacks.emplace_back(std::move(out_data.callback));
+              }
+              self->send_queue.clear();
+
+              lock.unlock();
+              for(auto &callback : callbacks)
+                callback(ec);
+            }
+          }
+        });
+      }
+
+      std::atomic<bool> closed;
+
+      void read_remote_endpoint() noexcept {
+        try {
+          remote_endpoint = socket->lowest_layer().remote_endpoint();
+        }
+        catch(const std::exception &e) {
+          std::cerr << e.what() << std::endl;
+        }
+      }
+
+    public:
+      /// fin_rsv_opcode: 129=one fragment, text, 130=one fragment, binary, 136=close connection.
+      /// See http://tools.ietf.org/html/rfc6455#section-5.2 for more information.
+      void send(const std::shared_ptr<OutMessage> &out_message, const std::function<void(const error_code &)> &callback = nullptr, unsigned char fin_rsv_opcode = 129) {
+        cancel_timeout();
+        set_timeout();
+
+        // Create mask
+        std::array<unsigned char, 4> mask;
+        std::uniform_int_distribution<unsigned short> dist(0, 255);
+        std::random_device rd;
+        for(std::size_t c = 0; c < 4; c++)
+          mask[c] = static_cast<unsigned char>(dist(rd));
+
+        std::size_t length = out_message->size();
+
+        std::size_t max_additional_bytes = 14; // ws protocol adds at most 14 bytes
+        auto out_header_and_message = std::make_shared<OutMessage>(length + max_additional_bytes);
+
+        out_header_and_message->put(static_cast<char>(fin_rsv_opcode));
+        // Masked (first length byte>=128)
+        if(length >= 126) {
+          std::size_t num_bytes;
+          if(length > 0xffff) {
+            num_bytes = 8;
+            out_header_and_message->put(static_cast<char>(127 + 128));
+          }
+          else {
+            num_bytes = 2;
+            out_header_and_message->put(static_cast<char>(126 + 128));
+          }
+
+          for(std::size_t c = num_bytes - 1; c != static_cast<std::size_t>(-1); c--)
+            out_header_and_message->put((static_cast<unsigned long long>(length) >> (8 * c)) % 256);
+        }
+        else
+          out_header_and_message->put(static_cast<char>(length + 128));
+
+        for(std::size_t c = 0; c < 4; c++)
+          out_header_and_message->put(static_cast<char>(mask[c]));