add pack and unpack function to packet, add ctest for packet

src/mil_common/electrical_protocol_cpp/CMakeLists.txt

@@ -13,7 +13,7 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-add_library(electrical_protocol_cpp src/driver.cpp src/packet.cpp)
+add_library(electrical_protocol_cpp src/driver.cpp)
 
 target_include_directories(electrical_protocol_cpp PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -38,16 +38,26 @@ install(TARGETS electrical_protocol_cpp
 # install(DIRECTORY include/
 #     DESTINATION include)
 
-# if(BUILD_TESTING)
-#   find_package(ament_lint_auto REQUIRED)
-#   # the following line skips the linter which checks for copyrights
-#   # comment the line when a copyright and license is added to all source files
-#   set(ament_cmake_copyright_FOUND TRUE)
-#   # the following line skips cpplint (only works in a git repo)
-#   # comment the line when this package is in a git repo and when
-#   # a copyright and license is added to all source files
-#   set(ament_cmake_cpplint_FOUND TRUE)
-#   ament_lint_auto_find_test_dependencies()
-# endif()
+if(BUILD_TESTING)
+  # find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  # set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  # set(ament_cmake_cpplint_FOUND TRUE)
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(${PROJECT_NAME}_packet_test test/packet.cpp)
+  target_include_directories(${PROJECT_NAME}_packet_test PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  target_link_libraries(${PROJECT_NAME}_packet_test ${PROJECT_NAME})
+
+  # ament_lint_auto_find_test_dependencies()
+endif()
 
 ament_package()

src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/packet.h

@@ -6,6 +6,7 @@
 #include <deque>
 
 #include <cppystruct.h>
+// #include <cppystruct/format.h>
 
 namespace electrical_protocol
 {
@@ -14,12 +15,69 @@ namespace electrical_protocol
     {
         public:
         friend class SerialDevice;
-        Packet(uint8_t classId, uint8_t subClassId);
-        ~Packet();
+        Packet(uint8_t classId, uint8_t subClassId)
+        {
+            id_ = (static_cast<uint16_t>(classId) << 8) + subClassId;
+        }
+
+        ~Packet()
+        {
+
+        }
+
+        template <typename Fmt, typename... Args>
+        void pack(Fmt, Args&&... args)
+        {
+            constexpr size_t itemCount = pystruct::countItems(Fmt{});
+            pack_<Fmt>(std::make_index_sequence<itemCount>(), std::forward<Args>(args)...);
+        }
+
+        template <typename Fmt>
+        constexpr auto unpack(Fmt)
+        {
+            return unpack_<Fmt>(std::make_index_sequence<countItems(Fmt{})>());
+        }
+
 
         private:
         std::vector<uint8_t> data_;
         uint16_t id_;
+
+        template <typename Fmt, size_t... Items, typename... Args>
+        inline void pack_(std::index_sequence<Items...>, Args&&... args)
+        {
+            static_assert(sizeof...(args) == sizeof...(Items), "pack expected items for packing != sizeof...(args) passed");
+            constexpr auto formatMode = pystruct::getFormatMode(Fmt{});
+            constexpr auto byteSize = pystruct::calcsize(Fmt{});
+            data_.resize(byteSize);
+
+            constexpr pystruct::FormatType formats[] = { pystruct::getTypeOfItem<Items>(Fmt{})... };
+            using Types = std::tuple<typename pystruct::RepresentedType<decltype(formatMode), formats[Items].formatChar> ...>;
+
+            // Convert args to a tuple of the represented types
+            Types t = std::make_tuple(pystruct::internal::convert<std::tuple_element_t<Items, Types>>(std::forward<Args>(args))...);
+
+            constexpr size_t offsets[] = { pystruct::getBinaryOffset<Items>(Fmt{})... };
+            int _[] = { 0, pystruct::internal::packElement(reinterpret_cast<char*>(data_.data()) + offsets[Items],  formatMode.isBigEndian(), formats[Items], std::get<Items>(t))... };
+            (void)_; // _ is a dummy for pack expansion
+        }
+
+        template <typename Fmt, size_t... Items>
+        inline auto unpack_(std::index_sequence<Items...>)
+        {
+            constexpr auto formatMode = pystruct::getFormatMode(Fmt{});
+
+            constexpr pystruct::FormatType formats[] = { pystruct::getTypeOfItem<Items>(Fmt{})... };
+
+            using Types = std::tuple<typename pystruct::RepresentedType<decltype(formatMode), formats[Items].formatChar> ...>;
+
+            constexpr size_t offsets[] = { pystruct::getBinaryOffset<Items>(Fmt{})... };
+            auto unpacked = std::make_tuple(pystruct::unpackElement<Items, std::tuple_element_t<Items, Types>>(
+            reinterpret_cast<char*>(data_.data()) + offsets[Items], formats[Items].size, formatMode.isBigEndian())...);
+
+            return unpacked;
+        }
+
     };
 
 }

src/mil_common/electrical_protocol_cpp/test/packet.cpp

@@ -0,0 +1,35 @@
+#include <electrical_protocol_cpp/packet.h>
+
+#include <gtest/gtest.h>
+
+TEST(Packet, Test)
+{
+    electrical_protocol::Packet packet(1,1);
+
+    unsigned ta = 42;
+    float tb = 3.14;
+    double tc = 2.7182818;
+    int td = 7;
+    char te = 'A';
+    std::string tf = "Hello";
+    bool tg = true;
+    uint64_t th = 987654321;
+
+    packet.pack(PY_STRING("Ifdhc5s?Q"), ta, tb, tc, td, te, tf, tg, th);
+    auto [ra, rb, rc, rd, re, rf, rg, rh] = packet.unpack(PY_STRING("Ifdhc5s?Q"));
+
+    EXPECT_EQ(ta, ra);
+    EXPECT_EQ(tb, rb);
+    EXPECT_EQ(tc, rc);
+    EXPECT_EQ(td, rd);
+    EXPECT_EQ(te, re);
+    EXPECT_EQ(tf, rf);
+    EXPECT_EQ(tg, rg);
+    EXPECT_EQ(th, rh);
+
+
+}
+
+
+
+