yangyakun921/MoGoEagleEye
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
xinfengkun
2022-05-12 19:07:26 +08:00
parent da525cf7e8
commit 8aae203a43
2 changed files with 141 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
app_ipc_monitoring/src/main/java/com/zhidao/adas/client/ui/MainActivity.java
View File

@@ -266,6 +266,40 @@ public class MainActivity extends BaseActivity implements OnAdasListener, OnAdas
disconnect.setOnClickListener(new View.OnClickListener() {
@Override
public void onClick(View v) {
// MessagePad.Header header = MessagePad.Header.newBuilder()
// .setTimestamp(1.6523361438761907E9).build();
// PointCloud.LidarPointCloud pointCloud = PointCloud.LidarPointCloud.newBuilder()
// .setSelfLongitude(112.57563687979261)
// .setSelfLatitude(26.828128487326854)
// .setSelfAltitude(52.89)
// .setSelfRoll(-0.00715584993317675)
// .setSelfPitch(0.008726646259971651)
// .setSelfYaw(0.4850269854068756)
//
// .addAddData(1F)
// .addAddData(2F)
// .addAddData(3F)
// .addAddData(4F)
// .addAddData(5F)
// .addAddData(6F)
// .addAddData(7F)
// .addAddData(8F)
//
// .addDelData(9F)
// .addDelData(10F)
// .addDelData(11F)
// .addDelData(12F)
// .addDelData(13F)
// .addDelData(14F)
// .addDelData(15F)
// .addDelData(16F)
// .addDelData(17F)
// .addDelData(18F)
// .addDelData(19F)
// .addDelData(20F)
// .build();
// String data = PointCloudDecoder.decode(header, pointCloud);
// Log.i("ddd", "转换数据=" + data);
connect(false);
}
});
@@ -814,6 +848,9 @@ public class MainActivity extends BaseActivity implements OnAdasListener, OnAdas
public void onPointCloud(MessagePad.Header header, PointCloud.LidarPointCloud pointCloud) {
MyPointCloud base = new MyPointCloud(header, pointCloud);
DataDistribution.getInstance().addData(base);
// String data = PointCloudDecoder.decode(header, pointCloud);
// Log.i("dddd", "data==" + data.length());
// LogSave.getInstance().saveLog("转换数据=" + data);
}
@Override

104
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/PointCloudDecoder.java Normal file
View File

@@ -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();
}
}