Merge pull request #12 from toppers/varray

可変配列長対応しました！

Author: tmori

README.md

@@ -81,4 +81,25 @@ pdu/types
 ```
 
 
-
+## 設計情報
+
+### offset
+
+* 0列目:配列有無
+  * single：配列ではない
+  * array：固定長配列
+  * varray：可変長配列
+* 1列目：データ型の種類
+  * primitive：プリミティブデータ型
+  * struct：構造体
+* 2列目：メンバ名
+* 3列目：データ型名
+* 4列目：メンバ・オフセット（単位：バイト）
+* 5列目：メンバ・サイズ（単位：バイト）
+  * 固定長配列の場合：データ型のサイズ✖️配列長
+  * 可変長配列の場合：データ型のサイズ（実行時まで配列長がわからないので配列長はかけられない）
+* 6列目：配列長
+  * 固定長配列の場合：配列長
+  * 可変長配列の場合：8固定（参照データ）
+    * 0バイト目：配列長
+    * 4バイト目：ヒープ領域のオフセット（単位：バイト）

config/ros_msgs.txt

@@ -29,4 +29,5 @@ builtin_interfaces/Time
 std_msgs/String
 std_msgs/Bool
 sensor_msgs/PointCloud2
-hako_msgs/GameControllerOperation
+hako_msgs/GameControllerOperation
+hako_msgs/SimpleStructVarray

config/varray_size.json

@@ -5,10 +5,6 @@
         }
     },
     "sensor_msgs": {
-        "LaserScan": {
-            "ranges": 360,
-            "intensities": 360
-        },
         "PointCloud2": {
             "fields": 4,
             "data": 176656

pdu/json/hako_msgs/SimpleStructVarray.json

@@ -0,0 +1,24 @@
+{
+  "fields": [
+    {
+      "name": "aaa",
+      "type": "int32"
+    },
+    {
+      "name": "fixed_str",
+      "type": "string[2]"
+    },
+    {
+      "name": "varray_str",
+      "type": "string[]"
+    },
+    {
+      "name": "fixed_array",
+      "type": "SimpleVarray[5]"
+    },
+    {
+      "name": "data",
+      "type": "SimpleVarray[]"
+    }
+  ]
+}

pdu/json/hako_msgs/SimpleVarray.json

@@ -0,0 +1,16 @@
+{
+  "fields": [
+    {
+      "name": "data",
+      "type": "int8[]"
+    },
+    {
+      "name": "fixed_array",
+      "type": "int8[10]"
+    },
+    {
+      "name": "p_mem1",
+      "type": "int32"
+    }
+  ]
+}

pdu/offset/hako_msgs/SimpleStructVarray.offset

@@ -0,0 +1,5 @@
+single:primitive:aaa:int32:0:4
+array:primitive:fixed_str:string:4:256:2
+varray:primitive:varray_str:string:260:128:8
+array:struct:fixed_array:SimpleVarray:268:120:5
+varray:struct:data:SimpleVarray:388:24:8

pdu/offset/hako_msgs/SimpleVarray.offset

@@ -0,0 +1,3 @@
+varray:primitive:data:int8:0:1:8
+array:primitive:fixed_array:int8:8:10:10
+single:primitive:p_mem1:int32:20:4

pdu/offset/sensor_msgs/LaserScan.offset

@@ -6,5 +6,5 @@ single:primitive:time_increment:float32:148:4
 single:primitive:scan_time:float32:152:4
 single:primitive:range_min:float32:156:4
 single:primitive:range_max:float32:160:4
-array:primitive:ranges:float32:164:1440:360
-array:primitive:intensities:float32:1604:1440:360
+varray:primitive:ranges:float32:164:4:8
+varray:primitive:intensities:float32:172:4:8

pdu/types/builtin_interfaces/pdu_ctype_conv_Time.hpp

@@ -4,6 +4,7 @@
 #include "pdu_primitive_ctypes.h"
 #include "ros_primitive_types.hpp"
 #include "pdu_primitive_ctypes_conv.hpp"
+#include "pdu_dynamic_memory.hpp"
 /*
  * Dependent pdu data
  */
@@ -22,80 +23,81 @@
  * PDU ==> ROS2
  *
  ***************************/
-static inline int hako_convert_pdu2ros_Time(Hako_Time &src,  builtin_interfaces::msg::Time &dst)
+
+static inline int _pdu2ros_Time(const char* heap_ptr, Hako_Time &src, builtin_interfaces::msg::Time &dst)
 {
-    //primitive convert
+    // primitive convert
     hako_convert_pdu2ros(src.sec, dst.sec);
-    //primitive convert
+    // primitive convert
     hako_convert_pdu2ros(src.nanosec, dst.nanosec);
+    (void)heap_ptr;
     return 0;
 }
 
-template<int _src_len, int _dst_len>
-int hako_convert_pdu2ros_array_Time(Hako_Time src[], std::array<builtin_interfaces::msg::Time, _dst_len> &dst)
+static inline int hako_convert_pdu2ros_Time(Hako_Time &src, builtin_interfaces::msg::Time &dst)
 {
-    int ret = 0;
-    int len = _dst_len;
-    if (_dst_len > _src_len) {
-        len = _src_len;
-        ret = -1;
+    void* base_ptr = (void*)&src;
+    void* heap_ptr = hako_get_heap_ptr_pdu(base_ptr);
+    // Validate magic number and version
+    if (heap_ptr == nullptr) {
+        return -1; // Invalid PDU metadata
     }
-    for (int i = 0; i < len; i++) {
-        (void)hako_convert_pdu2ros_Time(src[i], dst[i]);
+    else {
+        return _pdu2ros_Time((char*)heap_ptr, src, dst);
     }
-    return ret;
-}
-template<int _src_len, int _dst_len>
-int hako_convert_pdu2ros_array_Time(Hako_Time src[], std::vector<builtin_interfaces::msg::Time> &dst)
-{
-    dst.resize(_src_len);
-    for (int i = 0; i < _src_len; i++) {
-        (void)hako_convert_pdu2ros_Time(src[i], dst[i]);
-    }
-    return 0;
 }
 
 /***************************
  *
  * ROS2 ==> PDU
  *
  ***************************/
-static inline int hako_convert_ros2pdu_Time(builtin_interfaces::msg::Time &src, Hako_Time &dst)
+
+static inline bool _ros2pdu_Time(builtin_interfaces::msg::Time &src, Hako_Time &dst, PduDynamicMemory &dynamic_memory)
 {
-    //primitive convert
-    hako_convert_ros2pdu(src.sec, dst.sec);
-    //primitive convert
-    hako_convert_ros2pdu(src.nanosec, dst.nanosec);
-    return 0;
+    try {
+        // primitive convert
+        hako_convert_ros2pdu(src.sec, dst.sec);
+        // primitive convert
+        hako_convert_ros2pdu(src.nanosec, dst.nanosec);
+    } catch (const std::runtime_error& e) {
+        std::cerr << "convertor error: " << e.what() << std::endl;
+        return false;
+    }
+    (void)dynamic_memory;
+    return true;
 }
 
-template<int _src_len, int _dst_len>
-int hako_convert_ros2pdu_array_Time(std::array<builtin_interfaces::msg::Time, _src_len> &src, Hako_Time dst[])
+static inline int hako_convert_ros2pdu_Time(builtin_interfaces::msg::Time &src, Hako_Time** dst)
 {
-    int ret = 0;
-    int len = _dst_len;
-    if (_dst_len > _src_len) {
-        len = _src_len;
-        ret = -1;
+    PduDynamicMemory dynamic_memory;
+    Hako_Time out;
+    if (!_ros2pdu_Time(src, out, dynamic_memory)) {
+        return -1;
     }
-    for (int i = 0; i < len; i++) {
-        (void)hako_convert_ros2pdu_Time(src[i], dst[i]);
+    int heap_size = dynamic_memory.get_total_size();
+    void* base_ptr = hako_create_empty_pdu(sizeof(Hako_Time), heap_size);
+    if (base_ptr == nullptr) {
+        return -1;
     }
-    return ret;
+    // Copy out on base data
+    memcpy(base_ptr, (void*)&out, sizeof(Hako_Time));
+
+    // Copy dynamic part and set offsets
+    void* heap_ptr = hako_get_heap_ptr_pdu(base_ptr);
+    dynamic_memory.copy_to_pdu((char*)heap_ptr);
+
+    *dst = (Hako_Time*)base_ptr;
+    return hako_get_pdu_meta_data(base_ptr)->total_size;
 }
-template<int _src_len, int _dst_len>
-int hako_convert_ros2pdu_array_Time(std::vector<builtin_interfaces::msg::Time> &src, Hako_Time dst[])
+
+static inline Hako_Time* hako_create_empty_pdu_Time(int heap_size)
 {
-    int ret = 0;
-    int len = _dst_len;
-    if (_dst_len > _src_len) {
-        len = _src_len;
-        ret = -1;
-    }
-    for (int i = 0; i < len; i++) {
-        (void)hako_convert_ros2pdu_Time(src[i], dst[i]);
+    // Allocate PDU memory
+    char* base_ptr = (char*)hako_create_empty_pdu(sizeof(Hako_Time), heap_size);
+    if (base_ptr == nullptr) {
+        return nullptr;
     }
-    return ret;
+    return (Hako_Time*)base_ptr;
 }
-
 #endif /* _PDU_CTYPE_CONV_HAKO_builtin_interfaces_Time_HPP_ */

pdu/types/can_msgs/pdu_ctype_HakoCan.h

@@ -6,8 +6,8 @@
 #include "can_msgs/pdu_ctype_HakoCanHead.h"
 
 typedef struct {
-        Hako_HakoCanHead    head;
-        Hako_HakoCanBody    body;
+        Hako_HakoCanHead head;
+        Hako_HakoCanBody body;
 } Hako_HakoCan;
 
 #endif /* _pdu_ctype_can_msgs_HakoCan_H_ */