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

[644][adas] 能否启动自驾 添加原始数据日志

Browse Source
This commit is contained in:
xinfengkun
2024-05-22 20:01:14 +08:00
parent d8f7b53444
commit 98edf0ca49
23 changed files with 432 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/LaunchConditionData.java Normal file
View File

@@ -0,0 +1,163 @@
package com.zhjt.mogo.adas.data.bean;
import com.google.protobuf.GeneratedMessageV3;
import com.google.protobuf.InvalidProtocolBufferException;
import com.google.protobuf.TextFormat;
import com.google.protobuf.util.JsonFormat;
import org.json.JSONArray;
import org.json.JSONException;
import org.json.JSONObject;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import function_state_management.FSMStatusReasonQueryOuterClass;
import system_master.SsmInfo;
import system_master.SystemStatusInfo;
/**
* 检测启动自动驾驶的数据
* 此类目前主要用于启动自驾命令下发后的域控数据记录
*/
public class LaunchConditionData {
/**
* 所使用的检测版本
*/
public final String abilityVersion;
/**
* 底盘数据
* null表示未用到此数据
*/
public final ChassisStatesOuterClass.ChassisStates chassisStates;
public final Chassis.LightSwitch light;
public final float oldSteering;
/**
* 老SSM数据
* null表示未用到此数据
*/
public final SystemStatusInfo.StatusInfo statusInfo;
/**
* 新SSM数据
* null表示未用到此数据
*/
public final SsmInfo.SsmStatusInf ssmInfo;
public final FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond;//FSM数据
public final long createTime;
public LaunchConditionData(String abilityVersion,
SystemStatusInfo.StatusInfo statusInfo,
SsmInfo.SsmStatusInf ssmInfo,
FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) {
this(abilityVersion, null, Float.MAX_VALUE, null, statusInfo, ssmInfo, fsmStatusReasonRespond);
}
public LaunchConditionData(String abilityVersion,
ChassisStatesOuterClass.ChassisStates chassisStates,
float oldSteering,
Chassis.LightSwitch light,
SystemStatusInfo.StatusInfo statusInfo,
SsmInfo.SsmStatusInf ssmInfo,
FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) {
this.abilityVersion = abilityVersion;
this.chassisStates = chassisStates;
this.oldSteering = oldSteering;
this.light = light;
this.statusInfo = statusInfo;
this.ssmInfo = ssmInfo;
this.fsmStatusReasonRespond = fsmStatusReasonRespond;
createTime = System.currentTimeMillis();
}
public String getJson() {
JSONObject jsonObject = new JSONObject();
try {
jsonObject.put("ability_version", abilityVersion);
jsonObject.put("create_object_time", createTime);
JSONArray array = new JSONArray();
JSONObject chassisStatesObject = new JSONObject();
chassisStatesObject.put("name", "ChassisStates");
if (chassisStates != null) {
if (chassisStates.hasHeader()) {
chassisStatesObject.put("data_header", TextFormat.printer().escapingNonAscii(false).printToString(chassisStates.getHeader()));
}
//制动踏板
String temp = "未知";
if (chassisStates.hasBrakeSystemStates()) {
float brake = chassisStates.getBrakeSystemStates().getBrakePedalResponsePosition();
temp = String.valueOf(brake);
}
chassisStatesObject.put("brake", temp);
//油门踏板
temp = "未知";
if (chassisStates.hasDrivingSystemStates()) {
float throttle = chassisStates.getDrivingSystemStates().getThrottleResponsePosition();
temp = String.valueOf(throttle);
}
chassisStatesObject.put("throttle", temp);
//档位
temp = "未知";
if (chassisStates.hasGearSystemStates()) {
Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition();
temp = gear.name();
}
chassisStatesObject.put("gear", temp);
//方向盘
temp = "未知";
if (chassisStates.hasSteerSystemStates()) {
ChassisStatesOuterClass.SteerSystemStates steerSystemStates = chassisStates.getSteerSystemStates();
if (steerSystemStates.hasSteeringWheelAngle()) {
float steering = steerSystemStates.getSteeringWheelAngle();
temp = String.valueOf(steering);
}
}
chassisStatesObject.put("old_steering", oldSteering == Float.MAX_VALUE ? "未知" : oldSteering);
chassisStatesObject.put("steering", temp);
}
//车灯
if (light != null) {
chassisStatesObject.put("light", light.name());
}
array.put(chassisStatesObject);
if (statusInfo != null) {
arrayPut(array, statusInfo.getClass().getName(), statusInfo.getAutoPilotReady(), statusInfo.getAutoPilotUnreadyReason());
}
if (ssmInfo != null) {
arrayPut(array, ssmInfo.getClass().getName(), ssmInfo.getAutoPilotReady(), ssmInfo.getAutoPilotUnreadyReason());
}
if (fsmStatusReasonRespond != null) {
arrayPut(array, fsmStatusReasonRespond);
}
if (array.length() > 0) {
jsonObject.put("data", array);
}
} catch (Exception e) {
e.printStackTrace();
}
return jsonObject.toString();
}
private void arrayPut(JSONArray array, String name, boolean isReady, String reason) throws JSONException {
JSONObject objectParse = new JSONObject();
objectParse.put("auto_pilot_ready", isReady);
if (reason != null) {
objectParse.put("auto_pilot_unready_reason", reason);
}
JSONObject object = new JSONObject();
object.put("name", name);
object.put("parse", objectParse);
array.put(object);
}
private void arrayPut(JSONArray array, GeneratedMessageV3 message) throws JSONException, InvalidProtocolBufferException {
JSONObject object = new JSONObject();
object.put("name", message.getClass().getName());
// object.put("original", ByteUtil.byteArrToHex(message.toByteArray(), false));
object.put("parse", new JSONObject(JsonFormat.printer().print(message)));
array.put(object);
}
}

104
libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/UnableLaunchData.java
View File

@@ -1,104 +0,0 @@
package com.zhjt.mogo.adas.data.bean;
import com.google.protobuf.GeneratedMessageV3;
import com.google.protobuf.InvalidProtocolBufferException;
import com.google.protobuf.util.JsonFormat;
import org.json.JSONArray;
import org.json.JSONException;
import org.json.JSONObject;
import function_state_management.FSMStatusReasonQueryOuterClass;
import system_master.SsmInfo;
import system_master.SystemStatusInfo;
/**
* 检测到不能启动自动驾驶的数据
* 此类目前主要用于启动自驾命令下发后的域控数据记录
*/
public class UnableLaunchData {
/**
* 所使用的检测版本
*/
public final String abilityVersion;
/**
* 老底盘数据
* null表示未用到此数据
* TODO 不需要底盘数据，如果底盘数据存在不能启动自驾的原因会限制启动自驾命令的下发
*/
// public final VehicleStateOuterClass.VehicleState vehicleState;
/**
* 新底盘数据
* null表示未用到此数据
* TODO 不需要底盘数据，如果底盘数据存在不能启动自驾的原因会限制启动自驾命令的下发
*/
// public final ChassisStatesOuterClass.ChassisStates chassisStates;
/**
* 老SSM数据
* null表示未用到此数据
*/
public final SystemStatusInfo.StatusInfo statusInfo;
/**
* 新SSM数据
* null表示未用到此数据
*/
public final SsmInfo.SsmStatusInf ssmInfo;
public final FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond;//FSM数据
public UnableLaunchData(String abilityVersion,
SystemStatusInfo.StatusInfo statusInfo,
SsmInfo.SsmStatusInf ssmInfo,
FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) {
this.abilityVersion = abilityVersion;
this.statusInfo = statusInfo;
this.ssmInfo = ssmInfo;
this.fsmStatusReasonRespond = fsmStatusReasonRespond;
}
public String getJson() {
JSONObject jsonObject = new JSONObject();
try {
jsonObject.put("ability_version", abilityVersion);
JSONArray array = new JSONArray();
if (statusInfo != null) {
arrayPut(array, statusInfo.getClass().getName(), statusInfo.getAutoPilotReady(), statusInfo.getAutoPilotUnreadyReason());
}
if (ssmInfo != null) {
arrayPut(array, ssmInfo.getClass().getName(), ssmInfo.getAutoPilotReady(), ssmInfo.getAutoPilotUnreadyReason());
}
if (fsmStatusReasonRespond != null) {
arrayPut(array, fsmStatusReasonRespond);
}
if (array.length() > 0) {
jsonObject.put("data", array);
}
} catch (JSONException | InvalidProtocolBufferException e) {
e.printStackTrace();
}
return jsonObject.toString();
}
private void arrayPut(JSONArray array, String name, boolean isReady, String reason) throws JSONException {
JSONObject objectParse = new JSONObject();
objectParse.put("auto_pilot_ready", isReady);
if (reason != null) {
objectParse.put("auto_pilot_unready_reason", reason);
}
JSONObject object = new JSONObject();
object.put("name", name);
object.put("parse", objectParse);
array.put(object);
}
private void arrayPut(JSONArray array, GeneratedMessageV3 message) throws JSONException, InvalidProtocolBufferException {
JSONObject object = new JSONObject();
object.put("name", message.getClass().getName());
// object.put("original", ByteUtil.byteArrToHex(message.toByteArray(), false));
object.put("parse", new JSONObject(JsonFormat.printer().print(message)));
array.put(object);
}
}

4
libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/UnableLaunchReason.java
View File

@@ -58,12 +58,12 @@ public class UnableLaunchReason {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
UnableLaunchReason that = (UnableLaunchReason) o;
return source == that.source && TextUtils.equals(unableLaunchReason, that.unableLaunchReason);
return source == that.source && unableType == that.unableType && TextUtils.equals(unableLaunchReason, that.unableLaunchReason);
}
@Override
public int hashCode() {
return Objects.hash(source, unableLaunchReason);
return Objects.hash(source, unableType, unableLaunchReason);
}
@Override

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

@@ -12,7 +12,7 @@ import com.zhjt.mogo.adas.data.AiCloudTask;
import com.zhjt.mogo.adas.data.bean.AdasParam;
import com.zhjt.mogo.adas.data.bean.AutopilotStatistics;
import com.zhjt.mogo.adas.data.bean.ReceivedAck;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import com.zhjt.mogo.adas.data.sweeper.bootable.SweeperBootable;
import com.zhjt.mogo.adas.data.sweeper.task.SweeperTask;
@@ -492,10 +492,10 @@ public interface OnAdasListener {
* 是否有能力启动自动驾驶
*
* @param isAutopilotAbility 是否能启动自动驾驶
* @param unableLaunchData 原始数据
* @param launchConditionData 原始数据
* @param unableAutopilotReasons 不能启动自动驾驶原因
*/
void onAutopilotAbility(boolean isAutopilotAbility, @NonNull UnableLaunchData unableLaunchData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons);
void onAutopilotAbility(boolean isAutopilotAbility, @NonNull LaunchConditionData launchConditionData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons);
/**

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

@@ -2,7 +2,7 @@ package com.zhidao.support.adas.high.common.autopilot.ability;
import androidx.annotation.NonNull;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
@@ -30,7 +30,7 @@ public class AutopilotAbility230 extends BaseAutopilotAbilityChassis {
unableAutopilotReasons = onCallbackChassis(unableAutopilotReasons);
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), null, null, null), unableAutopilotReasons);
listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), chassisStates, oldSteering, getLightSwitch(), null, null, null), unableAutopilotReasons);
}
}

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

@@ -4,7 +4,7 @@ import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
@@ -107,7 +107,7 @@ public class AutopilotAbility250 extends BaseAutopilotAbilityChassis {
unableAutopilotReasons = onCallbackChassis(unableAutopilotReasons);
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), statusInfo, null, null), unableAutopilotReasons);
listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), chassisStates, oldSteering, getLightSwitch(), statusInfo, null, null), unableAutopilotReasons);
}
}

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

@@ -6,7 +6,7 @@ import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
@@ -138,7 +138,7 @@ public class AutopilotAbility330 {
}
}
if (listener != null) {
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), statusInfo, null, fsmStatusReasonRespond), unableAutopilotReasons);
listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), statusInfo, null, fsmStatusReasonRespond), unableAutopilotReasons);
}
fsmStatusReasonRespond = null;
}

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

@@ -4,7 +4,7 @@ import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
@@ -88,7 +88,7 @@ public class AutopilotAbility350And360 extends BaseAutopilotAbilityChassis {
unableAutopilotReasons = onCallbackChassis(unableAutopilotReasons);
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), null, statusInfo, null), unableAutopilotReasons);
listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), chassisStates, oldSteering, getLightSwitch(), null, statusInfo, null), unableAutopilotReasons);
}
}

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

@@ -6,7 +6,7 @@ import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
@@ -117,7 +117,7 @@ public class AutopilotAbility360 {
}
}
if (listener != null) {
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), null, statusInfo, fsmStatusReasonRespond), unableAutopilotReasons);
listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), null, statusInfo, fsmStatusReasonRespond), unableAutopilotReasons);
}
fsmStatusReasonRespond = null;
}

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

@@ -7,7 +7,7 @@ import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.OnAdasListener;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.AdasConstants;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
@@ -131,9 +131,9 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
}
@Override
public void onAutopilotAbility(boolean isAutopilotAbility, @NonNull UnableLaunchData unableLaunchData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons) {
public void onAutopilotAbility(boolean isAutopilotAbility, @NonNull LaunchConditionData launchConditionData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons) {
if (listener != null) {
listener.onAutopilotAbility(isAutopilotAbility, unableLaunchData, unableAutopilotReasons);
listener.onAutopilotAbility(isAutopilotAbility, launchConditionData, unableAutopilotReasons);
}
}

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

@@ -24,7 +24,7 @@ public abstract class BaseAutopilotAbilityChassis {
private static final String REASON_CHASSIS_STEERING = "方向盘";
@NonNull
protected final AutopilotAbilityManager manager;
private ChassisStatesOuterClass.ChassisStates chassisStates;
protected ChassisStatesOuterClass.ChassisStates chassisStates;
protected OnAutopilotAbilityListener listener;
protected int mapVersion = -1;//工控机版本
protected boolean isJinlv = false;//是否是金旅小巴
@@ -39,7 +39,7 @@ public abstract class BaseAutopilotAbilityChassis {
*/
private Set<Chassis.GearPosition> launchAutopilotGear;
private float oldThrottle = Float.MAX_VALUE;//方向盘转角
protected float oldSteering = Float.MAX_VALUE;//方向盘转角
public BaseAutopilotAbilityChassis(@NonNull AutopilotAbilityManager manager) {
this.manager = manager;
@@ -60,6 +60,9 @@ public abstract class BaseAutopilotAbilityChassis {
currentLight.set(light);
}
protected Chassis.LightSwitch getLightSwitch() {
return currentLight.get();
}
public void setLaunchAutopilotGear(Set<Chassis.GearPosition> launchAutopilotGear) {
this.launchAutopilotGear = launchAutopilotGear;
@@ -138,13 +141,13 @@ public abstract class BaseAutopilotAbilityChassis {
if (chassisStates.hasSteerSystemStates()) {
ChassisStatesOuterClass.SteerSystemStates steerSystemStates = chassisStates.getSteerSystemStates();
if (steerSystemStates.hasSteeringWheelAngle()) {
float throttle = steerSystemStates.getSteeringWheelAngle();
if (oldThrottle == Float.MAX_VALUE) {
oldThrottle = throttle;
float steering = steerSystemStates.getSteeringWheelAngle();
if (oldSteering == Float.MAX_VALUE) {
oldSteering = steering;
} else {
// CupidLogUtils.log(TAG, "方向盘当前角度=" + oldThrottle + " 方向盘1秒前角度=" + throttle);
boolean isTurning = Math.abs(throttle - oldThrottle) > 8.0F;
oldThrottle = throttle;
// CupidLogUtils.log(TAG, "方向盘当前角度=" + oldSteering + " 方向盘1秒前角度=" + steering);
boolean isTurning = Math.abs(steering - oldSteering) > 8.0F;
oldSteering = steering;
// CupidLogUtils.log(TAG, "方向盘是否转动=" + isTurning);
if (isTurning) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_STEERING, REASON_CHASSIS_STEERING);

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

@@ -3,11 +3,11 @@ package com.zhidao.support.adas.high.common.autopilot.ability;
import androidx.annotation.NonNull;
import androidx.annotation.Nullable;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
interface OnAutopilotAbilityListener {
void onAutopilotAbility(boolean isAutopilotAbility, @NonNull UnableLaunchData unableLaunchData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons);
void onAutopilotAbility(boolean isAutopilotAbility, @NonNull LaunchConditionData launchConditionData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons);
}