diff --git a/libraries/mogo-adas-data/src/main/proto/message_pad.proto b/libraries/mogo-adas-data/src/main/proto/message_pad.proto index 631aee93b6..0fb0670fc3 100644 --- a/libraries/mogo-adas-data/src/main/proto/message_pad.proto +++ b/libraries/mogo-adas-data/src/main/proto/message_pad.proto @@ -70,6 +70,7 @@ enum MessageType MsgTypeTaskCmd = 0x10122; //云控任务指令, 云控<->鹰眼双向透传 MsgTypeFSMStatusReasonQueryReq = 0x10123; //fsm状态原因查询 MsgTypeFSMStatusReasonQueryResp = 0x10124; //fsm状态原因查询应答 + MsgTypeGetDebugInfo = 0x10125; //debug信息查询 pad->telematics } message Header @@ -125,7 +126,7 @@ message TrackedSource message TrackedObject { - uint32 type = 1; //物体类型, 0:Background, 1:Person, 2:Bicycle, 3:Car, 4:MotorCycle, + uint32 type = 1; //物体类型, 0:Background, 1:Person, 2:Bicycle, 3:Car, 4:MotorCycle, //5:TrafficSign, 6:Bus, 7:CellPhone, 8:Truck, 9:Bottle, 10:TrafficLight, //11:Rider, 12:TriangleRoadblock, 13:WarningTriangle, 100:Unknown, //501:RoadWork_occupy_0501, 502:RoadWork_break_0502 @@ -175,7 +176,7 @@ message TrackedObjects // message definition for MsgTypeGnssInfo message GnssInfo { - double longitude = 1; //经度 + double longitude = 1; //经度 double latitude = 2; //纬度 double altitude = 3; //海拔 double heading = 4; //航向角 @@ -264,6 +265,7 @@ message BasicInfoResp string sn = 1; uint32 environment = 2; //1: 研发环境, 2:测试环境, 3:生产环境 4:演示环境 uint32 role = 3; //客户端角色：0: 司机屏，1: 乘客屏，2: 调试屏 + string version = 4; //鹰眼版本 } // message definition for MsgTypeSetAutopilotModeReq @@ -316,11 +318,20 @@ message CarConfigResp string macAddress = 3; ProtocolVersion protocolVersion = 4; //通信协议版本 double speedLimit = 5; //自动驾驶限速, 单位：m/s - double maxSpeedLimit = 6; //最大自动驾驶限速, 单位：m/s + double maxSpeedLimit = 6; //最大自动驾驶限速, 单位：m/s double minAcceleration = 7; //最小加速度, 单位：m/s² double maxAcceleration = 8; //最大加速度, 单位：m/s² string carType = 9; //车辆类型 string subCarType = 10; //车辆子类型 + reserved 11 to 100; + int32 mapVersion = 101;//dockVersion解析出版本号，解析失败为-1。例如："MAP-taxi_RoboTaxi_df_2.8.0.3_20220928_test" 解析结果为：280 + bool isDF = 102;//车型是否是东风 + bool isHQ = 103;//车型是否是红旗 + bool isJinlv = 104;//车型是否是金旅小巴 + bool isJinlvM1 = 105;//车型是否是金旅M1 + bool isJinlvM2 = 106;//车型是否是金旅M2 + bool isFutianSweeper = 107;//车型是否是福田清扫车 + bool isKaiwo = 108;//车型是否是开沃 } // message definition for MsgTypeRecordCause @@ -584,13 +595,14 @@ message PlanningActionMsg //message definition for MsgTypeSetParamReq message SetOneParam { - uint32 type = 1; // 0:default 1:绕障类功能开关(bool) 2:变道绕障的目标障碍物速度阈值(double, m/s) + uint32 type = 1; // 0:default 1:绕障类功能开关(bool) 2:变道绕障的目标障碍物速度阈值(double, m/s) // 3:AEB开关(bool) 0:关闭自动紧急制动功能 1:启用自动紧急制动功能 // 4:限制绕障开关(bool) 0:正常绕障 1：限制绕障 默认0 // 5:停车让行线前避让等待开关(bool) 0:停车让行线前无需等待 1:停车让行线前需要等待 默认0 // 6:地图限速功能开关(bool) 0:不使用地图限速功能 1:使用地图限速功能 默认0 // 7:环岛模式开关(bool) 0:普通模式 1:环岛模式 默认0 - // 8:弱网减速停车策略开关(bool) 0:关闭弱网减速停车策略 1:使用弱网减速停车策略 + // 8:弱网减速停车策略开关(bool) 0:关闭弱网减速停车策略 1:使用弱网减速停车策略 + // 9:m1拼接视频自车位置参数(string), 格式:x,y,width,height string value = 2; // 转成字符串的值 } @@ -647,9 +659,6 @@ message PlanningCmd //message definition for MsgTypeBackCameraVideo //payload:jpeg data -//message definition for MsgTypeM1StitchedVideo -//payload:jpeg data - //message definition for MsgTypeV2nCongestionEvent //refer to RSI_PB in mogo_v2x.proto for details @@ -669,4 +678,25 @@ message PlanningCmd //refer to FSMStatusReasonQuery.proto for details //message definition for MsgTypeFSMStatusReasonQueryResp -//refer to FSMStatusReasonQuery.proto for details \ No newline at end of file +//refer to FSMStatusReasonQuery.proto for details + +//message definition for MsgTypeM1StitchedVideo +//payload:jpeg data + +//message definition for MsgTypeGetDebugInfo +message DebugInfo +{ + uint32 seq = 1; //seq num + repeated SessionInfo sessions = 2; //有效session信息 +} + +message SessionInfo +{ + uint32 sessionID = 1; + uint32 role = 2; + string sn = 3; + uint64 connectedTimeStamp = 4; + string version = 5; +} + + diff --git a/libraries/mogo-adas/build.gradle b/libraries/mogo-adas/build.gradle index 5710f48d1e..cbfe6e34dd 100644 --- a/libraries/mogo-adas/build.gradle +++ b/libraries/mogo-adas/build.gradle @@ -65,6 +65,10 @@ android { lintOptions { abortOnError false } + compileOptions { + sourceCompatibility JavaVersion.VERSION_1_8 + targetCompatibility JavaVersion.VERSION_1_8 + } } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java index a3311dccab..dc487d8475 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java @@ -66,6 +66,7 @@ import java.util.concurrent.atomic.AtomicInteger; import bag_manager.BagManagerOuterClass; import chassis.SpecialVehicleTaskCmdOuterClass; import common.HeaderOuterClass; +import function_state_management.FSMStatusReasonQueryOuterClass; import mogo.telematics.pad.MessagePad; import okio.ByteString; @@ -1576,6 +1577,21 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec return sendPBMessage(MessageType.TYPE_SEND_BAG_MANAGER_CMD.typeCode, bagManager.toByteArray()); } + /** + * FSM状态原因查询 + * 当FSMFunctionStates的类型是XXX_DRIVING_OFF时查询OFF的原因 + * 当FSMFunctionStates的类型是PILOT_DRIVING_OFF时会自动调用 + * + * @param type 0:default, 1:请求反馈原因 + * @return boolean + */ + @Override + public boolean sendFSMStatusReasonQuery(int type) { + FSMStatusReasonQueryOuterClass.FSMStatusReasonQuery query = FSMStatusReasonQueryOuterClass.FSMStatusReasonQuery.newBuilder() + .setType(type) + .build(); + return sendPBMessage(MessageType.TYPE_SEND_FSM_STATUS_REASON_QUERY_REQ.typeCode, query.toByteArray()); + } /** * 发送清扫车指令到云控 diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java index 6f6b0d5c67..abcc99229c 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java @@ -1,12 +1,14 @@ package com.zhidao.support.adas.high; import android.content.Context; +import android.text.TextUtils; import androidx.annotation.NonNull; import com.zhidao.support.adas.high.bean.VersionCompatibility; import com.zhidao.support.adas.high.common.AppPreferenceHelper; import com.zhidao.support.adas.high.common.Constants; +import com.zhidao.support.adas.high.common.CupidLogUtils; import com.zhidao.support.adas.high.common.Define; import com.zhidao.support.adas.high.common.MessageType; import com.zhidao.support.adas.high.common.ReceiveTimeoutManager; @@ -19,6 +21,8 @@ import com.zhjt.mogo.adas.data.sweeper.task.stop.SweeperTaskStop; import java.util.HashSet; import java.util.List; import java.util.Set; +import java.util.regex.Matcher; +import java.util.regex.Pattern; import bag_manager.BagManagerOuterClass; import chassis.SpecialVehicleTaskCmdOuterClass; @@ -44,7 +48,7 @@ public class AdasManager implements IAdasNetCommApi { private static final int PROTOCOL_VERSION = MessagePad.ProtocolVersion.CurrentVersion.getNumber(); private volatile MessagePad.CarConfigResp carConfig; private static final String ADAS_VERSION = BuildConfig.VERSION_NAME; - + private final Pattern pattern = Pattern.compile("\\d+\\.\\d+\\.\\d+"); private AdasChannel mChannel; public static AdasManager getInstance() { @@ -827,6 +831,19 @@ public class AdasManager implements IAdasNetCommApi { return mChannel != null && mChannel.sendBagManagerCmd(bagManager); } + /** + * FSM状态原因查询 + * 当FSMFunctionStates的类型是XXX_DRIVING_OFF时查询OFF的原因 + * 当FSMFunctionStates的类型是PILOT_DRIVING_OFF时会自动调用 + * + * @param type 0:default, 1:请求反馈原因 + * @return boolean + */ + @Override + public boolean sendFSMStatusReasonQuery(int type) { + return mChannel != null && mChannel.sendFSMStatusReasonQuery(type); + } + /** * 开始大任务响应 * 1、云端下发大任务信息（type: 100001）-> sweeper_task.proto/StartTaskResp 响应信息 @@ -978,4 +995,48 @@ public class AdasManager implements IAdasNetCommApi { public long getTimeoutDetectionTime() { return ReceiveTimeoutManager.getInstance().getTimeout(); } + + /** + * 解析版本 格式 xxx.xxx.xxx(x的数量不固定) + * 如果用于比较，仅适用于除个位数非0的字符串，例如："12.03.04" 解析出为120304可能无法正常对比 + * 目前已用于DockerVersion和MaserVersion的解析 + * + * @param ver 版本字符串 例如："MAP-taxi_RoboTaxi_df_2.8.0.3_20220928_test" 解析结果为：280 + * @return -1表示解析失败 + */ + public int parseVersion(String ver) { + return parseVersion(true, ver); + } + + /** + * 解析版本 格式 xxx.xxx.xxx(x的数量不固定) + * 如果用于比较，仅适用于非个位数非0的字符串，例如："12.03.04" 解析出为120304可能无法正常对比 + * 目前已用于DockerVersion和MaserVersion的解析 + * + * @param isUseAll 是否使用全部截取数据 true：表示 12.34.56 截取之后 123456 false：表示12.34.56 截取之后 12 + * @param ver 版本字符串 例如："MAP-taxi_RoboTaxi_df_2.8.0.3_20220928_test" 解析结果为：280 + * @return -1表示解析失败 + */ + public int parseVersion(boolean isUseAll, String ver) { + int version = -1; + if (!TextUtils.isEmpty(ver)) { + try { + Matcher matcher = pattern.matcher(ver); + if (matcher.find()) { + String group = matcher.group(); + if (!TextUtils.isEmpty(group)) { + if (isUseAll) { + group = group.replace(".", ""); + } else { + group = group.split("\\.")[0]; + } + version = Integer.parseInt(group); + } + } + } catch (Exception e) { + CupidLogUtils.e("AdasManager", "版本解析失败=" + ver, e); + } + } + return version; + } } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java index af1c9776b0..76a39773be 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java @@ -511,6 +511,16 @@ public interface IAdasNetCommApi { */ boolean sendBagManagerCmd(BagManagerOuterClass.BagManager bagManager); + /** + * FSM状态原因查询 + * 当FSMFunctionStates的类型是XXX_DRIVING_OFF时查询OFF的原因 + * 当FSMFunctionStates的类型是PILOT_DRIVING_OFF时会自动调用 + * + * @param type 0:default, 1:请求反馈原因 + * @return boolean + */ + boolean sendFSMStatusReasonQuery(int type); + /** * 开始大任务响应 * 1、云端下发大任务信息（type: 100001）-> sweeper_task.proto/StartTaskResp 响应信息 diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java index 6e0e3d6460..b6428d708c 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java @@ -240,13 +240,21 @@ public interface OnAdasListener { void onObuMapMath(MessagePad.Header header, ObuScene.MapMatchData mapMatchData); /** - * 重构后的功能状态 + * 重构后的功能状态 20hz * * @param header 头 * @param functionStates 数据 */ void onFunctionStates(MessagePad.Header header, FunctionStates.FSMFunctionStates functionStates); + /** + * FSM状态原因查询 回调 + * + * @param header 头 + * @param respond 数据 + */ +// void onFSMStatusReasonRespond(@NonNull MessagePad.Header header, @NonNull FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond respond); + /** * 清扫车后部摄像头视频 10Hz * diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java index 4d8477a1e4..8597b10e14 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java @@ -65,6 +65,8 @@ public enum MessageType { TYPE_SEND_PLANNING_CMD(MessagePad.MessageType.MsgTypePlanningCmd, "给Planning指令"), TYPE_SEND_SWEEPER_CLOUD_TASK_CMD(MessagePad.MessageType.MsgTypeTaskCmd, "发送清扫车指令到云控"), TYPE_RECEIVE_SWEEPER_CLOUD_TASK_CMD(MessagePad.MessageType.MsgTypeTaskCmd, "云控下发清扫车任务指令"), + TYPE_SEND_FSM_STATUS_REASON_QUERY_REQ(MessagePad.MessageType.MsgTypeFSMStatusReasonQueryReq, "FSM状态原因查询"), + TYPE_RECEIVE_FSM_STATUS_REASON_QUERY_RESP(MessagePad.MessageType.MsgTypeFSMStatusReasonQueryResp, "FSM状态原因查询应答"), //TODO 透传原始pb文件中不存在以下type。由于Java中无法强转,所以在mogo-adas-data/message_pad.proto中放开注释 TYPE_RECEIVE_PLANNING_DECISION_STATE(MessagePad.MessageType.MsgTypePlanningDecisionState, "Planning决策状态"), TYPE_RECEIVE_SWEEPER_TASK_INDEX_DATA(MessagePad.MessageType.MsgTypeSweeperTaskIndexData, "清扫车指标数据"), diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java index ff336195ee..81afbb44dc 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java @@ -13,7 +13,7 @@ import chassis.ChassisStatesOuterClass; import system_master.SystemStatusInfo; /** - * 是否可以启动自动驾驶能力检测 工控机版本>=250使用此类 + * 是否可以启动自动驾驶能力检测 非清扫车MAP版本>=250使用此类 清扫车MAP版本>=250&& <330使用此类 * 目前监控了底盘的一些状态和查询节点状态应答的数据 * 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知 *

@@ -53,10 +53,15 @@ public class AutopilotAbility250 { if (statusInfo.hasMasterVersion()) { //截取Master Version String masterVersion = statusInfo.getMasterVersion(); - version = AutopilotAbilityManager.getInstance().parseVersion(false, masterVersion); + version = AdasManager.getInstance().parseVersion(false, masterVersion); } - //如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行 - if (version > 1 && statusInfo.hasAutoPilotReady()) { + //SSM 3版本兼容 + if (version > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) { + isAutopilotAbility = statusInfo.getAutoPilotReady(); + if (!isAutopilotAbility) { + unableAutopilotReason = statusInfo.getAutoPilotUnreadyReason(); + } + } else if (version > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行 isAutopilotAbility = statusInfo.getAutoPilotReady(); if (!isAutopilotAbility) { SystemStatusInfo.NodeFaultList nodeFaultList = statusInfo.getAutoPilotUnreadyList(); diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java new file mode 100644 index 0000000000..49c8c5c192 --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java @@ -0,0 +1,175 @@ +package com.zhidao.support.adas.high.common.autopilot.ability; + +import android.os.Message; + +import com.google.protobuf.TextFormat; +import com.zhidao.support.adas.high.AdasManager; + +import java.util.List; +import java.util.Timer; +import java.util.TimerTask; + +import function_state_management.FSMStatusReasonQueryOuterClass; +import system_master.SystemStatusInfo; + +/** + * 是否可以启动自动驾驶能力检测 工控机版本>=330使用此类 + * 目前监控了查询节点状态应答的数据和FSM状态原因查询 + * TODO 目前只用于清扫车，其他车型需要MAP支持 FSM的相关功能 + * 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知 + *

+ * 此定时器不能停止 鹰眼中存在UI更新依赖循环查询系统状态 + */ +public class AutopilotAbility330 { + private static final String TAG = AutopilotAbility330.class.getSimpleName(); + private static final int WHAT_TIMEOUT = 0; + private static final int DEFAULT_TIMEOUT = 1500; + private static final long DEFAULT_DETECTION_TIME = 3 * 1000L;//默认检测时间 + private static final String[] NODE_INFO_STATE = {"未知状态 ", "依赖未就绪 ", "启动中 ", "运行 ", "停止 ", "无法启动状态 ", "人为启动状态 ", "人为关闭状态 "}; + private volatile Timer timer; + + private volatile FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond;//自动驾驶状态为OFF的原因 + + public AutopilotAbility330() { + } + + public synchronized void setFsmStatusReasonRespond(FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) { + this.fsmStatusReasonRespond = fsmStatusReasonRespond; + } + + public void setStatusInfo(SystemStatusInfo.StatusInfo statusInfo) { + if (AutopilotAbilityManager.getInstance().getHandler() != null) { + if (AutopilotAbilityManager.getInstance().getHandler().hasMessages(WHAT_TIMEOUT)) + AutopilotAbilityManager.getInstance().getHandler().removeMessages(WHAT_TIMEOUT); + } + onCallback(statusInfo); + } + + + private void onCallback(SystemStatusInfo.StatusInfo statusInfo) { + boolean isAutopilotAbility = true;//是否能启动自动驾驶 + String unableAutopilotReason = null;//不能启动自动驾驶原因 + //检测节点状态相关 + if (statusInfo != null) { + int version = -1; + if (statusInfo.hasMasterVersion()) { + //截取Master Version + String masterVersion = statusInfo.getMasterVersion(); + version = AdasManager.getInstance().parseVersion(false, masterVersion); + } + //SSM 3版本兼容 + if (version > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) { + isAutopilotAbility = statusInfo.getAutoPilotReady(); + if (!isAutopilotAbility) { + unableAutopilotReason = statusInfo.getAutoPilotUnreadyReason(); + } + } else if (version > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行 + isAutopilotAbility = statusInfo.getAutoPilotReady(); + if (!isAutopilotAbility) { + SystemStatusInfo.NodeFaultList nodeFaultList = statusInfo.getAutoPilotUnreadyList(); + if (nodeFaultList.getSum() > 0) { + List list = nodeFaultList.getNodeList(); + StringBuilder builder = new StringBuilder(); + for (SystemStatusInfo.NodeInfo info : list) { + builder.append(info.getNodeName()); + int state = info.getState(); + if (state < NODE_INFO_STATE.length) { + builder.append(NODE_INFO_STATE[state]); + } else { + builder.append("未知异常 "); + } + } + unableAutopilotReason = builder.toString(); + } else { + unableAutopilotReason = "未知异常节点"; + } + } + } else { + SystemStatusInfo.SystemState systemState = statusInfo.getSysState(); + // 目前已知可以下发启动自驾命令的状态： SystemState.SYS_RUNNING、SystemState.PILOT_READY、SystemState.AUTO_PILOT_STARTING、SystemState.AUTO_PILOT_RUNNING + if (systemState != SystemStatusInfo.SystemState.SYS_RUNNING && systemState != SystemStatusInfo.SystemState.PILOT_READY) { + isAutopilotAbility = false; + if (systemState == SystemStatusInfo.SystemState.SYS_STARTING) { + unableAutopilotReason = "系统正在启动"; + } else if (systemState == SystemStatusInfo.SystemState.SYS_EXITING) { + unableAutopilotReason = "系统正在关闭"; + } else if (systemState == SystemStatusInfo.SystemState.SYS_FAULT) { + unableAutopilotReason = "系统异常"; + } else if (systemState == SystemStatusInfo.SystemState.AUTO_PILOT_STARTING) { + //TODO 如果第一次下发了启动自动驾驶，没起来或者存在干预 此时systemState是AUTO_PILOT_STARTING 需要二次下发启动自驾命令 所以需要排除此状态 + isAutopilotAbility = true; + //unableAutopilotReason = "正在开始自动驾驶"; + } else if (systemState == SystemStatusInfo.SystemState.AUTO_PILOT_RUNNING) { + //TODO 车辆进站属于未退自驾状态，此状态不进行限制，否则无法再次发下启动自驾命令 + isAutopilotAbility = true; +// unableAutopilotReason = "自动驾驶运行中"; + } else if (systemState == SystemStatusInfo.SystemState.REMOTE_PILOT_STARTING) { + unableAutopilotReason = "平行驾驶启动中"; + } else if (systemState == SystemStatusInfo.SystemState.REMOTE_PILOT_RUNNING) { + unableAutopilotReason = "平行驾驶运行中"; + } else { + unableAutopilotReason = "未知系统状态"; + } + } + } + } else { + isAutopilotAbility = false;//是否能启动自动驾驶 + unableAutopilotReason = "SSM状态查询超时无响应";//不能启动自动驾驶原因 + } + if (isAutopilotAbility) { + if (fsmStatusReasonRespond != null) { + int count = fsmStatusReasonRespond.getFsmStatusReasonRespondCount(); + StringBuilder builder = null; + if (count > 0) { + builder = new StringBuilder(); + for (int i = 0; i < count; i++) { + String respond = fsmStatusReasonRespond.getFsmStatusReasonRespond(i); + builder.append(respond).append(' '); + } + } + if (builder != null) { + isAutopilotAbility = false; + builder.deleteCharAt(builder.length() - 1);//删除末尾 + unableAutopilotReason = builder.toString(); + } + } + } + AutopilotAbilityManager.getInstance().onAutopilotAbility(isAutopilotAbility, unableAutopilotReason); + } + + public synchronized void start() { + if (timer == null) { + timer = new Timer(); + timer.schedule(new TimerTask() { + @Override + public void run() { + if (AutopilotAbilityManager.getInstance().getOnAutopilotAbilityListener() != null) { + AutopilotAbilityManager.getInstance().getOnAutopilotAbilityListener().onStatusQuery(); + } + AdasManager.getInstance().sendStatusQueryReq(); + if (AutopilotAbilityManager.getInstance().getHandler() != null) { + if (AutopilotAbilityManager.getInstance().getHandler().hasMessages(WHAT_TIMEOUT)) + AutopilotAbilityManager.getInstance().getHandler().removeMessages(WHAT_TIMEOUT); + AutopilotAbilityManager.getInstance().getHandler().sendEmptyMessageDelayed(WHAT_TIMEOUT, DEFAULT_TIMEOUT); + } + } + }, 2000L, DEFAULT_DETECTION_TIME);//延迟执行，避免刚连接成功后底盘信息无法及时同步 + } + } + + public synchronized void stop() { + if (timer != null) { + timer.cancel(); + timer = null; + } + } + + + public void onHandleMessage(Message msg) { + if (msg.what == WHAT_TIMEOUT) { + onCallback(null); + } + } + + +} diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java index 5ba1d3d65b..4f06bcbaed 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java @@ -2,7 +2,7 @@ package com.zhidao.support.adas.high.common.autopilot.ability; import android.os.Handler; import android.os.Message; -import android.text.TextUtils; +import android.util.Log; import com.zhidao.support.adas.high.AdasManager; import com.zhidao.support.adas.high.OnAdasListener; @@ -12,11 +12,10 @@ import com.zhidao.support.adas.high.common.CupidLogUtils; import java.util.Set; import java.util.Timer; import java.util.TimerTask; -import java.util.regex.Matcher; -import java.util.regex.Pattern; import chassis.Chassis; import chassis.ChassisStatesOuterClass; +import function_state_management.FSMStatusReasonQueryOuterClass; import mogo.telematics.pad.MessagePad; import system_master.SystemStatusInfo; @@ -31,12 +30,13 @@ public class AutopilotAbilityManager { private static final String TAG = AutopilotAbilityManager.class.getSimpleName(); private static volatile AutopilotAbilityManager INSTANCE; private OnAdasListener listener; - private final Pattern pattern = Pattern.compile("\\d+\\.\\d+\\.\\d+"); private Handler handler; private OnAutopilotAbilityListener onAutopilotAbilityListener; - private int dockerVersion = -1;//工控机版本 + private int mapVersion = -1;//工控机版本 + private boolean isFutianSweeper = false;//是否是福田清扫车 private AutopilotAbility230 autopilotAbility230; private AutopilotAbility250 autopilotAbility250; + private AutopilotAbility330 autopilotAbility330; private Timer startTimer; /** * 不能启动自动驾驶的档位 @@ -81,15 +81,12 @@ public class AutopilotAbilityManager { } public void setCarConfig(MessagePad.CarConfigResp carConfig) { - if (dockerVersion == -1) { - String v = carConfig.getDockVersion(); - int version = parseVersion(true, v); - if (version != -1) { - stopTimer(); - dockerVersion = version; - CupidLogUtils.i(TAG, "工控机版本=" + dockerVersion); - initAutopilotAbility(); - } + mapVersion = carConfig.getMapVersion(); + isFutianSweeper = carConfig.getIsFutianSweeper(); + if (mapVersion != -1) { + stopTimer(); + CupidLogUtils.i(TAG, "工控机版本=" + mapVersion); + initAutopilotAbility(); } } @@ -120,12 +117,25 @@ public class AutopilotAbilityManager { } + /** + * SSM状态更新 + * + * @param statusInfo SSM + */ public void setStatusInfo(SystemStatusInfo.StatusInfo statusInfo) { if (autopilotAbility250 != null) { autopilotAbility250.setStatusInfo(statusInfo); } + if (autopilotAbility330 != null) { + autopilotAbility330.setStatusInfo(statusInfo); + } } + /** + * 底盘状态更新 + * + * @param chassisStates 底盘 + */ public void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates) { if (autopilotAbility230 != null) { autopilotAbility230.setChassisStates(chassisStates); @@ -135,6 +145,17 @@ public class AutopilotAbilityManager { } } + /** + * FSM OFF状态原因更新 + * + * @param fsmStatusReasonRespond 原因 + */ + public void setFsmStatusReasonRespond(FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) { + if (autopilotAbility330 != null) { + autopilotAbility330.setFsmStatusReasonRespond(fsmStatusReasonRespond); + } + } + public void onHandleMessage(Message msg) { if (autopilotAbility250 != null) { autopilotAbility250.onHandleMessage(msg); @@ -142,14 +163,27 @@ public class AutopilotAbilityManager { } private void initAutopilotAbility() { - if (dockerVersion >= 250) { + //目前只有MAP330的清扫车用的新的FSM状态原因查询 + if (mapVersion >= 330 && isFutianSweeper) { + Log.i(TAG, "是否可以启动自动驾驶能力检测使用330版本"); stop230(); + stop250(); + if (autopilotAbility330 == null) { + autopilotAbility330 = new AutopilotAbility330(); + autopilotAbility330.start(); + } + } else if (mapVersion >= 250) { + Log.i(TAG, "是否可以启动自动驾驶能力检测使用250版本"); + stop230(); + stop330(); if (autopilotAbility250 == null) { autopilotAbility250 = new AutopilotAbility250(); autopilotAbility250.start(); } } else { + Log.i(TAG, "是否可以启动自动驾驶能力检测使用230版本"); stop250(); + stop330(); if (autopilotAbility230 == null) { autopilotAbility230 = new AutopilotAbility230(); autopilotAbility230.start(); @@ -172,6 +206,13 @@ public class AutopilotAbilityManager { } } + private void stop330() { + if (autopilotAbility330 != null) { + autopilotAbility330.stop(); + autopilotAbility330 = null; + } + } + private void stopTimer() { if (startTimer != null) { startTimer.cancel(); @@ -189,8 +230,8 @@ public class AutopilotAbilityManager { startTimer.schedule(new TimerTask() { @Override public void run() { - if (dockerVersion == -1 && AdasManager.getInstance().getIpcConnectionStatus() == Constants.IPC_CONNECTION_STATUS.CONNECTED) { - dockerVersion = 230; + if (mapVersion == -1 && AdasManager.getInstance().getIpcConnectionStatus() == Constants.IPC_CONNECTION_STATUS.CONNECTED) { + mapVersion = 230; initAutopilotAbility(); } } @@ -204,37 +245,8 @@ public class AutopilotAbilityManager { stop230(); stop250(); handler = null; - dockerVersion = -1; + mapVersion = -1; + isFutianSweeper = false; } - - /** - * 解析版本 格式 xxx.xxx.xxx(x的数量不固定) - * - * @param isUserAll 是否使用全部截取数据 true：表示 12.34.56 截取之后 123456 false：表示12.34.56 截取之后 12 - * @param ver 版本字符串 - * @return -1表示解析失败 - */ - public int parseVersion(boolean isUserAll, String ver) { - int version = -1; - if (!TextUtils.isEmpty(ver)) { - try { - Matcher matcher = pattern.matcher(ver); - if (matcher.find()) { - String group = matcher.group(); - if (!TextUtils.isEmpty(group)) { - if (isUserAll) { - group = group.replace(".", ""); - } else { - group = group.split("\\.")[0]; - } - version = Integer.parseInt(group); - } - } - } catch (Exception e) { - CupidLogUtils.e(TAG, "版本解析失败=" + ver, e); - } - } - return version; - } } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java index e35bbb8071..a1b5876195 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java @@ -4,6 +4,7 @@ import android.os.SystemClock; import com.google.protobuf.InvalidProtocolBufferException; import com.zhidao.support.adas.high.AdasChannel; +import com.zhidao.support.adas.high.AdasManager; import com.zhidao.support.adas.high.OnAdasListener; import com.zhidao.support.adas.high.common.AutopilotReview; import com.zhidao.support.adas.high.common.CupidLogUtils; @@ -23,18 +24,21 @@ public class AutopilotStateMessage extends MyAbstractMessageHandler { @Override public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { - MessagePad.AutopilotState autopilotState = MessagePad.AutopilotState.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); - AdasChannel.calculateTimeConsumingOnDispatchRaw("自动驾驶状态", raw.receiveTime); - long nowTime = 0; - if (CupidLogUtils.isEnableLog()) - nowTime = SystemClock.elapsedRealtime(); - if (adasListener != null) { - adasListener.onAutopilotState(raw.getHeader(), autopilotState); - } - if (autopilotReview != null) { - autopilotReview.onAutopilotResult(autopilotState); - } - AdasChannel.calculateTimeConsumingBusiness("自动驾驶状态", nowTime); + //判断当前工控机非清扫车330及以上将进行解析（清扫车330及以上使用FSM，此接口将被兼容调用） + if (AdasManager.getInstance().getCarConfig() == null || !(AdasManager.getInstance().getCarConfig().getMapVersion() >= 330 && AdasManager.getInstance().getCarConfig().getIsFutianSweeper())) { + MessagePad.AutopilotState autopilotState = MessagePad.AutopilotState.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); + AdasChannel.calculateTimeConsumingOnDispatchRaw("自动驾驶状态", raw.receiveTime); + long nowTime = 0; + if (CupidLogUtils.isEnableLog()) + nowTime = SystemClock.elapsedRealtime(); + if (adasListener != null) { + adasListener.onAutopilotState(raw.getHeader(), autopilotState); + } + if (autopilotReview != null) { + autopilotReview.onAutopilotResult(autopilotState); + } + AdasChannel.calculateTimeConsumingBusiness("自动驾驶状态", nowTime); // CupidLogUtils.e("自动驾驶状态--->" + autopilotState.toString()); + } } } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/CarConfigRespMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/CarConfigRespMessage.java index 7a63aed0b6..01b40555df 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/CarConfigRespMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/CarConfigRespMessage.java @@ -1,6 +1,7 @@ package com.zhidao.support.adas.high.msg; import android.os.SystemClock; +import android.text.TextUtils; import com.google.protobuf.InvalidProtocolBufferException; import com.zhidao.support.adas.high.AdasChannel; @@ -20,6 +21,7 @@ public class CarConfigRespMessage extends MyAbstractMessageHandler { @Override public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { MessagePad.CarConfigResp carConfigResp = MessagePad.CarConfigResp.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); + carConfigResp = addPersonalParameter(carConfigResp); AdasChannel.calculateTimeConsumingOnDispatchRaw("车机基础信息应答", raw.receiveTime); AutopilotAbilityManager.getInstance().setCarConfig(carConfigResp); AdasManager.getInstance().setCarConfig(carConfigResp); @@ -32,4 +34,27 @@ public class CarConfigRespMessage extends MyAbstractMessageHandler { AdasChannel.calculateTimeConsumingBusiness("车机基础信息应答", nowTime); CupidLogUtils.e("车机基础信息应答--->" + carConfigResp.toString()); } + + /** + * 添加私有参数 + */ + private MessagePad.CarConfigResp addPersonalParameter(MessagePad.CarConfigResp carConfigResp) { + if (carConfigResp != null) { + String dockVersion = carConfigResp.getDockVersion(); + String carType = carConfigResp.getCarType().toLowerCase(); + String subCarType = carConfigResp.getSubCarType().toLowerCase(); + MessagePad.CarConfigResp.Builder builder = carConfigResp.toBuilder(); + builder.setMapVersion(AdasManager.getInstance().parseVersion(dockVersion)); + builder.setIsDF("df".equals(carType))//是否是东风 + .setIsHQ("hq".equals(carType))//是否是红旗 + .setIsJinlv("jinlv".equals(carType) && TextUtils.isEmpty(subCarType))//是否是金旅小巴 + .setIsJinlvM1("jinlv".equals(carType) && "m1".equals(subCarType))//是否是金旅M1 + .setIsJinlvM2("jinlv".equals(carType) && "m2".equals(subCarType))//是否是金旅M2 + .setIsFutianSweeper("sweeper".equals(carType))//是否是福田清扫车 + .setIsKaiwo("kaiwo".equals(carType));//是否是开沃 + return builder.build(); + } + return null; + } + } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSMStatusReasonRespondMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSMStatusReasonRespondMessage.java new file mode 100644 index 0000000000..6a6a623aab --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSMStatusReasonRespondMessage.java @@ -0,0 +1,36 @@ +package com.zhidao.support.adas.high.msg; + +import android.os.SystemClock; +import android.util.Log; + +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.common.autopilot.ability.AutopilotAbilityManager; +import com.zhidao.support.adas.high.protocol.RawData; + +import function_state_management.FSMStatusReasonQueryOuterClass; + +/** + * FSM状态原因查询 当FSMFunctionStates的类型是XXX_DRIVING_OFF时查询OFF的原因结果回调 + */ +public class FSMStatusReasonRespondMessage extends MyAbstractMessageHandler { + + public FSMStatusReasonRespondMessage() { + } + + @Override + public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { + FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond respond = FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); + AdasChannel.calculateTimeConsumingOnDispatchRaw("FSM状态原因查询", raw.receiveTime); + long nowTime = 0; + if (CupidLogUtils.isEnableLog()) + nowTime = SystemClock.elapsedRealtime(); + AutopilotAbilityManager.getInstance().setFsmStatusReasonRespond(respond); +// if (adasListener != null) { +// adasListener.onFSMStatusReasonRespond(raw.getHeader(), respond); +// } + AdasChannel.calculateTimeConsumingBusiness("FSM状态原因查询", nowTime); + } +} diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java index ff734645a7..435f84dff3 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java @@ -4,28 +4,119 @@ import android.os.SystemClock; import com.google.protobuf.InvalidProtocolBufferException; import com.zhidao.support.adas.high.AdasChannel; +import com.zhidao.support.adas.high.AdasManager; import com.zhidao.support.adas.high.OnAdasListener; import com.zhidao.support.adas.high.common.CupidLogUtils; +import com.zhidao.support.adas.high.common.autopilot.ability.AutopilotAbilityManager; import com.zhidao.support.adas.high.protocol.RawData; +import java.util.HashMap; +import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.atomic.AtomicLong; + import function_state_management.FunctionStates; +import mogo.telematics.pad.MessagePad; /** - * 重构后的功能状态 + * 重构后的功能状态(FSM) 20hz */ public class FunctionStatesMessage extends MyAbstractMessageHandler { + private final static String TAG = FunctionStatesMessage.class.getSimpleName(); + private final Map modeMap = new HashMap<>(); + private final AtomicLong timesCount = new AtomicLong(0);//几次，此接口1秒20次 + private final AtomicInteger oldMode = new AtomicInteger(0); + + private FunctionStates.PilotDrivingFunctionState oldPilotDrivingFunctionState = null;//上次自动驾驶状态 + + public FunctionStatesMessage() { + initModeMap(); + } + @Override public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { FunctionStates.FSMFunctionStates functionStates = FunctionStates.FSMFunctionStates.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); AdasChannel.calculateTimeConsumingOnDispatchRaw("重构后的功能状态", raw.receiveTime); long nowTime = 0; - if (CupidLogUtils.isEnableLog()) - nowTime = SystemClock.elapsedRealtime(); + if (CupidLogUtils.isEnableLog()) nowTime = SystemClock.elapsedRealtime(); + checkPilotDrivingFunctionStateOFF(functionStates.getPilotDrivingFunctionState()); if (adasListener != null) { - adasListener.onFunctionStates(raw.getHeader(), functionStates); + MessagePad.Header header = raw.getHeader(); + adasListener.onFunctionStates(header, functionStates); + AdasManager manager = AdasManager.getInstance(); + if (manager.getCarConfig() != null && manager.getCarConfig().getMapVersion() >= 330 && manager.getCarConfig().getIsFutianSweeper()) {//判断当前MAP版本>=330并且是清扫车才调用兼容方法 + compatibility(header, functionStates, adasListener); + } } AdasChannel.calculateTimeConsumingBusiness("重构后的功能状态", nowTime); } + /** + * 检查自动驾驶状态是否为OFF + * + * @param pilotDrivingFunctionState 自动驾驶状态 + */ + private void checkPilotDrivingFunctionStateOFF(FunctionStates.PilotDrivingFunctionState pilotDrivingFunctionState) { + if (oldPilotDrivingFunctionState != pilotDrivingFunctionState) { + oldPilotDrivingFunctionState = pilotDrivingFunctionState; + if (pilotDrivingFunctionState == FunctionStates.PilotDrivingFunctionState.PILOT_DRIVING_OFF) { + //如果自动驾驶状态为OFF需要查询原因 + AdasManager.getInstance().sendFSMStatusReasonQuery(1); + } else { + AutopilotAbilityManager.getInstance().setFsmStatusReasonRespond(null); + } + } + + } + + //兼容老版本的自动驾驶状态接口 + private void compatibility(MessagePad.Header header, FunctionStates.FSMFunctionStates functionStates, OnAdasListener adasListener) { + int mode = 0; + int iter = functionStates.getPilotDrivingFunctionState().getNumber(); + if (modeMap.containsKey(iter)) { + if (modeMap.get(iter)) { + mode = 1; + } + } + iter = functionStates.getParallelDrivingFunctionState().getNumber(); + if (modeMap.containsKey(iter)) { + if (modeMap.get(iter)) { + mode = 6; + } + } + //如果有变化立即更新 + if (oldMode.get() != mode) { + oldMode.set(mode); + timesCount.set(-19); + sendAutopilotState(mode, header, adasListener); + CupidLogUtils.i(TAG, "FSM转老自动驾驶,立即变更状态=" + (mode + 1)); + } + //降频1秒发一次 + if (timesCount.getAndIncrement() % 20 == 0) { + sendAutopilotState(mode, header, adasListener); + CupidLogUtils.i(TAG, "FSM转老自动驾驶,定频状态=" + (mode + 1)); + } + } + + private void sendAutopilotState(int mode, MessagePad.Header header, OnAdasListener adasListener) { + MessagePad.AutopilotState autopilotState = MessagePad.AutopilotState.newBuilder().setAutopilotMode(mode).setState(mode + 1).build(); + adasListener.onAutopilotState(header.toBuilder().setMsgType(MessagePad.MessageType.MsgTypeAutopilotState).build(), autopilotState); + + } + + private void initModeMap() { + modeMap.put(0, false); + modeMap.put(1, false); + modeMap.put(2, true); + modeMap.put(3, true); + modeMap.put(4, true); + modeMap.put(5, true); + modeMap.put(6, false); + modeMap.put(7, true); + modeMap.put(8, true); + modeMap.put(9, true); + modeMap.put(10, true); + } + } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java index 1cb5b0dc9c..f522fbf5a7 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java @@ -40,6 +40,7 @@ public class MyMessageFactory implements IMyMessageFactory { private IMsg pointCloudMessage;//3D点云 private IMsg sweeperCloudTaskMessage;//云控下发清扫车任务指令 private IMsg m1StitchedVideoMessage;//M1拼接视频 + private IMsg fSMStatusReasonRespondMessage;//FSM状态原因查询 private final AutopilotReview autopilotReview; @@ -206,12 +207,18 @@ public class MyMessageFactory implements IMyMessageFactory { } return sweeperCloudTaskMessage; } else if (messageType == MessageType.TYPE_RECEIVE_M1_STITCHED_VIDEO.typeCode) { - //清扫车后摄像头 + //M1拼接视频 if (m1StitchedVideoMessage == null) { m1StitchedVideoMessage = new M1StitchedVideoMessage(); } return m1StitchedVideoMessage; - } else { + } else if (messageType == MessageType.TYPE_RECEIVE_FSM_STATUS_REASON_QUERY_RESP.typeCode) { + //FSM状态原因查询 + if (fSMStatusReasonRespondMessage == null) { + fSMStatusReasonRespondMessage = new FSMStatusReasonRespondMessage(); + } + return fSMStatusReasonRespondMessage; + } else { //MessageType.TYPE_DEFAULT.typeCode return null; }