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

[sweeper310][adas] 新增FSM状态查询和响应接口，SSM3.0.0版本兼容，是否可以启动自动驾驶能力检测三端兼容（FSM、SSM、底盘）

Browse Source
This commit is contained in:
xinfengkun
2023-05-20 20:06:48 +08:00
parent 50ee8a5ca7
commit 6f23246aa8
15 changed files with 567 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/mogo-adas-data/src/main/proto/message_pad.proto
View File

@@ -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
//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;
}

4
libraries/mogo-adas/build.gradle
View File

@@ -65,6 +65,10 @@ android {
lintOptions {
abortOnError false
}
compileOptions {
sourceCompatibility JavaVersion.VERSION_1_8
targetCompatibility JavaVersion.VERSION_1_8
}
}

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

@@ -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());
}
/**
* 发送清扫车指令到云控

63
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java
View File

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

10
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java
View File

@@ -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 响应信息

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

@@ -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
*

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

@@ -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, "清扫车指标数据"),

13
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java
View File

@@ -13,7 +13,7 @@ import chassis.ChassisStatesOuterClass;
import system_master.SystemStatusInfo;
/**
* 是否可以启动自动驾驶能力检测 工控机版本>=250使用此类
* 是否可以启动自动驾驶能力检测 非清扫车MAP版本>=250使用此类 清扫车MAP版本>=250&& <330使用此类
* 目前监控了底盘的一些状态和查询节点状态应答的数据
* 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知
* <p>
@@ -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();

175
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java Normal file
View File

@@ -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的相关功能
* 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知
* <p>
* 此定时器不能停止 鹰眼中存在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<SystemStatusInfo.NodeInfo> 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);
}
}
}

108
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java
View File

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

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

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

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

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

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

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

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

@@ -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<Integer, Boolean> 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);
}
}

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

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