Feat/lwjson

codegen/cmake/AddService.cmake

@@ -1,24 +1,31 @@
 function(add_service SERVICE_NAME JSON_FILE)
-    add_library(${SERVICE_NAME} OBJECT EXCLUDE_FROM_ALL
-    )
+    add_library(${SERVICE_NAME} OBJECT EXCLUDE_FROM_ALL)
     target_add_service(${SERVICE_NAME} ${SERVICE_NAME} ${JSON_FILE})
 endfunction()
 
 function(target_add_service TARGET_NAME SERVICE_NAME JSON_FILE)
     # generate the output directory, otherwise python will complain when multiple instances are run
     file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/generated/include)
-    add_custom_command(
-            OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_NAME}Base.hpp ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_NAME}Base.cpp
-            COMMAND ${Python3_EXECUTABLE} -m cogapp -d -I ${XBOT_CODEGEN_PATH}/xbot_codegen -D service_file=${JSON_FILE} -o ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_NAME}Base.hpp ${XBOT_CODEGEN_PATH}/templates/ServiceTemplate.hpp
-            COMMAND ${Python3_EXECUTABLE} -m cogapp -d -I ${XBOT_CODEGEN_PATH}/xbot_codegen -D service_file=${JSON_FILE} -o ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_NAME}Base.cpp ${XBOT_CODEGEN_PATH}/templates/ServiceTemplate.cpp
-            DEPENDS ${XBOT_CODEGEN_PATH}/templates/ServiceTemplate.hpp ${XBOT_CODEGEN_PATH}/templates/ServiceTemplate.cpp ${JSON_FILE}
-            COMMENT "Generating code for service ${SERVICE_NAME}."
-    )
 
-    target_sources(${TARGET_NAME} PRIVATE ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_NAME}Base.cpp
-            ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_NAME}Base.hpp
+    set(COG_BASE_ARGS -m cogapp -d -I ${XBOT_CODEGEN_PATH}/xbot_codegen -Dservice_file=${JSON_FILE})
+    set(HEADER_TEMPLATE ${XBOT_CODEGEN_PATH}/templates/ServiceTemplate.hpp)
+    set(SOURCE_TEMPLATE ${XBOT_CODEGEN_PATH}/templates/ServiceTemplate.cpp)
+    set(HEADER_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_NAME}Base.hpp)
+    set(SOURCE_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_NAME}Base.cpp)
+
+    if (DEFINED XBOT_SERVICE_EXT)
+        list(APPEND COG_BASE_ARGS -Dservice_ext="${XBOT_SERVICE_EXT}")
+    endif ()
+
+    add_custom_command(
+        OUTPUT ${HEADER_OUTPUT} ${SOURCE_OUTPUT}
+        COMMAND ${Python3_EXECUTABLE} ${COG_BASE_ARGS} -o ${HEADER_OUTPUT} ${HEADER_TEMPLATE}
+        COMMAND ${Python3_EXECUTABLE} ${COG_BASE_ARGS} -o ${SOURCE_OUTPUT} ${SOURCE_TEMPLATE}
+        DEPENDS ${HEADER_TEMPLATE} ${SOURCE_TEMPLATE} ${XBOT_CODEGEN_PATH}/xbot_codegen/xbot_codegen.py ${JSON_FILE}
+        COMMENT "Generating code for service ${SERVICE_NAME}."
     )
 
+    target_sources(${TARGET_NAME} PRIVATE ${SOURCE_OUTPUT} ${HEADER_OUTPUT})
     target_include_directories(${TARGET_NAME} PUBLIC ${CMAKE_CURRENT_BINARY_DIR}/generated/include)
     target_link_libraries(${TARGET_NAME} PUBLIC xbot-service)
 endfunction()

codegen/cmake/AddServiceInterface.cmake

@@ -1,24 +1,27 @@
 function(add_service_interface SERVICE_INTERFACE_NAME JSON_FILE)
-    add_library(${SERVICE_INTERFACE_NAME} OBJECT EXCLUDE_FROM_ALL
-    )
+    add_library(${SERVICE_INTERFACE_NAME} OBJECT EXCLUDE_FROM_ALL)
     target_add_service_interface(${SERVICE_INTERFACE_NAME} ${SERVICE_INTERFACE_NAME} ${JSON_FILE})
 endfunction()
 
 function(target_add_service_interface TARGET_NAME SERVICE_INTERFACE_NAME JSON_FILE)
     # generate the output directory, otherwise python will complain when multiple instances are run
     file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/generated/include)
-    add_custom_command(
-            OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_INTERFACE_NAME}Base.hpp ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_INTERFACE_NAME}Base.cpp
-            COMMAND ${Python3_EXECUTABLE} -m cogapp -d -I ${XBOT_CODEGEN_PATH}/xbot_codegen -D service_file=${JSON_FILE} -o ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_INTERFACE_NAME}Base.hpp ${XBOT_CODEGEN_PATH}/templates/ServiceInterfaceTemplate.hpp
-            COMMAND ${Python3_EXECUTABLE} -m cogapp -d -I ${XBOT_CODEGEN_PATH}/xbot_codegen -D service_file=${JSON_FILE} -o ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_INTERFACE_NAME}Base.cpp ${XBOT_CODEGEN_PATH}/templates/ServiceInterfaceTemplate.cpp
-            DEPENDS ${XBOT_CODEGEN_PATH}/templates/ServiceInterfaceTemplate.hpp ${XBOT_CODEGEN_PATH}/templates/ServiceInterfaceTemplate.cpp ${JSON_FILE}
-            COMMENT "Generating code for service interface ${SERVICE_INTERFACE_NAME}."
-    )
 
-    target_sources(${TARGET_NAME} PRIVATE ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_INTERFACE_NAME}Base.cpp
-            ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_INTERFACE_NAME}Base.hpp
+    set(COG_BASE_ARGS -m cogapp -d -I ${XBOT_CODEGEN_PATH}/xbot_codegen -Dservice_file=${JSON_FILE})
+    set(HEADER_TEMPLATE ${XBOT_CODEGEN_PATH}/templates/ServiceInterfaceTemplate.hpp)
+    set(SOURCE_TEMPLATE ${XBOT_CODEGEN_PATH}/templates/ServiceInterfaceTemplate.cpp)
+    set(HEADER_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/generated/include/${SERVICE_INTERFACE_NAME}Base.hpp)
+    set(SOURCE_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/generated/${SERVICE_INTERFACE_NAME}Base.cpp)
+
+    add_custom_command(
+        OUTPUT ${HEADER_OUTPUT} ${SOURCE_OUTPUT}
+        COMMAND ${Python3_EXECUTABLE} ${COG_BASE_ARGS} -o ${HEADER_OUTPUT} ${HEADER_TEMPLATE}
+        COMMAND ${Python3_EXECUTABLE} ${COG_BASE_ARGS} -o ${SOURCE_OUTPUT} ${SOURCE_TEMPLATE}
+        DEPENDS ${HEADER_TEMPLATE} ${SOURCE_TEMPLATE} ${XBOT_CODEGEN_PATH}/xbot_codegen/xbot_codegen.py ${JSON_FILE}
+        COMMENT "Generating code for service interface ${SERVICE_INTERFACE_NAME}."
     )
 
+    target_sources(${TARGET_NAME} PRIVATE ${SOURCE_OUTPUT} ${HEADER_OUTPUT})
     target_include_directories(${TARGET_NAME} PUBLIC ${CMAKE_CURRENT_BINARY_DIR}/generated/include)
     target_link_libraries(${TARGET_NAME} PUBLIC xbot-service-interface)
 endfunction()

codegen/templates/ServiceTemplate.hpp

@@ -16,7 +16,11 @@ cog.outl(f"#define {service['class_name'].upper()}_HPP")
 #define SERVICETEMPLATEBASE_HPP
 //[[[end]]]
 
+/*[[[cog
+cog.outl(f"#include <{vars().get('service_ext', 'xbot-service/Service.hpp')}>")
+]]]*/
 #include <xbot-service/Service.hpp>
+//[[[end]]]
 
 /*[[[cog
 xbot_codegen.generateEnums(service)
@@ -37,29 +41,29 @@ namespace ExampleBitmaskEnum {
 
 //[[[end]]]
 /*[[[cog
-cog.outl(f"class {service['class_name']} : public xbot::service::Service {{")
+service_class = 'ServiceExt' if 'service_ext' in vars() else 'Service'
+cog.outl(f"class {service['class_name']} : public xbot::service::{service_class} {{")
 ]]]*/
 class ServiceTemplateBase : public xbot::service::Service {
 //[[[end]]]
 public:
     /*[[[cog
     cog.outl("#ifdef XBOT_ENABLE_STATIC_STACK")
     cog.outl(f"explicit {service['class_name']}(uint16_t service_id, void* stack, size_t stack_size)")
+    cog.outl(f"    : {service_class}(service_id, stack, stack_size) {{")
     cog.outl("#else")
     cog.outl(f"explicit {service['class_name']}(uint16_t service_id)")
+    cog.outl(f"    : {service_class}(service_id, nullptr, 0) {{")
     cog.outl("#endif")
     ]]]*/
     #ifdef XBOT_ENABLE_STATIC_STACK
     explicit ServiceTemplateBase(uint16_t service_id, void* stack, size_t stack_size)
-    #else
-    explicit ServiceTemplateBase(uint16_t service_id)
-    #endif
-    //[[[end]]]
-    #ifdef XBOT_ENABLE_STATIC_STACK
         : Service(service_id, stack, stack_size) {
     #else
+    explicit ServiceTemplateBase(uint16_t service_id)
         : Service(service_id, nullptr, 0) {
     #endif
+    //[[[end]]]
     }
 
     /*[[[cog

examples/services/EchoService/EchoService.hpp

@@ -16,8 +16,7 @@ class EchoService : public EchoServiceBase {
 
  private:
   void tick();
-  ManagedSchedule tick_schedule_{scheduler_, IsRunning(), 1'000'000,
-                                 XBOT_FUNCTION_FOR_METHOD(EchoService, &EchoService::tick, this)};
+  ServiceSchedule tick_schedule_{*this, 1'000'000, XBOT_FUNCTION_FOR_METHOD(EchoService, &EchoService::tick, this)};
 
   uint32_t echo_count = 0;
 

ext/CMakeLists.txt

@@ -1,10 +1,13 @@
 if (XBOT_BUILD_LIB_SERVICE)
     add_subdirectory(ulog/src)
     add_subdirectory(cpputest)
+    add_subdirectory(lwjson)
 endif ()
 
 if (XBOT_BUILD_LIB_SERVICE_INTERFACE)
     add_subdirectory(json)
     add_subdirectory(spdlog)
     add_subdirectory(crow)
 endif ()
+
+add_subdirectory(heatshrink)

ext/heatshrink/CMakeLists.txt

@@ -0,0 +1,22 @@
+include(FetchContent)
+
+# Use "develop" branch
+FetchContent_Declare(heatshrink URL https://github.com/atomicobject/heatshrink/archive/e4084caa5c570c5e733de6b12ee1c9000c52d1b4.tar.gz)
+FetchContent_MakeAvailable(heatshrink)
+
+add_library(heatshrink STATIC
+    ${heatshrink_SOURCE_DIR}/src/heatshrink_decoder.c
+    ${heatshrink_SOURCE_DIR}/src/heatshrink_encoder.c
+)
+
+target_compile_definitions(heatshrink PUBLIC
+    HEATSHRINK_DYNAMIC_ALLOC=0
+    HEATSHRINK_STATIC_WINDOW_BITS=9
+    HEATSHRINK_STATIC_LOOKAHEAD_BITS=5
+)
+
+target_include_directories(heatshrink
+    PUBLIC
+        ${heatshrink_SOURCE_DIR}/include
+        ${heatshrink_SOURCE_DIR}/src
+)

ext/lwjson/CMakeLists.txt

@@ -0,0 +1,8 @@
+include(FetchContent)
+FetchContent_Declare(
+    lwjson
+    URL https://github.com/MaJerle/lwjson/archive/97dfff90b12772d1ec5f4bf4b80cdfe187693423.tar.gz
+    PATCH_COMMAND sed -i "s/^cmake_minimum_required.*$/cmake_minimum_required(VERSION 3.16)/" CMakeLists.txt lwjson/CMakeLists.txt
+
+)
+FetchContent_MakeAvailable(lwjson)

libxbot-service-interface/CMakeLists.txt

@@ -15,14 +15,15 @@ add_library(xbot-service-interface
         src/ServiceIOImpl.hpp
         src/XbotServiceInterface.cpp
         src/RemoteLoggingReceiverImpl.cpp
+        src/HeatshrinkEncode.cpp
 )
 
 target_include_directories(xbot-service-interface PUBLIC
         include
         ../include
 )
 target_link_libraries(xbot-service-interface PUBLIC
-        nlohmann_json::nlohmann_json spdlog::spdlog Crow::Crow
+        nlohmann_json::nlohmann_json spdlog::spdlog Crow::Crow heatshrink
 )
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
         DESTINATION include

libxbot-service-interface/include/xbot-service-interface/HeatshrinkEncode.hpp

@@ -0,0 +1,4 @@
+#include <cstdint>
+#include <vector>
+
+std::vector<uint8_t> HeatshrinkEncode(uint8_t *data, const size_t size);

libxbot-service-interface/src/HeatshrinkEncode.cpp

@@ -0,0 +1,31 @@
+#include <xbot-service-interface/HeatshrinkEncode.hpp>
+
+extern "C" {
+#include <heatshrink_encoder.h>
+}
+
+std::vector<uint8_t> HeatshrinkEncode(uint8_t *data, const size_t size) {
+  heatshrink_encoder encoder;
+  std::vector<uint8_t> out;
+  uint8_t buf[128];
+  size_t processed = 0, processed_now, polled_now;
+  HSE_poll_res poll_res;
+
+  heatshrink_encoder_reset(&encoder);
+  while (processed < size) {
+    heatshrink_encoder_sink(&encoder, &data[processed], size - processed, &processed_now);
+    processed += processed_now;
+
+    do {
+      poll_res = heatshrink_encoder_poll(&encoder, buf, sizeof(buf), &polled_now);
+      out.insert(out.end(), buf, buf + polled_now);
+    } while (poll_res == HSER_POLL_MORE);
+  }
+
+  while (heatshrink_encoder_finish(&encoder) == HSER_FINISH_MORE) {
+    heatshrink_encoder_poll(&encoder, buf, sizeof(buf), &polled_now);
+    out.insert(out.end(), buf, buf + polled_now);
+  }
+
+  return out;
+}

libxbot-service-interface/src/RemoteLoggingReceiverImpl.cpp

@@ -52,16 +52,14 @@ void RemoteLoggingReceiverImpl::Run() {
 
         // Validate reported length
         if (packet.size() == header->payload_size + sizeof(datatypes::XbotHeader)) {
-          if (header->payload_size > 1) {
-            std::string_view view{reinterpret_cast<const char *>(packet.data() + sizeof(datatypes::XbotHeader)),
-                                  header->payload_size};
-            switch (header->arg1) {
-              case 1: spdlog::trace(">>> {}", view); break;
-              case 2: spdlog::debug(">>> {}", view); break;
-              case 3: spdlog::info(">>> {}", view); break;
-              case 4: spdlog::warn(">>> {}", view); break;
-              default: spdlog::error(">>> {}", view); break;
-            }
+          std::string_view view{reinterpret_cast<const char *>(packet.data() + sizeof(datatypes::XbotHeader)),
+                                header->payload_size};
+          switch (header->arg1) {
+            case 1: spdlog::trace(">>> {}", view); break;
+            case 2: spdlog::debug(">>> {}", view); break;
+            case 3: spdlog::info(">>> {}", view); break;
+            case 4: spdlog::warn(">>> {}", view); break;
+            default: spdlog::error(">>> {}", view); break;
           }
         }
       }

libxbot-service/CMakeLists.txt

@@ -15,6 +15,7 @@ add_library(xbot-service STATIC
         src/Schedule.cpp
         src/Scheduler.cpp
         src/ServiceIo.cpp
+        src/DataSource.cpp
 )
 target_compile_options(xbot-service PRIVATE -Wall -Wextra -Werror -Wno-volatile)
 
@@ -31,7 +32,7 @@ target_sources(xbot-service PRIVATE ${PORT_SOURCES})
 
 
 target_include_directories(xbot-service PUBLIC include ../include)
-target_link_libraries(xbot-service PUBLIC ulog ${XBOT_CUSTOM_PORT_LIBS})
+target_link_libraries(xbot-service PUBLIC ulog ${XBOT_CUSTOM_PORT_LIBS} heatshrink)
 
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
         DESTINATION include

libxbot-service/include/xbot-service/DataSource.hpp

@@ -0,0 +1,76 @@
+#include <cstddef>
+#include <cstdint>
+
+extern "C" {
+#include <heatshrink_decoder.h>
+}
+
+namespace xbot::service {
+
+class DataSource {
+ public:
+  DataSource(const uint8_t* data, const size_t size) : data_(data), size_(size) {
+  }
+
+  virtual bool HasNext() = 0;
+  virtual uint8_t Next() = 0;
+  virtual void Rewind() = 0;
+  virtual size_t Position() {
+    return pos_;
+  };
+
+ protected:
+  const uint8_t* data_;
+  const size_t size_;
+  size_t pos_ = 0;
+};
+
+class RawDataSource : public DataSource {
+ public:
+  using DataSource::DataSource;
+
+  bool HasNext() override {
+    return pos_ < size_;
+  }
+
+  uint8_t Next() override {
+    return data_[pos_++];
+  }
+
+  void Rewind() override {
+    pos_ = 0;
+  }
+};
+
+class HeatshrinkDataSource : public DataSource {
+ public:
+  using DataSource::DataSource;
+
+  bool HasNext() override {
+    return buf_pos_ < buf_size_ || Decode();
+  }
+
+  uint8_t Next() override {
+    pos_++;
+    return buf_[buf_pos_++];
+  }
+
+  void Rewind() override {
+    heatshrink_decoder_reset(&decoder_);
+    pos_ = 0;
+    compressed_pos_ = 0;
+    buf_pos_ = 0;
+    buf_size_ = 0;
+  }
+
+ private:
+  heatshrink_decoder decoder_;
+  size_t compressed_pos_;
+  uint8_t buf_[32];
+  size_t buf_pos_;
+  size_t buf_size_;
+
+  bool Decode();
+};
+
+}  // namespace xbot::service

libxbot-service/include/xbot-service/Service.hpp

@@ -16,7 +16,11 @@
 #include "xbot/datatypes/XbotHeader.hpp"
 
 namespace xbot::service {
+
 class Service : public ServiceIo {
+  friend class ServiceExt;
+  friend class ServiceSchedule;
+
  public:
   explicit Service(uint16_t service_id, void *processing_thread_stack, size_t processing_thread_stack_size);
 
@@ -135,6 +139,11 @@ class Service : public ServiceIo {
   void heartbeat();
 
   void runProcessing();
+  virtual uint32_t OnLoop(uint32_t now_micros, uint32_t last_tick_micros) {
+    (void)now_micros;
+    (void)last_tick_micros;
+    return UINT32_MAX;
+  };
 
   void HandleClaimMessage(datatypes::XbotHeader *header, const void *payload, size_t payload_len);
   void HandleDataMessage(datatypes::XbotHeader *header, const void *payload, size_t payload_len);
@@ -163,6 +172,14 @@ class Service : public ServiceIo {
 
   virtual bool setRegister(uint16_t target_id, const void *payload, size_t length) = 0;
 };
+
+class ServiceSchedule : public ManagedSchedule {
+ public:
+  explicit ServiceSchedule(Service &service, uint32_t interval, Callback callback)
+      : ManagedSchedule(service.scheduler_, service.IsRunning(), interval, callback) {
+  }
+};
+
 }  // namespace xbot::service
 
 #endif  // SERVICE_HPP