Merge pull request #2073 from mavlink/pr-fix-rtk-python

rtk: parse Python RTCM strings

src/mavsdk/plugins/rtk/CMakeLists.txt

@@ -12,4 +12,9 @@ target_include_directories(mavsdk PUBLIC
 install(FILES
     include/plugins/rtk/rtk.h
     DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/mavsdk/plugins/rtk
-)
+)
+
+list(APPEND UNIT_TEST_SOURCES
+    ${CMAKE_CURRENT_SOURCE_DIR}/rtk_parse_test.cpp
+)
+set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE)

src/mavsdk/plugins/rtk/rtk_impl.cpp

@@ -39,13 +39,38 @@ Rtk::Result RtkImpl::send_rtcm_data(Rtk::RtcmData rtcm_data)
         return Rtk::Result::TooLong;
     }
 
-    // Copy length before we change it.
-    size_t bytes_to_send = rtcm_data.data.size();
+    // From Python, we get the bytes in a Python str such as:
+    // b'\xd3\x00\x13>\xd0\x00\x034\x14|\t\xe0\x81\x17\xca\xde[7=s\xf0?\x1b\t\x8a'
+
+    // Instead of changing the interface at this point for MAVSDK v1.4, we just
+    // try to detect Python strings instead and convert it back.
+
+    const std::string original_bytes = [&]() {
+        // Some use single inverted commas.
+        if (rtcm_data.data.size() >= 2 && char(rtcm_data.data[0]) == 'b' &&
+            char(rtcm_data.data[1]) == '\'' &&
+            char(rtcm_data.data[rtcm_data.data.size() - 1]) == '\'') {
+            return convert_from_python_string(rtcm_data.data);
+        }
+
+        // Others use double inverted commas.
+        else if (
+            rtcm_data.data.size() >= 2 && char(rtcm_data.data[0]) == 'b' &&
+            char(rtcm_data.data[1]) == '"' &&
+            char(rtcm_data.data[rtcm_data.data.size() - 1]) == '"') {
+            return convert_from_python_string(rtcm_data.data);
+
+        } else {
+            return rtcm_data.data;
+        }
+    }();
 
     // The mavlink helpers memcpy, so we need to make sure we're not
     // copying from where we shouldn't.
     rtcm_data.data.resize(num_packets_required * field_len);
 
+    size_t bytes_to_send = original_bytes.size();
+
     for (size_t i = 0; i < num_packets_required; ++i) {
         const uint8_t flags =
             (num_packets_required > 1 ? 0x1 : 0x0) | ((i & 0x3) << 1) | ((_sequence & 0x1F) << 3);
@@ -57,7 +82,7 @@ Rtk::Result RtkImpl::send_rtcm_data(Rtk::RtcmData rtcm_data)
             &message,
             flags,
             static_cast<uint8_t>(std::min(field_len, bytes_to_send)),
-            reinterpret_cast<const uint8_t*>(rtcm_data.data.c_str() + (i * field_len)));
+            reinterpret_cast<const uint8_t*>(original_bytes.c_str() + (i * field_len)));
 
         if (!_parent->send_message(message)) {
             ++_sequence;
@@ -71,4 +96,67 @@ Rtk::Result RtkImpl::send_rtcm_data(Rtk::RtcmData rtcm_data)
     return Rtk::Result::Success;
 }
 
+std::string RtkImpl::convert_from_python_string(const std::string& pstr)
+{
+    std::string bytes;
+
+    // Skip first two bytes and last.
+    const size_t end = pstr.size() - 1;
+    size_t i = 2;
+    while (i < end) {
+        // According to:
+        // https://www.w3schools.com/python/gloss_python_escape_characters.asp
+
+        // \' -> '.
+        if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == '\'') {
+            bytes += '\'';
+            i += 2;
+
+            // \\ -> \.
+        } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == '\\') {
+            bytes += '\\';
+            i += 2;
+
+            // \n -> 0x0A
+        } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'n') {
+            bytes += 0x0A;
+            i += 2;
+
+            // \r -> 0x0D
+        } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'r') {
+            bytes += 0x0D;
+            i += 2;
+
+            // \t -> 0x09
+        } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 't') {
+            bytes += 0x09;
+            i += 2;
+
+            // \b -> 0x08
+        } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'b') {
+            bytes += 0x08;
+            i += 2;
+
+            // \f -> 0x0C
+        } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'f') {
+            bytes += 0x0C;
+            i += 2;
+
+            // \x00 -> hex number
+        } else if (i + 3 < end && pstr[i] == '\\' && pstr[i + 1] == 'x') {
+            std::string hex = {pstr[i + 2], pstr[i + 3]};
+            const uint8_t byte = std::stoul(hex, nullptr, 16);
+            bytes += byte;
+            i += 4;
+
+            // some valid char that we can use as is
+        } else {
+            bytes += pstr[i];
+            i += 1;
+        }
+    }
+
+    return bytes;
+}
+
 } // namespace mavsdk

src/mavsdk/plugins/rtk/rtk_impl.h

@@ -19,6 +19,8 @@ class RtkImpl : public PluginImplBase {
 
     Rtk::Result send_rtcm_data(Rtk::RtcmData rtcm_data);
 
+    static std::string convert_from_python_string(const std::string& python_string);
+
 private:
     unsigned _sequence{0};
 };

src/mavsdk/plugins/rtk/rtk_parse_test.cpp

@@ -0,0 +1,37 @@
+#include <string>
+#include <gtest/gtest.h>
+
+#include "plugins/rtk/rtk.h"
+#include "rtk_impl.h"
+
+using namespace mavsdk;
+
+// String generated using:
+//
+// python -c "print(bytes(range(256)))" with " and \ escaped
+
+static std::string sample_python_string =
+    "b'\\x00\\x01\\x02\\x03\\x04\\x05\\x06\\x07\\x08\\t\\n\\x0b\\x0c\\r\\x0e"
+    "\\x0f\\x10\\x11\\x12\\x13\\x14\\x15\\x16\\x17\\x18\\x19\\x1a\\x1b\\x1c"
+    "\\x1d\\x1e\\x1f !\"#$%&\\'()*+,-./0123456789:;<=>?@"
+    "ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\\\]^_`abcdefghijklmnopqrstuvwxyz{|}~\\x7f"
+    "\\x80\\x81\\x82\\x83\\x84\\x85\\x86\\x87\\x88\\x89\\x8a\\x8b\\x8c\\x8d"
+    "\\x8e\\x8f\\x90\\x91\\x92\\x93\\x94\\x95\\x96\\x97\\x98\\x99\\x9a\\x9b"
+    "\\x9c\\x9d\\x9e\\x9f\\xa0\\xa1\\xa2\\xa3\\xa4\\xa5\\xa6\\xa7\\xa8\\xa9"
+    "\\xaa\\xab\\xac\\xad\\xae\\xaf\\xb0\\xb1\\xb2\\xb3\\xb4\\xb5\\xb6\\xb7"
+    "\\xb8\\xb9\\xba\\xbb\\xbc\\xbd\\xbe\\xbf\\xc0\\xc1\\xc2\\xc3\\xc4\\xc5"
+    "\\xc6\\xc7\\xc8\\xc9\\xca\\xcb\\xcc\\xcd\\xce\\xcf\\xd0\\xd1\\xd2\\xd3"
+    "\\xd4\\xd5\\xd6\\xd7\\xd8\\xd9\\xda\\xdb\\xdc\\xdd\\xde\\xdf\\xe0\\xe1"
+    "\\xe2\\xe3\\xe4\\xe5\\xe6\\xe7\\xe8\\xe9\\xea\\xeb\\xec\\xed\\xee\\xef"
+    "\\xf0\\xf1\\xf2\\xf3\\xf4\\xf5\\xf6\\xf7\\xf8\\xf9\\xfa\\xfb\\xfc\\xfd"
+    "\\xfe\\xff'";
+
+TEST(Rtk, ParsePythonString)
+{
+    auto result = RtkImpl::convert_from_python_string(sample_python_string);
+
+    uint8_t counter = 0;
+    for (uint8_t ch : result) {
+        EXPECT_EQ(ch, counter++);
+    }
+}