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-06-14 14:38:07 +08:00
committed by liujing
parent d8c05d09d2
commit fcb10cff56
8 changed files with 110 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
app_ipc_monitoring/src/main/java/com/zhidao/adas/client/DataDistribution.java
View File

@@ -12,6 +12,7 @@ import com.zhidao.adas.client.bean.GlobalPathResp;
import com.zhidao.adas.client.bean.GnssInfo;
import com.zhidao.adas.client.bean.MogoReportMessage;
import com.zhidao.adas.client.bean.MyPointCloud;
import com.zhidao.adas.client.bean.OriginalPointCloudData;
import com.zhidao.adas.client.bean.PerceptionTrafficLight;
import com.zhidao.adas.client.bean.PlanningObjects;
import com.zhidao.adas.client.bean.PredictionObstacleTrajectory;
@@ -112,6 +113,7 @@ public class DataDistribution {
public final List<String> listStatusInfo = new ArrayList<>();
public final List<String> listErrorData = new ArrayList<>();
public final List<String> listPointCloud = new ArrayList<>();
public final List<String> listOriginalPointCloud = new ArrayList<>();
public final List<String> listPlanningObjects = new ArrayList<>();
public String cutDown(String str) {
@@ -148,6 +150,14 @@ public class DataDistribution {
if (listener != null && Constants.TITLE.RECEIVE_POINT_CLOUD.equals(listener.first)) {
listener.second.onRefresh();
}
} else if (data instanceof OriginalPointCloudData) {
listOriginalPointCloud.add(0, time + str);
if (listOriginalPointCloud.size() > LIST_SIZE) {
listOriginalPointCloud.remove(listOriginalPointCloud.size() - 1);
}
if (listener != null && Constants.TITLE.RECEIVE_POINT_CLOUD_ORIGINAL.equals(listener.first)) {
listener.second.onRefresh();
}
} else if (data instanceof PlanningObjects) {
listPlanningObjects.add(0, time + str);
if (listPlanningObjects.size() > LIST_SIZE) {

17
app_ipc_monitoring/src/main/java/com/zhidao/adas/client/bean/OriginalPointCloudData.java Normal file
View File

@@ -0,0 +1,17 @@
package com.zhidao.adas.client.bean;
import com.zhidao.support.adas.high.common.ByteUtil;
public class OriginalPointCloudData extends BaseInfo {
private final byte[] pointCloud;
public OriginalPointCloudData(byte[] pointCloud) {
super("接收", pointCloud.length);
this.pointCloud = pointCloud;
}
@Override
public String toString() {
return "\n" + ByteUtil.byteArrToHex(pointCloud);
}
}

2
app_ipc_monitoring/src/main/java/com/zhidao/adas/client/ui/InfoFragment.java
View File

@@ -129,6 +129,8 @@ public class InfoFragment extends BaseFragment {
adapter.setData(DataDistribution.getInstance().listPredictionObstacleTrajectory);
} else if (Constants.TITLE.RECEIVE_POINT_CLOUD.equals(title)) {
adapter.setData(DataDistribution.getInstance().listPointCloud);
}else if (Constants.TITLE.RECEIVE_POINT_CLOUD_ORIGINAL.equals(title)) {
adapter.setData(DataDistribution.getInstance().listOriginalPointCloud);
} else if (Constants.TITLE.RECEIVE_PLANNING_OBJECTS.equals(title)) {
adapter.setData(DataDistribution.getInstance().listPlanningObjects);
}else{

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

@@ -65,6 +65,7 @@ import com.zhidao.adas.client.bean.GnssInfo;
import com.zhidao.adas.client.bean.IPCConnectState;
import com.zhidao.adas.client.bean.MogoReportMessage;
import com.zhidao.adas.client.bean.MyPointCloud;
import com.zhidao.adas.client.bean.OriginalPointCloudData;
import com.zhidao.adas.client.bean.PerceptionTrafficLight;
import com.zhidao.adas.client.bean.PlanningObjects;
import com.zhidao.adas.client.bean.PredictionObstacleTrajectory;
@@ -580,6 +581,7 @@ public class MainActivity extends BaseActivity implements OnAdasListener, OnAdas
titleFragmentData.add(Constants.TITLE.RECEIVE_PERCEPTION_TRAFFIC_LIGHT);
titleFragmentData.add(Constants.TITLE.RECEIVE_PREDICTION_OBSTACLE_TRAJECTORY);
titleFragmentData.add(Constants.TITLE.RECEIVE_POINT_CLOUD);
titleFragmentData.add(Constants.TITLE.RECEIVE_POINT_CLOUD_ORIGINAL);
titleFragmentData.add(Constants.TITLE.RECEIVE_PLANNING_OBJECTS);
titleFragmentData.add(Constants.TITLE.RECEIVE_CAR_CONFIG_RESP);
titleFragmentData.add(Constants.TITLE.RECEIVE_RECORD_RESULT);
@@ -810,6 +812,12 @@ public class MainActivity extends BaseActivity implements OnAdasListener, OnAdas
// LogSave.getInstance().saveLog("转换数据=" + data);
}
@Override
public void onPointCloud(byte[] pointCloud) {
OriginalPointCloudData base = new OriginalPointCloudData(pointCloud);
DataDistribution.getInstance().addData(base);
}
@Override
public void onPlanningObjects(MessagePad.Header header, MessagePad.PlanningObjects planningObjects) {
PlanningObjects base = new PlanningObjects(header, planningObjects, sdf);

1
app_ipc_monitoring/src/main/java/com/zhidao/adas/client/utils/Constants.java
View File

@@ -158,6 +158,7 @@ public class Constants {
String RECEIVE_PERCEPTION_TRAFFIC_LIGHT = MessageType.TYPE_RECEIVE_PERCEPTION_TRAFFIC_LIGHT.desc;
String RECEIVE_PREDICTION_OBSTACLE_TRAJECTORY = MessageType.TYPE_RECEIVE_PREDICTION_OBSTACLE_TRAJECTORY.desc;
String RECEIVE_POINT_CLOUD = MessageType.TYPE_RECEIVE_POINT_CLOUD.desc;
String RECEIVE_POINT_CLOUD_ORIGINAL = "点云原始透传";
String RECEIVE_PLANNING_OBJECTS = MessageType.TYPE_RECEIVE_PLANNING_OBJECTS.desc;
// String RECEIVE_BASIC_INFO_REQ = "自动驾驶设备基础信息请求";

114
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java
View File

@@ -61,11 +61,15 @@ import okio.ByteString;
*/
public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnectListener, IPCFixationIPHelper.IIPCFixationIPListener, DispatchHandler.OnDispatchHandlerListener {
private static final String TAG = AdasChannel.class.getSimpleName();
private static final String THREAD_NAME_DISPATCH_EVENT = "IPCEventDispatchHandler";//除点云单独拆分线程以外都是用此名称
private static final String THREAD_NAME_DISPATCH_POINT_CLOUD = "IPCPointCloudDispatchHandler";
private static final String THREAD_NAME_DISPATCH_PARSE_POINT_CLOUD = "IPCParsePointCloudDispatchHandler";//解析点云线程 地图支持后将会删除
private FpgaSocket mSocket;
private RawUnpack rawUnpack;//业务数据拆包
private RawPack rawPack;//数据打包
private DispatchHandler dispatchHandler;//分发
private DispatchHandler dispatchHandlerPointCloud;//点云分发
private DispatchHandler dispatchHandlerPointCloud;//原始的点云数据分发
private DispatchHandler dispatchHandlerParsePointCloud;//解析过的点云数据分发 地图支持后将会删除
private Timer checkCompatibilityTimer;//检查版本兼容性定时器 连接成功后5秒内等待工控机发送配置信息
/**
* 与工控机链接状态
@@ -78,10 +82,6 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
*/
public static final boolean isUseQueue = false;
/**
* 本通道是否启用分发线程 收发和分发拆分成两个线程
*/
public static final boolean ADAS_CHANNEL_IS_USE_DISPATCH_HANDLER = true;
/**
* 消息工厂
@@ -156,11 +156,10 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
rawPack = new RawPack();
//消息工厂
myMessageFactory = new MyMessageFactory();
//司机端以及启用线程分发时
if (!adasOptions.isClient() && ADAS_CHANNEL_IS_USE_DISPATCH_HANDLER) {
dispatchHandler = new DispatchHandler(this);
dispatchHandlerPointCloud = new DispatchHandler(this);
}
//启用线程分发
dispatchHandler = new DispatchHandler(THREAD_NAME_DISPATCH_EVENT, this);
dispatchHandlerPointCloud = new DispatchHandler(THREAD_NAME_DISPATCH_POINT_CLOUD, this);
dispatchHandlerParsePointCloud = new DispatchHandler(THREAD_NAME_DISPATCH_PARSE_POINT_CLOUD, this);
}
/**
@@ -267,16 +266,15 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
ReceiveTimeoutManager.getInstance().refreshLast(header.getTimestamp());
// CupidLogUtils.w("--->websocket byte read header = " + messageType.toString());
//判断是否是司机屏幕，是否切换分发线程
if (!adasOptions.isClient() && ADAS_CHANNEL_IS_USE_DISPATCH_HANDLER) {
if (header.getMsgType() == MessageType.TYPE_RECEIVE_POINT_CLOUD.typeCode) {
if (dispatchHandlerPointCloud != null)
dispatchHandlerPointCloud.sendRawMessage(raw);
} else {
if (dispatchHandler != null)
dispatchHandler.sendRawMessage(raw);
if (header.getMsgType() == MessageType.TYPE_RECEIVE_POINT_CLOUD.typeCode) {
if (dispatchHandlerPointCloud != null) {
dispatchHandlerPointCloud.sendRawMessage(raw);
}
if (dispatchHandlerParsePointCloud != null)
dispatchHandlerParsePointCloud.sendRawMessage(raw);
} else {
dispatchRaw(raw);
if (dispatchHandler != null)
dispatchHandler.sendRawMessage(raw);
}
} else {
callError(raw.getProtocolStatus(), byteString.toByteArray());
@@ -288,37 +286,37 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
}
}
/**
* 分发和解析
*
* @param raw
*/
private void dispatchRaw(RawData raw) {
try {
if (rawUnpack != null) {
if (raw.getProtocolStatus() == ProtocolStatus.SUCCEED) {
MessagePad.Header header = raw.getHeader();
MessagePad.MessageType messageType = header.getMsgType();
IMsg iMsg = myMessageFactory.createMessage(messageType);
if (iMsg == null) {
callError(ProtocolStatus.MESSAGE_TYPE_UNKNOWN, raw.originalData.toByteArray());
return;
}
iMsg.handlerMsg(raw, mAdasListener);
} else {
callError(raw.getProtocolStatus(), raw.originalData.toByteArray());
}
}
} catch (Exception e) {
callError(ProtocolStatus.BUSINESS_DATA_PARSE_FAILED, raw.originalData.toByteArray());
CupidLogUtils.e(TAG, "原始数据:" + ByteUtil.byteArrToHex(raw.originalData.toByteArray()), e);
}
}
@Override
public void onDispatchRaw(RawData raw) {
dispatchRaw(raw);
public void onDispatchRaw(String threadName, RawData raw) {
//分发点云原始数据
if (THREAD_NAME_DISPATCH_POINT_CLOUD.equals(threadName)) {
mAdasListener.onPointCloud(raw.originalData.toByteArray());
} else {
try {
if (rawUnpack != null) {
if (raw.getProtocolStatus() == ProtocolStatus.SUCCEED) {
MessagePad.Header header = raw.getHeader();
MessagePad.MessageType messageType = header.getMsgType();
IMsg iMsg = myMessageFactory.createMessage(messageType);
if (iMsg == null) {
callError(ProtocolStatus.MESSAGE_TYPE_UNKNOWN, raw.originalData.toByteArray());
return;
}
iMsg.handlerMsg(raw, mAdasListener);
} else {
callError(raw.getProtocolStatus(), raw.originalData.toByteArray());
}
}
} catch (Exception e) {
callError(ProtocolStatus.BUSINESS_DATA_PARSE_FAILED, raw.originalData.toByteArray());
CupidLogUtils.e(TAG, "原始数据:" + ByteUtil.byteArrToHex(raw.originalData.toByteArray()), e);
}
}
}
private void callError(ProtocolStatus status, byte[] bytes) {
@@ -475,13 +473,14 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
adasConnectStatusListener.onConnectionIPCStatus(ipcConnectionStatus.get(), reason);
}
if (status == Constants.IPC_CONNECTION_STATUS.CONNECTED) {
if (ADAS_CHANNEL_IS_USE_DISPATCH_HANDLER) {
if (dispatchHandler != null) {
dispatchHandler.start();
}
if (dispatchHandlerPointCloud != null) {
dispatchHandlerPointCloud.start();
}
if (dispatchHandler != null) {
dispatchHandler.start();
}
if (dispatchHandlerPointCloud != null) {
dispatchHandlerPointCloud.start();
}
if (dispatchHandlerParsePointCloud != null) {
dispatchHandlerParsePointCloud.start();
}
startCheckCompatibility();
} else {
@@ -489,13 +488,14 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
}
if (status == Constants.IPC_CONNECTION_STATUS.DISCONNECTED) {
AdasManager.getInstance().setCarConfig(null);
if (ADAS_CHANNEL_IS_USE_DISPATCH_HANDLER) {
if (dispatchHandler != null) {
dispatchHandler.stop();
}
if (dispatchHandlerPointCloud != null) {
dispatchHandlerPointCloud.stop();
}
if (dispatchHandler != null) {
dispatchHandler.stop();
}
if (dispatchHandlerPointCloud != null) {
dispatchHandlerPointCloud.stop();
}
if (dispatchHandlerParsePointCloud != null) {
dispatchHandlerParsePointCloud.stop();
}
}
CupidLogUtils.i(TAG, "工控机连接状态 status=" + status + " reason=" + reason);

7
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java
View File

@@ -97,6 +97,13 @@ public interface OnAdasListener {
*/
void onPointCloud(MessagePad.Header header, MogoPointCloudOuterClass.MogoPointCloud pointCloud);
/**
* 透传的点云数据
*
* @param pointCloud 原始数据 包括Header+pointCloud
*/
void onPointCloud(byte[] pointCloud);
/**
* planning障碍物
*

18
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/thread/DispatchHandler.java
View File

@@ -11,31 +11,29 @@ import com.zhidao.support.adas.high.protocol.RawData;
import java.lang.ref.WeakReference;
import okio.ByteString;
/**
* 将WebSocket线程分发到工作线程
*/
public class DispatchHandler {
public static final int WHAT_DISPATCH_RAW = 0x01;//分发消息 what
private final String name;
private final OnDispatchHandlerListener listener;
private HandlerThread mThread;
private BaseHandler mBaseHandler;
private OnDispatchHandlerListener listener;
public interface OnDispatchHandlerListener {
void onDispatchRaw(RawData raw);
void onDispatchRaw(String threadName, RawData raw);
}
public DispatchHandler(@NonNull OnDispatchHandlerListener listener) {
this.listener = listener;
public DispatchHandler(String name, @NonNull OnDispatchHandlerListener listener) {
this.listener = listener;
this.name = name;
}
public void start() {
if (mThread == null) {
mThread = new HandlerThread("IPCEventDispatchHandler");
mThread = new HandlerThread(name);
mThread.start();
initHandler(mThread.getLooper());
}
@@ -72,7 +70,7 @@ public class DispatchHandler {
protected void handleMessage(Message msg) {
if (msg.what == WHAT_DISPATCH_RAW) {
if (listener != null) {
listener.onDispatchRaw((RawData) msg.obj);
listener.onDispatchRaw(name, (RawData) msg.obj);
}
}
}