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

[opt3.0_back_camera] 福田清扫车后摄像头数据接收

Browse Source
This commit is contained in:
xinfengkun
2023-03-06 16:05:31 +08:00
parent c95013f792
commit e7ea7ae9f4
28 changed files with 1117 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -21,7 +21,6 @@ import androidx.annotation.NonNull;
import androidx.annotation.Nullable;
import com.google.protobuf.InvalidProtocolBufferException;
import com.zhjt.mogo.adas.data.bean.AutopilotStatistics;
import com.zhidao.support.adas.high.bean.VersionCompatibility;
import com.zhidao.support.adas.high.common.AutopilotReview;
import com.zhidao.support.adas.high.common.ByteUtil;
@@ -45,6 +44,7 @@ import com.zhidao.support.adas.high.socket.FpgaSocket;
import com.zhidao.support.adas.high.subscribe.SubscribeInterface;
import com.zhidao.support.adas.high.subscribe.SubscribeInterfaceOptions;
import com.zhidao.support.adas.high.thread.DispatchHandler;
import com.zhjt.mogo.adas.data.bean.AutopilotStatistics;
import com.zhjt.service.chain.ChainLog;
import com.zhjt.service.chain.TracingConstants;
@@ -251,7 +251,8 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
// dispatchHandlers.put(MessagePad.MessageType.MsgTypeRecordDataConfigResp, new DispatchHandler(MessagePad.MessageType.MsgTypeRecordDataConfigResp, this));
//Planning决策状态
// dispatchHandlers.put(MessagePad.MessageType.MsgTypePlanningDecisionState, new DispatchHandler(MessagePad.MessageType.MsgTypePlanningDecisionState, this));
//清扫车后摄像头数据
dispatchHandlers.put(MessagePad.MessageType.MsgTypeBackCameraVideo, new DispatchHandler(MessagePad.MessageType.MsgTypeBackCameraVideo, this));
}
/**
@@ -446,32 +447,17 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
public void onDispatchRaw(MessagePad.MessageType type, RawData raw) {
// CupidLogUtils.i(TAG, "onDispatchRaw=" + Thread.currentThread().getName() + " TID=" + android.os.Process.myTid());
try {
//分发点云原始数据
if (type == MessagePad.MessageType.MsgTypePointCloud) {
int len = raw.getPackageLengthValue() - raw.getOffsetValue();
byte[] bytes = new byte[len];
System.arraycopy(raw.originalData.toByteArray(), raw.getOffsetValue(), bytes, 0, len);
calculateTimeConsumingOnDispatchRaw("3D点云", raw.receiveTime);
long nowTime = 0;
if (CupidLogUtils.isEnableLog())
nowTime = SystemClock.elapsedRealtime();
mAdasListener.onPointCloud(bytes);
calculateTimeConsumingBusiness("3D点云", nowTime);
} else {
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());
}
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());

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

@@ -1,15 +1,16 @@
package com.zhidao.support.adas.high;
import com.mogo.support.obu.ObuScene;
import com.zhjt.mogo.adas.data.bean.AutopilotStatistics;
import com.zhidao.support.adas.high.common.ProtocolStatus;
import com.zhjt.mogo.adas.data.bean.AutopilotStatistics;
import org.jetbrains.annotations.NotNull;
import bag_manager.BagManagerOuterClass;
import chassis.ChassisStatesOuterClass;
import chassis.VehicleStateOuterClass;
import function_state_management.FunctionStates;
import mogo.telematics.pad.MessagePad;
import mogo.v2x.ObuWarningEvent;
import mogo_msg.MogoReportMsg;
import perception.TrafficLightOuterClass;
import planning.RoboSweeperTaskIndexOuterClass;
@@ -108,9 +109,10 @@ public interface OnAdasListener {
/**
* 透传的点云数据
*
* @param header 头
* @param pointCloud 原始数据 只包含PointCloud数据
*/
void onPointCloud(byte[] pointCloud);
void onPointCloud(MessagePad.Header header, byte[] pointCloud);
/**
* planning障碍物
@@ -231,10 +233,18 @@ public interface OnAdasListener {
* 重构后的功能状态
*
* @param header 头
* @param functionStates 头
* @param functionStates 数据
*/
void onFunctionStates(MessagePad.Header header, FunctionStates.FSMFunctionStates functionStates);
/**
* 清扫车后部摄像头视频 10Hz
*
* @param header 头
* @param data 数据
*/
void onBackCameraVideo(@NotNull MessagePad.Header header, @NotNull byte[] data);
/**
* 清扫车指标数据
*

1
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java
View File

@@ -25,6 +25,7 @@ public enum MessageType {
TYPE_RECEIVE_PLANNING_OBJECTS(MessagePad.MessageType.MsgTypePlanningObjects, "Planning障碍物"),
TYPE_RECEIVE_CHASSIS_STATES(MessagePad.MessageType.MsgTypeChassisStates, "底盘状态"),
TYPE_RECEIVE_FUNCTION_STATES(MessagePad.MessageType.MsgTypeFunctionStates, "重构后功能状态"),
TYPE_RECEIVE_BACK_CAMERA_VIDEO(MessagePad.MessageType.MsgTypeBackCameraVideo, "清扫车后摄像头"),
TYPE_RECEIVE_BASIC_INFO_REQ(MessagePad.MessageType.MsgTypeBasicInfoReq, "自动驾驶设备基础信息请求"),
TYPE_SEND_BASIC_INFO_RESP(MessagePad.MessageType.MsgTypeBasicInfoResp, "自动驾驶设备基础信息应答"),

33
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/BackCameraVideoMessage.java Normal file
View File

@@ -0,0 +1,33 @@
package com.zhidao.support.adas.high.msg;
import android.os.SystemClock;
import com.google.protobuf.InvalidProtocolBufferException;
import com.zhidao.support.adas.high.AdasChannel;
import com.zhidao.support.adas.high.OnAdasListener;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhidao.support.adas.high.protocol.RawData;
/**
* 清扫车后摄像头
*/
public class BackCameraVideoMessage extends MyAbstractMessageHandler {
public BackCameraVideoMessage() {
}
@Override
public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException {
int len = raw.getPackageLengthValue() - raw.getOffsetValue();
byte[] data = new byte[len];
System.arraycopy(raw.originalData.toByteArray(), raw.getOffsetValue(), data, 0, len);
AdasChannel.calculateTimeConsumingOnDispatchRaw("清扫车后摄像头", raw.receiveTime);
long nowTime = 0;
if (CupidLogUtils.isEnableLog())
nowTime = SystemClock.elapsedRealtime();
if (adasListener != null) {
adasListener.onBackCameraVideo(raw.getHeader(), data);
}
AdasChannel.calculateTimeConsumingBusiness("清扫车后摄像头", nowTime);
}
}

14
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java
View File

@@ -36,6 +36,8 @@ public class MyMessageFactory implements IMyMessageFactory {
private IMsg functionStatesMessage;//重构后的功能状态
private IMsg sweeperTaskIndexDataMessage;//清扫车指标数据
private IMsg bagManagerMessage;//Bag管理应答
private IMsg backCameraVideoMessage;//清扫车后摄像头
private IMsg pointCloudMessage;//3D点云
private final AutopilotReview autopilotReview;
@@ -99,6 +101,12 @@ public class MyMessageFactory implements IMyMessageFactory {
predictionObstacleTrajectoryMessage = new PredictionObstacleTrajectoryMessage();
}
return predictionObstacleTrajectoryMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_POINT_CLOUD.typeCode) {
//3D点云
if (pointCloudMessage == null) {
pointCloudMessage = new PointCloudMessage();
}
return pointCloudMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_PLANNING_OBJECTS.typeCode) {
//planning障碍物
if (planningObjectsMessage == null) {
@@ -183,6 +191,12 @@ public class MyMessageFactory implements IMyMessageFactory {
bagManagerMessage = new BagManagerMessage();
}
return bagManagerMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_BACK_CAMERA_VIDEO.typeCode) {
//清扫车后摄像头
if (backCameraVideoMessage == null) {
backCameraVideoMessage = new BackCameraVideoMessage();
}
return backCameraVideoMessage;
} else {
//MessageType.TYPE_DEFAULT.typeCode
return null;

34
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/PointCloudMessage.java Normal file
View File

@@ -0,0 +1,34 @@
package com.zhidao.support.adas.high.msg;
import android.os.SystemClock;
import com.google.protobuf.InvalidProtocolBufferException;
import com.zhidao.support.adas.high.AdasChannel;
import com.zhidao.support.adas.high.OnAdasListener;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhidao.support.adas.high.protocol.RawData;
/**
* 3D点云原始数据
*/
public class PointCloudMessage extends MyAbstractMessageHandler {
public PointCloudMessage() {
}
@Override
public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException {
int len = raw.getPackageLengthValue() - raw.getOffsetValue();
byte[] data = new byte[len];
System.arraycopy(raw.originalData.toByteArray(), raw.getOffsetValue(), data, 0, len);
AdasChannel.calculateTimeConsumingOnDispatchRaw("3D点云", raw.receiveTime);
long nowTime = 0;
if (CupidLogUtils.isEnableLog())
nowTime = SystemClock.elapsedRealtime();
if (adasListener != null) {
adasListener.onPointCloud(raw.getHeader(), data);
}
AdasChannel.calculateTimeConsumingBusiness("3D点云", nowTime);
}
}