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
parent a41fa032b8
commit 29dea8e817
8 changed files with 110 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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