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

[pb12][清扫车新老底盘兼容] 清扫车分支 老地盘数据转新地盘数据

Browse Source
This commit is contained in:
xinfengkun
2023-01-09 16:58:46 +08:00
parent d48c59f1bf
commit 23b96286ee
25 changed files with 625 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,6 +4,7 @@ import com.zhidao.support.adas.high.bean.AutopilotAbility;
import com.zhidao.support.adas.high.bean.AutopilotStatistics;
import com.zhidao.support.adas.high.common.ProtocolStatus;
import chassis.ChassisStatesOuterClass;
import chassis.VehicleStateOuterClass;
import mogo.telematics.pad.MessagePad;
import mogo_msg.MogoReportMsg;
@@ -51,13 +52,24 @@ public interface OnAdasListener {
void onGnssInfo(MessagePad.Header header, MessagePad.GnssInfo gnssInfo);
/**
* 底盘信息, 透传底盘状态，pb参考底盘
* 老底盘信息,透传底盘状态，pb参考底盘
* TODO 老地盘数据会转发到新地盘
* TODO 鹰眼不要用此接口展示页面相关，可以用来记录链路日志
* TODO 后期可能会优化掉
*
* @param header 头
* @param vehicleState 数据
*/
void onVehicleState(MessagePad.Header header, VehicleStateOuterClass.VehicleState vehicleState);
/**
* 底盘状态
*
* @param header 头
* @param chassisStates 头
*/
void onChassisStates(MessagePad.Header header, ChassisStatesOuterClass.ChassisStates chassisStates);
/**
* 自动驾驶状态
*

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

@@ -16,13 +16,14 @@ public enum MessageType {
TYPE_RECEIVE_TRAJECTORY(MessagePad.MessageType.MsgTypeTrajectory, "车前引导线"),
TYPE_RECEIVE_TRACKED_OBJECTS(MessagePad.MessageType.MsgTypeTrackedObjects, "障碍物信息"),
TYPE_RECEIVE_GNSS_INFO(MessagePad.MessageType.MsgTypeGnssInfo, "惯导信息"),
TYPE_RECEIVE_VEHICLE_STATE(MessagePad.MessageType.MsgTypeVehicleState, "底盘信息"),
TYPE_RECEIVE_VEHICLE_STATE(MessagePad.MessageType.MsgTypeVehicleState, "老底盘状态"),
TYPE_RECEIVE_AUTOPILOT_STATE(MessagePad.MessageType.MsgTypeAutopilotState, "自动驾驶状态"),
TYPE_RECEIVE_REPORT_MESSAGE(MessagePad.MessageType.MsgTypeReportMessage, "监控事件报告"),
TYPE_RECEIVE_PERCEPTION_TRAFFIC_LIGHT(MessagePad.MessageType.MsgTypePerceptionTrafficLight, "感知红绿灯"),
TYPE_RECEIVE_PREDICTION_OBSTACLE_TRAJECTORY(MessagePad.MessageType.MsgTypePredictionObstacleTrajectory, "他车轨迹预测"),
TYPE_RECEIVE_POINT_CLOUD(MessagePad.MessageType.MsgTypePointCloud, "点云透传"),
TYPE_RECEIVE_PLANNING_OBJECTS(MessagePad.MessageType.MsgTypePlanningObjects, "Planning障碍物"),
TYPE_RECEIVE_CHASSIS_STATES(MessagePad.MessageType.MsgTypeChassisStates, "底盘状态"),
TYPE_RECEIVE_BASIC_INFO_REQ(MessagePad.MessageType.MsgTypeBasicInfoReq, "自动驾驶设备基础信息请求"),
TYPE_SEND_BASIC_INFO_RESP(MessagePad.MessageType.MsgTypeBasicInfoResp, "自动驾驶设备基础信息应答"),
@@ -52,7 +53,7 @@ public enum MessageType {
TYPE_SEND_SET_PARAM_REQ(MessagePad.MessageType.MsgTypeSetParamReq, "设置参数命令"),
TYPE_SEND_TRIP_INFO_REQ(MessagePad.MessageType.MsgTypeTripInfoEvent, "行程信息"),
TYPE_SEND_SPECIAL_VEHICLE_TASK_CMD(MessagePad.MessageType.MsgTypeSpecialVehicleTaskCmd, "特种车辆命令"),
//透传 原始pb文件中不存在以下type。由于Java中无法强转所以在mogo-adas-data message_pad.proto中放开注释
//TODO 透传原始pb文件中不存在以下type。由于Java中无法强转,所以在mogo-adas-data/message_pad.proto中放开注释
TYPE_RECEIVE_PLANNING_DECISION_STATE(MessagePad.MessageType.MsgTypePlanningDecisionState, "Planning决策状态");

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

@@ -0,0 +1,22 @@
package com.zhidao.support.adas.high.msg;
import com.google.protobuf.InvalidProtocolBufferException;
import com.zhidao.support.adas.high.OnAdasListener;
import com.zhidao.support.adas.high.protocol.RawData;
import chassis.ChassisStatesOuterClass;
/**
* 重构后的底盘状态
*/
public class ChassisStatesMessage extends MyAbstractMessageHandler {
@Override
public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException {
ChassisStatesOuterClass.ChassisStates chassisStates = ChassisStatesOuterClass.ChassisStates.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue());
if (adasListener != null) {
adasListener.onChassisStates(raw.getHeader(), chassisStates);
}
}
}

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

@@ -15,7 +15,8 @@ public class MyMessageFactory implements IMyMessageFactory {
private IMsg trajectoryMessage;//局部轨迹消息
private IMsg trackedObjectsMessage;//障碍物信息
private IMsg gnssInfoMessage;//惯导信息
private IMsg vehicleStateMessage;//底盘信息, 透传底盘状态，pb参考底盘
private IMsg vehicleStateMessage;//老底盘信息, 透传底盘状态，pb参考底盘
private IMsg chassisStatesMessage;//重构后的底盘状态
private IMsg autopilotStateMessage;//自动驾驶状态
private IMsg reportMessage;//监控事件报告
private IMsg perceptionTrafficLightMessage;//感知红绿灯
@@ -64,6 +65,12 @@ public class MyMessageFactory implements IMyMessageFactory {
vehicleStateMessage = new VehicleStateMessage();
}
return vehicleStateMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_CHASSIS_STATES.typeCode) {
//重构后的底盘状态
if (chassisStatesMessage == null) {
chassisStatesMessage = new ChassisStatesMessage();
}
return chassisStatesMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_AUTOPILOT_STATE.typeCode) {
//自动驾驶状态
if (autopilotStateMessage == null) {

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

@@ -9,7 +9,10 @@ import com.zhidao.support.adas.high.common.AutopilotAbilityManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhidao.support.adas.high.protocol.RawData;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import chassis.VehicleStateOuterClass;
import common.HeaderOuterClass;
/**
* 底盘信息, 透传底盘状态，pb参考底盘
@@ -25,9 +28,312 @@ public class VehicleStateMessage extends MyAbstractMessageHandler {
if (CupidLogUtils.isEnableLog())
nowTime = SystemClock.elapsedRealtime();
if (adasListener != null) {
compatibility(adasListener, raw, vehicleState);
adasListener.onVehicleState(raw.getHeader(), vehicleState);
}
AdasChannel.calculateTimeConsumingBusiness("底盘信息", nowTime);
// CupidLogUtils.e("底盘信息--->" + vehicleState.toString());
}
/**
* 将老地盘数据转换成新地盘数据
* 字段注释中 鹰眼已用 的 已和底盘相关开发 确认过，其他字段均为确认对应关系
* 新底盘中未找到字段：
* parking_brake longitude_driving_mode eps_steering_mode
* steering_sign location_missing trajectory_missing
* chassis_status_missing pilot_mode_condition_met vehicletakeoverstatus
*/
private void compatibility(OnAdasListener adasListener, RawData raw, VehicleStateOuterClass.VehicleState vehicleState) throws InvalidProtocolBufferException {
/**************老地盘转换成新地盘PB***************/
ChassisStatesOuterClass.ChassisStates.Builder builder = ChassisStatesOuterClass.ChassisStates.newBuilder();
//工控机所用Header
if (vehicleState.hasHeader()) {
HeaderOuterClass.Header outHeader = vehicleState.getHeader();
if (outHeader != null) {
builder.setHeader(HeaderOuterClass.Header.parseFrom(vehicleState.getHeader().toByteArray()));
}
}
setBCMSystemStates(builder, vehicleState);
setSteerSystemStates(builder, vehicleState);
setGearSystemStates(builder, vehicleState);
setVehicleMotionStates(builder, vehicleState);
setDrivingSystemStates(builder, vehicleState);
setBrakeSystemStates(builder, vehicleState);
setChassisAutopilotAssistanceInformation(builder, vehicleState);
//电子驻车制动系统
if (vehicleState.hasEpb()) {
Chassis.EPBWorkStatus old = vehicleState.getEpb();
if (old != null) {
ChassisStatesOuterClass.EPBWorkStates epb = null;
if (old == Chassis.EPBWorkStatus.EPBSTATUS_NONE) {
epb = ChassisStatesOuterClass.EPBWorkStates.EPB_STATE_NONE;
} else if (old == Chassis.EPBWorkStatus.EPBSTATUS_LOCKED) {
epb = ChassisStatesOuterClass.EPBWorkStates.EPB_STATE_LOCKED;
} else if (old == Chassis.EPBWorkStatus.EPBSTATUS_RELEASED) {
epb = ChassisStatesOuterClass.EPBWorkStates.EPB_STATE_RELEASED;
} else if (old == Chassis.EPBWorkStatus.EPBSTATUS_LOCKED_FAIL) {
epb = ChassisStatesOuterClass.EPBWorkStates.EPB_STATE_LOCKED_FAIL;
} else if (old == Chassis.EPBWorkStatus.EPBSTATUS_RELEASE_FAIL) {
epb = ChassisStatesOuterClass.EPBWorkStates.EPB_STATE_RELEASE_FAIL;
} else if (old == Chassis.EPBWorkStatus.EPBSTATUS_FAULT) {
epb = ChassisStatesOuterClass.EPBWorkStates.EPB_STATE_FAULT;
}
if (epb != null) {
builder.setEpbSystemStates(ChassisStatesOuterClass.EPBSystemStates.newBuilder()
.setEpbWorkState(epb));
}
}
}
// 整车故障状态
if (vehicleState.hasVehiclefaultlevel()) {
Chassis.VehicleFaultLevel vehicleFaultLevel = vehicleState.getVehiclefaultlevel();
if (vehicleFaultLevel != null) {
ChassisStatesOuterClass.VehicleFaultLevels vehicleFaultLevels = null;
if (vehicleFaultLevel == Chassis.VehicleFaultLevel.None_Fault) {
vehicleFaultLevels = ChassisStatesOuterClass.VehicleFaultLevels.NONE_FAULT;
} else if (vehicleFaultLevel == Chassis.VehicleFaultLevel.General_Fault) {
vehicleFaultLevels = ChassisStatesOuterClass.VehicleFaultLevels.GENERAL_FAULT;
} else if (vehicleFaultLevel == Chassis.VehicleFaultLevel.Serious_Fault) {
vehicleFaultLevels = ChassisStatesOuterClass.VehicleFaultLevels.SERIOUS_FAULT;
} else if (vehicleFaultLevel == Chassis.VehicleFaultLevel.Critical_Fault) {
vehicleFaultLevels = ChassisStatesOuterClass.VehicleFaultLevels.CRITICAL_FAULT;
}
if (vehicleFaultLevels != null) {
builder.setVehicleChassisFaultInformation(ChassisStatesOuterClass.VehicleChassisFaultInformation.newBuilder()
.setVehiclefaultlevel(vehicleFaultLevels));
}
}
}
//电量
if (vehicleState.hasBmsSoc()) {
builder.setBmsSystemStates(ChassisStatesOuterClass.BMSSystemStates.newBuilder().setBmsSoc(vehicleState.getBmsSoc()));
}
//清扫车（福田）清扫控制系统状态
if (vehicleState.hasSweeperFutianCleanSystemState()) {
VehicleStateOuterClass.SweeperFuTianCleanSystemState sweeperFuTianCleanSystemState = vehicleState.getSweeperFutianCleanSystemState();
if (sweeperFuTianCleanSystemState != null) {
byte[] bytes = sweeperFuTianCleanSystemState.toByteArray();
builder.setTaskSystemStates(ChassisStatesOuterClass.TaskSystemStates.newBuilder()
.setSweeperFutianTaskSystemStates(ChassisStatesOuterClass.SweeperFuTianTaskSystemStates.parseFrom(bytes)));//鹰眼已用 清扫车专用
}
}
adasListener.onChassisStates(raw.getHeader(), builder.build());
}
private void setChassisAutopilotAssistanceInformation(ChassisStatesOuterClass.ChassisStates.Builder builder, VehicleStateOuterClass.VehicleState vehicleState) {
ChassisStatesOuterClass.ChassisAutopilotAssistanceInformation.Builder chassisAutopilotAssistanceInformation = null;
if (vehicleState.hasPilotMode()) {
Chassis.PilotMode pilotMode = vehicleState.getPilotMode();
if (pilotMode != null) {
chassisAutopilotAssistanceInformation = ChassisStatesOuterClass.ChassisAutopilotAssistanceInformation.newBuilder();
chassisAutopilotAssistanceInformation.setChassisPilotModeState(pilotMode);
}
}
if (vehicleState.hasEmergencyStopSwitch()) {
if (chassisAutopilotAssistanceInformation == null) {
chassisAutopilotAssistanceInformation = ChassisStatesOuterClass.ChassisAutopilotAssistanceInformation.newBuilder();
}
// 急停开关状态
chassisAutopilotAssistanceInformation.setEmergencyStopSwitchState(vehicleState.getEmergencyStopSwitch());
}
if (chassisAutopilotAssistanceInformation != null) {
builder.setChassisAutopilotAssistanceInformation(chassisAutopilotAssistanceInformation);
}
}
/**
* 档位
*/
private void setGearSystemStates(ChassisStatesOuterClass.ChassisStates.Builder builder, VehicleStateOuterClass.VehicleState vehicleState) {
ChassisStatesOuterClass.GearSystemStates.Builder gearSystemStates = null;
if (vehicleState.hasGear()) {
Chassis.GearPosition gear = vehicleState.getGear();
if (gear != null) {
gearSystemStates = ChassisStatesOuterClass.GearSystemStates.newBuilder();
//挂挡档位数据
gearSystemStates.setGearPosition(gear);//鹰眼已用
}
}
if (vehicleState.hasGearSwitchInference()) {
if (gearSystemStates == null) {
gearSystemStates = ChassisStatesOuterClass.GearSystemStates.newBuilder();
}
//档位干预
gearSystemStates.setGearSwitchInferenceState(vehicleState.getGearSwitchInference());
}
if (gearSystemStates != null) {
builder.setGearSystemStates(gearSystemStates);
}
}
/**
* 油门
*/
private void setDrivingSystemStates(ChassisStatesOuterClass.ChassisStates.Builder builder, VehicleStateOuterClass.VehicleState vehicleState) {
ChassisStatesOuterClass.DrivingSystemStates.Builder drivingSystemStates = null;
if (vehicleState.hasThrottle()) {
drivingSystemStates = ChassisStatesOuterClass.DrivingSystemStates.newBuilder();
//油门
drivingSystemStates.setThrottleResponsePosition(vehicleState.getThrottle());//鹰眼已用
}
if (vehicleState.hasAccelInference()) {
if (drivingSystemStates == null) {
drivingSystemStates = ChassisStatesOuterClass.DrivingSystemStates.newBuilder();
}
//加速踏板干预
drivingSystemStates.setAccelerationPedalInferenceState(vehicleState.getAccelInference());
}
if (drivingSystemStates != null) {
builder.setDrivingSystemStates(drivingSystemStates);
}
}
/**
* 刹车
*/
private void setBrakeSystemStates(ChassisStatesOuterClass.ChassisStates.Builder builder, VehicleStateOuterClass.VehicleState vehicleState) {
ChassisStatesOuterClass.BrakeSystemStates.Builder brakeSystemStates = null;
if (vehicleState.hasBrake()) {
brakeSystemStates = ChassisStatesOuterClass.BrakeSystemStates.newBuilder();
//刹车
brakeSystemStates.setBrakePedalResponsePosition(vehicleState.getBrake());//鹰眼已用
}
if (vehicleState.hasBrakeInference()) {
if (brakeSystemStates == null) {
brakeSystemStates = ChassisStatesOuterClass.BrakeSystemStates.newBuilder();
}
//刹车干预
brakeSystemStates.setBrakePedalInferenceState(vehicleState.getBrakeInference());
}
if (brakeSystemStates != null) {
builder.setBrakeSystemStates(brakeSystemStates);
}
}
/**
* 方向盘
*/
private void setSteerSystemStates(ChassisStatesOuterClass.ChassisStates.Builder builder, VehicleStateOuterClass.VehicleState vehicleState) {
ChassisStatesOuterClass.SteerSystemStates.Builder steerSystemStates = null;
if (vehicleState.hasSteering()) {
steerSystemStates = ChassisStatesOuterClass.SteerSystemStates.newBuilder();
//方向盘转向角数据
steerSystemStates.setSteeringWheelAngle(vehicleState.getSteering());//鹰眼已用
}
if (vehicleState.hasSteerInference()) {
if (steerSystemStates == null) {
steerSystemStates = ChassisStatesOuterClass.SteerSystemStates.newBuilder();
}
//方向盘干预
steerSystemStates.setSteeringWheelInferenceState(vehicleState.getSteerInference());
}
if (vehicleState.hasSteeringSpd()) {
if (steerSystemStates == null) {
steerSystemStates = ChassisStatesOuterClass.SteerSystemStates.newBuilder();
}
steerSystemStates.setSteeringWheelSpeed(vehicleState.getSteeringSpd());
}
if (vehicleState.hasLeftFrontWheelAngle()) {
if (steerSystemStates == null) {
steerSystemStates = ChassisStatesOuterClass.SteerSystemStates.newBuilder();
}
steerSystemStates.setFrontLeftWheelAngle(vehicleState.getLeftFrontWheelAngle());
}
if (vehicleState.hasRightFrontWheelAngle()) {
if (steerSystemStates == null) {
steerSystemStates = ChassisStatesOuterClass.SteerSystemStates.newBuilder();
}
steerSystemStates.setFrontRightWheelAngle(vehicleState.getRightFrontWheelAngle());
}
if (steerSystemStates != null) {
builder.setSteerSystemStates(steerSystemStates);
}
}
private void setVehicleMotionStates(ChassisStatesOuterClass.ChassisStates.Builder builder, VehicleStateOuterClass.VehicleState vehicleState) {
ChassisStatesOuterClass.VehicleMotionStates.Builder vehicleMotionStates = null;
if (vehicleState.hasSpeed()) {
vehicleMotionStates = ChassisStatesOuterClass.VehicleMotionStates.newBuilder();
vehicleMotionStates.setSpeed(vehicleState.getSpeed());
}
if (vehicleState.hasAccel()) {
if (vehicleMotionStates == null) {
vehicleMotionStates = ChassisStatesOuterClass.VehicleMotionStates.newBuilder();
}
//加速度
vehicleMotionStates.setAcceleration(vehicleState.getAccel());//鹰眼已用
}
if (vehicleMotionStates != null) {
builder.setVehicleMotionStates(vehicleMotionStates);
}
}
/**
* 车身控制模块
*/
private void setBCMSystemStates(ChassisStatesOuterClass.ChassisStates.Builder builder, VehicleStateOuterClass.VehicleState vehicleState) {
ChassisStatesOuterClass.BCMSystemStates.Builder bcmSystemStates = null;
if (vehicleState.hasLight()) {
Chassis.LightSwitch light = vehicleState.getLight();
if (light != null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
//转向灯数据
bcmSystemStates.setTurnLightState(light);//鹰眼已用
}
}
if (vehicleState.hasBrakeLightStatus()) {
if (bcmSystemStates == null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
}
//刹车灯数据
bcmSystemStates.setBrakeLightState(vehicleState.getBrakeLightStatus() ? 1 : 0);//鹰眼已用
}
if (vehicleState.hasHorn()) {
if (bcmSystemStates == null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
}
//鸣笛
bcmSystemStates.setHornState(vehicleState.getHorn() ? 1 : 0);
}
if (vehicleState.hasHighbeam()) {
if (bcmSystemStates == null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
}
//远光灯
bcmSystemStates.setHighbeamState(vehicleState.getHighbeam() ? 1 : 0);
}
if (vehicleState.hasLowbeam()) {
if (bcmSystemStates == null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
}
//近光灯
bcmSystemStates.setLowbeamState(vehicleState.getLowbeam() ? 1 : 0);
}
if (vehicleState.hasFoglight()) {
if (bcmSystemStates == null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
}
//雾灯 新底盘分前后雾灯 老地盘只有一个 所以同时赋值
bcmSystemStates.setFrontFoglightState(vehicleState.getFoglight() ? 1 : 0);
bcmSystemStates.setBackFoglightState(vehicleState.getFoglight() ? 1 : 0);
}
if (vehicleState.hasClearanceLamps()) {
if (bcmSystemStates == null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
}
//示廓灯
bcmSystemStates.setClearanceLampsState(vehicleState.getClearanceLamps() ? 1 : 0);
}
if (vehicleState.hasWarnLight()) {
if (bcmSystemStates == null) {
bcmSystemStates = ChassisStatesOuterClass.BCMSystemStates.newBuilder();
}
//报警灯
bcmSystemStates.setAlarmLightState(vehicleState.getWarnLight() ? 1 : 0);
}
if (bcmSystemStates != null) {
builder.setBcmSystemStates(bcmSystemStates);
}
}
}