[change]增加点云数据解码器

2022-05-12 19:07:26 +08:00
parent da525cf7e8
commit 8aae203a43
2 changed files with 141 additions and 0 deletions
--- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/PointCloudDecoder.java
+++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/PointCloudDecoder.java
@@ -0,0 +1,104 @@
+package com.zhidao.support.adas.high.common;
+
+import mogo.telematics.pad.MessagePad;
+import rule_segement.PointCloud;
+
+/**
+ * 点云数据解码器
+ * 时间戳，lon，lat，alt，yaw，roll，patch，新增数据大小，id，x，y，z，强度，……，删除数据大小，id，x，y，z，强度，……
+ * 如果add_data 没有数据 新增数据大小用-1表示
+ * 如果del_data 没有数据 删除数据大小用-2表示
+ * 如果又不存在的数会使用0.0补位
+ * 目前工控机所发送数据没用ID 所以默认用0代替
+ */
+public class PointCloudDecoder {
+    /**
+     * add 和del 列表中 多少一个数据为一组
+     * 目前缺少id所以是4个
+     */
+    private static final int NUMBER_OF_DATA = 4;
+
+    public static String decode(MessagePad.Header header, PointCloud.LidarPointCloud pointCloud) {
+        StringBuilder builder = new StringBuilder();
+        builder.append(((long) header.getTimestamp() * 1000.0D)).append(",");
+        double d;
+        if (pointCloud.hasSelfLongitude()) {
+            d = pointCloud.getSelfLongitude();
+        } else {
+            d = 0;
+        }
+        builder.append(d).append(",");
+        if (pointCloud.hasSelfLatitude()) {
+            d = pointCloud.getSelfLatitude();
+        } else {
+            d = 0;
+        }
+        builder.append(d).append(",");
+        if (pointCloud.hasSelfAltitude()) {
+            d = pointCloud.getSelfAltitude();
+        } else {
+            d = 0;
+        }
+        builder.append(d).append(",");
+        if (pointCloud.hasSelfYaw()) {
+            d = pointCloud.getSelfYaw();
+        } else {
+            d = 0;
+        }
+        builder.append(d).append(",");
+        if (pointCloud.hasSelfRoll()) {
+            d = pointCloud.getSelfRoll();
+        } else {
+            d = 0;
+        }
+        builder.append(d).append(",");
+        if (pointCloud.hasSelfPitch()) {
+            d = pointCloud.getSelfPitch();
+        } else {
+            d = 0;
+        }
+        builder.append(d).append(",");
+        int addSize = pointCloud.getAddDataCount() / NUMBER_OF_DATA;
+        int delSize = pointCloud.getDelDataCount() / NUMBER_OF_DATA;
+        if (addSize == 0 && delSize == 0) {
+            builder.append(-1).append(",").append(-2);
+        } else {
+            StringBuilder add = new StringBuilder();
+            StringBuilder del = new StringBuilder();
+            int i = 0;
+            while (i < delSize || i < addSize) {
+                int temp = i * NUMBER_OF_DATA;
+                if (i < addSize) {
+                    //目前没有id 默认插入0
+                    add.append(0).append(",");
+                    add.append(pointCloud.getAddData(temp++)).append(",");
+                    add.append(pointCloud.getAddData(temp++)).append(",");
+                    add.append(pointCloud.getAddData(temp++)).append(",");
+                    add.append(pointCloud.getAddData(temp)).append(",");
+                }
+                if (i < delSize) {
+                    temp = i * NUMBER_OF_DATA;
+                    //目前没有id 默认插入0
+                    del.append(0).append(",");
+                    del.append(pointCloud.getDelData(temp++)).append(",");
+                    del.append(pointCloud.getDelData(temp++)).append(",");
+                    del.append(pointCloud.getDelData(temp++)).append(",");
+                    del.append(pointCloud.getDelData(temp)).append(",");
+                }
+                i++;
+            }
+            if (addSize == 0) {
+                builder.append(-1).append(",");
+            } else {
+                builder.append(addSize).append(",").append(add);
+            }
+            if (delSize == 0) {
+                builder.append(-2);
+            } else {
+                del.deleteCharAt(del.length() - 1);
+                builder.append(delSize).append(",").append(del);
+            }
+        }
+        return builder.toString();
+    }
+}