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_wss.hpp

+#ifndef SIMPLE_WEB_CLIENT_WSS_HPP
+#define SIMPLE_WEB_CLIENT_WSS_HPP
+
+#include "client_ws.hpp"
+
+#ifdef USE_STANDALONE_ASIO
+#include <asio/ssl.hpp>
+#else
+#include <boost/asio/ssl.hpp>
+#endif
+
+namespace SimpleWeb {
+  using WSS = asio::ssl::stream<asio::ip::tcp::socket>;
+
+  template <>
+  class SocketClient<WSS> : public SocketClientBase<WSS> {
+  public:
+    SocketClient(const std::string &server_port_path, bool verify_certificate = true,
+                 const std::string &cert_file = std::string(), const std::string &private_key_file = std::string(),
+                 const std::string &verify_file = std::string())
+        : SocketClientBase<WSS>::SocketClientBase(server_port_path, 443), context(asio::ssl::context::tlsv12) {
+      if(cert_file.size() > 0 && private_key_file.size() > 0) {
+        context.use_certificate_chain_file(cert_file);
+        context.use_private_key_file(private_key_file, asio::ssl::context::pem);
+      }
+
+      if(verify_certificate)
+        context.set_verify_callback(asio::ssl::rfc2818_verification(host));
+
+      if(verify_file.size() > 0)
+        context.load_verify_file(verify_file);
+      else
+        context.set_default_verify_paths();
+
+      if(verify_file.size() > 0 || verify_certificate)
+        context.set_verify_mode(asio::ssl::verify_peer);
+      else
+        context.set_verify_mode(asio::ssl::verify_none);
+    };
+
+  protected:
+    asio::ssl::context context;
+
+    void connect() override {
+      LockGuard connection_lock(connection_mutex);
+      auto connection = this->connection = std::shared_ptr<Connection>(new Connection(handler_runner, config.timeout_idle, *io_service, context));
+      connection_lock.unlock();
+
+      std::pair<std::string, std::string> host_port;
+      if(config.proxy_server.empty())
+        host_port = {host, std::to_string(port)};
+      else {
+        auto proxy_host_port = parse_host_port(config.proxy_server, 8080);
+        host_port = {proxy_host_port.first, std::to_string(proxy_host_port.second)};
+      }
+
+      auto resolver = std::make_shared<asio::ip::tcp::resolver>(*io_service);
+      connection->set_timeout(config.timeout_request);
+      async_resolve(*resolver, host_port, [this, connection, resolver](const error_code &ec, resolver_results results) {
+        connection->cancel_timeout();
+        auto lock = connection->handler_runner->continue_lock();
+        if(!lock)
+          return;
+        if(!ec) {
+          connection->set_timeout(this->config.timeout_request);
+          asio::async_connect(connection->socket->lowest_layer(), results, [this, connection, resolver](const error_code &ec, async_connect_endpoint /*endpoint*/) {
+            connection->cancel_timeout();
+            auto lock = connection->handler_runner->continue_lock();
+            if(!lock)
+              return;
+            if(!ec) {
+              asio::ip::tcp::no_delay option(true);
+              error_code ec;
+              connection->socket->lowest_layer().set_option(option, ec);
+
+              if(!this->config.proxy_server.empty()) {
+                auto write_buffer = std::make_shared<asio::streambuf>();
+                std::ostream write_stream(write_buffer.get());
+                auto host_port = this->host + ':' + std::to_string(this->port);
+                write_stream << "CONNECT " + host_port + " HTTP/1.1\r\n"
+                             << "Host: " << host_port << "\r\n\r\n";
+                connection->set_timeout(this->config.timeout_request);
+                asio::async_write(connection->socket->next_layer(), *write_buffer, [this, connection, write_buffer](const error_code &ec, std::size_t /*bytes_transferred*/) {
+                  connection->cancel_timeout();
+                  auto lock = connection->handler_runner->continue_lock();
+                  if(!lock)
+                    return;
+                  if(!ec) {
+                    connection->set_timeout(this->config.timeout_request);
+                    asio::async_read_until(connection->socket->next_layer(), connection->in_message->streambuf, "\r\n\r\n", [this, connection](const error_code &ec, std::size_t /*bytes_transferred*/) {
+                      connection->cancel_timeout();
+                      auto lock = connection->handler_runner->continue_lock();
+                      if(!lock)
+                        return;
+                      if(!ec) {
+                        if(!ResponseMessage::parse(*connection->in_message, connection->http_version, connection->status_code, connection->header))
+                          this->connection_error(connection, make_error_code::make_error_code(errc::protocol_error));
+                        else {
+                          if(connection->status_code.compare(0, 3, "200") != 0)
+                            this->connection_error(connection, make_error_code::make_error_code(errc::permission_denied));
+                          else
+                            this->handshake(connection);
+                        }
+                      }
+                      else
+                        this->connection_error(connection, ec);
+                    });
+                  }
+                  else
+                    this->connection_error(connection, ec);
+                });
+              }
+              else
+                this->handshake(connection);
+            }
+            else
+              this->connection_error(connection, ec);
+          });
+        }
+        else
+          this->connection_error(connection, ec);
+      });
+    }
+
+    void handshake(const std::shared_ptr<Connection> &connection) {
+      SSL_set_tlsext_host_name(connection->socket->native_handle(), this->host.c_str());
+
+      connection->set_timeout(this->config.timeout_request);
+      connection->socket->async_handshake(asio::ssl::stream_base::client, [this, connection](const error_code &ec) {
+        connection->cancel_timeout();
+        auto lock = connection->handler_runner->continue_lock();
+        if(!lock)
+          return;
+        if(!ec)
+          upgrade(connection);
+        else
+          this->connection_error(connection, ec);
+      });
+    }
+  };
+} // namespace SimpleWeb
+
+#endif /* SIMPLE_WEB_CLIENT_WSS_HPP */

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

+#ifndef SIMPLE_WEB_CRYPTO_HPP
+#define SIMPLE_WEB_CRYPTO_HPP
+
+#include <cmath>
+#include <iomanip>
+#include <istream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include <openssl/buffer.h>
+#include <openssl/evp.h>
+#include <openssl/md5.h>
+#include <openssl/sha.h>
+
+namespace SimpleWeb {
+// TODO 2017: remove workaround for MSVS 2012
+#if _MSC_VER == 1700                       // MSVS 2012 has no definition for round()
+  inline double round(double x) noexcept { // Custom definition of round() for positive numbers
+    return floor(x + 0.5);
+  }
+#endif
+
+  class Crypto {
+    const static std::size_t buffer_size = 131072;
+
+  public:
+    class Base64 {
+    public:
+      static std::string encode(const std::string &ascii) noexcept {
+        std::string base64;
+
+        BIO *bio, *b64;
+        BUF_MEM *bptr = BUF_MEM_new();
+
+        b64 = BIO_new(BIO_f_base64());
+        BIO_set_flags(b64, BIO_FLAGS_BASE64_NO_NL);
+        bio = BIO_new(BIO_s_mem());
+        BIO_push(b64, bio);
+        BIO_set_mem_buf(b64, bptr, BIO_CLOSE);
+
+        // Write directly to base64-buffer to avoid copy
+        auto base64_length = static_cast<std::size_t>(round(4 * ceil(static_cast<double>(ascii.size()) / 3.0)));
+        base64.resize(base64_length);
+        bptr->length = 0;
+        bptr->max = base64_length + 1;
+        bptr->data = &base64[0];
+
+        if(BIO_write(b64, &ascii[0], static_cast<int>(ascii.size())) <= 0 || BIO_flush(b64) <= 0)
+          base64.clear();
+
+        // To keep &base64[0] through BIO_free_all(b64)
+        bptr->length = 0;
+        bptr->max = 0;
+        bptr->data = nullptr;
+
+        BIO_free_all(b64);
+
+        return base64;
+      }
+
+      static std::string decode(const std::string &base64) noexcept {
+        std::string ascii;
+
+        // Resize ascii, however, the size is a up to two bytes too large.
+        ascii.resize((6 * base64.size()) / 8);
+        BIO *b64, *bio;
+
+        b64 = BIO_new(BIO_f_base64());
+        BIO_set_flags(b64, BIO_FLAGS_BASE64_NO_NL);
+// TODO: Remove in 2020
+#if(defined(OPENSSL_VERSION_NUMBER) && OPENSSL_VERSION_NUMBER <= 0x1000115fL) || (defined(LIBRESSL_VERSION_NUMBER) && LIBRESSL_VERSION_NUMBER < 0x2080000fL)
+        bio = BIO_new_mem_buf((char *)&base64[0], static_cast<int>(base64.size()));
+#else
+        bio = BIO_new_mem_buf(&base64[0], static_cast<int>(base64.size()));
+#endif
+        bio = BIO_push(b64, bio);
+
+        auto decoded_length = BIO_read(bio, &ascii[0], static_cast<int>(ascii.size()));
+        if(decoded_length > 0)
+          ascii.resize(static_cast<std::size_t>(decoded_length));
+        else
+          ascii.clear();
+
+        BIO_free_all(b64);
+
+        return ascii;
+      }
+    };
+
+    /// Return hex string from bytes in input string.
+    static std::string to_hex_string(const std::string &input) noexcept {
+      std::stringstream hex_stream;
+      hex_stream << std::hex << std::internal << std::setfill('0');
+      for(auto &byte : input)
+        hex_stream << std::setw(2) << static_cast<int>(static_cast<unsigned char>(byte));
+      return hex_stream.str();
+    }
+
+    static std::string md5(const std::string &input, std::size_t iterations = 1) noexcept {
+      std::string hash;
+
+      hash.resize(128 / 8);
+      MD5(reinterpret_cast<const unsigned char *>(&input[0]), input.