[8.0.0] ADAS LIB 障碍物轨迹预测2025（老版本完全废弃已移除）;增加域控障碍物类型转换成鹰眼使用的障碍物类型方法;

2025-04-24 11:56:53 +08:00
parent 128df8a27f
commit 2ccf1a18f2
11 changed files with 176 additions and 47 deletions
--- a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt
+++ b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt
@@ -56,6 +56,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotGetParamRespon
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager.invokeAutopilotIdentifyDataUpdate
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager.invokeAutopilotIdentifyPlanningObj
+import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager.invokePredictionObstacleTrajectory
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotPointCloudListenerManager
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotRecordListenerManager
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotRecordListenerManager.invokeAutopilotRecordConfig
@@ -136,7 +137,7 @@ import packet_mytest_upload.CopyBag
 import perception.FusionTrafficLightOuterClass
 import perception.LaneMarkOuterClass
 import planning.RoboSweeperTaskIndexOuterClass
-import prediction.Prediction
+import prediction2025.Prediction2025
 import record_cache.RecordPanelOuterClass
 import system_master.SsmInfo
 import system_master.SystemStatusInfo
@@ -435,12 +436,12 @@ class MoGoAdasListenerImpl : OnAdasListener {
        return type
    }

-    //他车轨迹预测
+    //障碍物轨迹预测
    override fun onPredictionObstacleTrajectory(
-        header: MessagePad.Header?,
-        predictionObjects: Prediction.mPredictionObjects?
+        header: MessagePad.Header,
+        predictionObjects: Prediction2025.mPredictionObjects
    ) {
-        //他车轨迹预测
+        invokePredictionObstacleTrajectory(predictionObjects)
    }

    //todo 新增稳定设备类型需要添加，目的避免在nuc设备上使用此类功能
--- a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotIdentifyListener.kt
+++ b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotIdentifyListener.kt
@@ -3,6 +3,7 @@ package com.mogo.eagle.core.function.api.autopilot
 import mogo.telematics.pad.MessagePad
 import mogo.v2x.*
 import perception.FusionTrafficLightOuterClass
+import prediction2025.Prediction2025

 /**
 * @author xiaoyuzhou
@@ -34,6 +35,12 @@ interface IMoGoAutopilotIdentifyListener {
     */
    fun onAutopilotIdentifyDataUpdate(trafficData: List<MessagePad.TrackedObject>?) {}

+    /**
+     * 障碍物轨迹预测
+     *
+     * @param predictionObjects 预测数据
+     */
+    fun onPredictionObstacleTrajectory(predictionObjects: Prediction2025.mPredictionObjects){}
    /**
     * planning识别感知预警物体
     */
--- a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotIdentifyListenerManager.kt
+++ b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotIdentifyListenerManager.kt
@@ -2,9 +2,11 @@ package com.mogo.eagle.core.function.call.autopilot

 import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotIdentifyListener
 import com.mogo.eagle.core.function.call.base.CallerBase
+import com.zhjt.mogo.adas.data.Adas
 import mogo.telematics.pad.MessagePad
 import mogo.v2x.*
 import perception.FusionTrafficLightOuterClass
+import prediction2025.Prediction2025

 /**
 * 域控制器感知数据
@@ -13,6 +15,33 @@ import perception.FusionTrafficLightOuterClass
 */
 object CallerAutopilotIdentifyListenerManager : CallerBase<IMoGoAutopilotIdentifyListener>() {

+    private val typeMap: Map<Adas.TrackedObjectType, Adas.TrackedObjectClassID> = mapOf(
+        Adas.TrackedObjectType.TYPE_PEDESTRIAN to Adas.TrackedObjectClassID.Person,
+        Adas.TrackedObjectType.TYPE_MOTOR to Adas.TrackedObjectClassID.MotorCycle,
+        Adas.TrackedObjectType.TYPE_BICYCLE to Adas.TrackedObjectClassID.Bicycle,
+        Adas.TrackedObjectType.TYPE_CAR to Adas.TrackedObjectClassID.Car,
+        Adas.TrackedObjectType.TYPE_TRUCK to Adas.TrackedObjectClassID.Truck,
+        Adas.TrackedObjectType.TYPE_BUS to Adas.TrackedObjectClassID.Bus,
+        Adas.TrackedObjectType.TYPE_WARNINGTRIANGLE to Adas.TrackedObjectClassID.WarningTriangle,
+        Adas.TrackedObjectType.TYPE_ROADWORK_OCCUPY_0501 to Adas.TrackedObjectClassID.RoadWork_occupy_0501,
+        Adas.TrackedObjectType.TYPE_ROADWORK_BREAK_0502 to Adas.TrackedObjectClassID.RoadWork_break_0502,
+        Adas.TrackedObjectType.TYPE_ROAD_CONGESTION to Adas.TrackedObjectClassID.ROAD_CONGESTION
+    )
+
+    /**
+     * 域控障碍物类型转换成鹰眼使用的类型
+     *
+     * @param classType 域控障碍物类型（目前只有Prediction2025.mPredictionObjects.objs_app.classtype 是域控的障碍物类型）
+     */
+    fun trackedObjectTypeTransform(classType: Int): Int {
+        var type = Adas.TrackedObjectType.forNumber(classType)
+        if (type == null) {
+            type = Adas.TrackedObjectType.TYPE_UNKNOWN
+        }
+        val classId = typeMap[type] ?: Adas.TrackedObjectClassID.Unknown
+        return classId.number
+    }
+
    /**
     * 识别交通元素数据发生更新 回调
     */
@@ -24,6 +53,14 @@ object CallerAutopilotIdentifyListenerManager : CallerBase<IMoGoAutopilotIdentif
        }
    }

+    @Synchronized
+    fun invokePredictionObstacleTrajectory(predictionObjects: Prediction2025.mPredictionObjects) {
+        M_LISTENERS.forEach {
+            val listener = it.value
+            listener.onPredictionObstacleTrajectory(predictionObjects)
+        }
+    }
+
    /**
     * planning识别感知预警物体
     */
--- a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java
+++ b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java
@@ -33,6 +33,7 @@ public enum MessageType {
    TYPE_RECEIVE_FSM2024_STATE(MessagePad.MessageType.MsgTypeFSM2024State, "FSM状态"),
    TYPE_RECEIVE_LOC_STATE(MessagePad.MessageType.MsgTypeLocState, "定位状态"),
    TYPE_RECEIVE_CLOUD_REGULATORY_WARNING(MessagePad.MessageType.MsgTypeCloudRegulatoryWarning, "云控监管预警"),
+    TYPE_RECEIVE_PREDICTION_OBSTACLE_TRAJECTORY2025(MessagePad.MessageType.MsgTypePredictionObjects2025, "障碍物轨迹预测"),

    TYPE_RECEIVE_BASIC_INFO_REQ(MessagePad.MessageType.MsgTypeBasicInfoReq, "自动驾驶设备基础信息请求"),
    TYPE_SEND_BASIC_INFO_RESP(MessagePad.MessageType.MsgTypeBasicInfoResp, "自动驾驶设备基础信息应答"),
--- a/libraries/mogo-adas-data/src/main/proto/message_pad.proto
+++ b/libraries/mogo-adas-data/src/main/proto/message_pad.proto
@@ -47,6 +47,7 @@ enum MessageType
    MsgTypeFSM2024State = 0x10011; //20240531 FSM状态 定频10hz （telematics做状态变化判断，无变化10hz上报，有变化立即上报） BUS MAP440开始支持，其他车型暂未支持
    MsgTypeLocState = 0x10012; //定位呈现状态透传 用于pad图标显示 1hz 所有车型MAP440开始支持
    MsgTypeCloudRegulatoryWarning = 0x10013; //云控监管预警信息上报
+    MsgTypePredictionObjects2025 = 0x10015; //自车他车轨迹预测2025 定频10hz

    //### 以下消息全部不定频 ###
    MsgTypeBasicInfoReq = 0x10100; //自动驾驶设备基础信息请求
--- a/libraries/mogo-adas-data/src/main/proto/personal/adas.proto
+++ b/libraries/mogo-adas-data/src/main/proto/personal/adas.proto
@@ -15,3 +15,74 @@ message LastMessage{
 message MessageIdInfo{
  map<uint64, LastMessage> lastMessages = 1;// key：data（日期） value：数据
 }
+
+//障碍物类型（域控内部使用）
+enum TrackedObjectType {
+  TYPE_UNKNOWN = 0;
+  TYPE_PEDESTRIAN = 3;
+  TYPE_BICYCLE = 4;
+  TYPE_MOTOR = 5;
+  TYPE_RIDER = 6;
+  TYPE_CAR = 7;
+  TYPE_TRUCK = 8;
+  TYPE_BUS = 9;
+  TYPE_TRAIN = 10;
+  TYPE_SIGN = 20;
+  TYPE_LIGHT = 30;
+  TYPE_RED = 31;
+  TYPE_GREEN = 32;
+  TYPE_YELLOW = 33;
+  TYPE_BLACK = 34;
+  TYPE_TRIANGLEROADBLOCK = 35;
+  TYPE_WARNINGTRIANGLE = 36;
+  TYPE_VEGETATION = 37;
+  TYPE_TRICYCLE = 38;
+  TYPE_BICYCLE_RIDER = 39;
+  TYPE_TRICYCLE_RIDER = 40;
+  TYPE_MOVETRFCLTSPOLE = 41;
+  TYPE_MULTISEMANTICLTS = 42;
+  TYPE_UNKNOWN_SMALL = 91;
+  TYPE_UNKNOWN_BIG = 92;
+  TYPE_UNKNOWN_STATIC = 93;
+  TYPE_UNKNOWN_DYNAMIC = 94;
+  TYPE_FENCE = 95;
+  TYPE_TRAFFIC_SIGN = 96;
+  TYPE_NOISE = 97;
+  TYPE_ROAD = 98;
+  TYPE_SIDEWALK = 99;
+  TYPE_CURB = 100;
+  TYPE_OTHER_FLAT = 101;
+  TYPE_BUILDING = 102;
+  TYPE_POLE = 103;
+  TYPE_TERRAIN = 104;
+  TYPE_OBSTACLE = 105;
+  TYPE_OTHER_NOTHINGS = 106;
+  TYPE_EGO = 107;
+  TYPE_IGNORE = 110;
+  TYPE_TAILBACK = 447;
+  TYPE_ROADWORK_OCCUPY_0501 = 501;
+  TYPE_ROADWORK_BREAK_0502 = 502;
+  TYPE_ROAD_CONGESTION = 707;
+}
+
+//障碍物类型（鹰眼使用，目前用于类型转换，鹰眼UI层使用int类型值）
+enum TrackedObjectClassID {
+  Background = 0;
+  Person = 1;
+  Bicycle = 2;
+  Car = 3;
+  MotorCycle = 4;
+  TrafficSign = 5;
+  Bus = 6;
+  CellPhone = 7;
+  Truck = 8;
+  Bottle = 9;
+  TrafficLight = 10;
+  Rider = 11;
+  TriangleRoadblock = 12;
+  WarningTriangle = 13;
+  Unknown = 100;
+  RoadWork_occupy_0501 = 501;
+  RoadWork_break_0502 = 502;
+  ROAD_CONGESTION = 707;
+};
--- a/libraries/mogo-adas-data/src/main/proto/prediction.proto
+++ b/libraries/mogo-adas-data/src/main/proto/prediction.proto
@@ -1,33 +0,0 @@
-syntax = "proto2";
-
-package prediction;
-
-// common
-import "header.proto";
-import "geometry.proto";
-
-
-// estimated obstacle intent
-
-message mPredictionObject {
-  optional int64    m_nid =1;    //target id
-  optional int32    m_nquality =2; //target tracking life quality
-  optional int32    classtype =3;  //target classtype
-  optional int32    m_preconfidence=4;  //target predciton confidence
-
-  repeated geometry.Point  prediction_trajectory = 5;  //target prediction trajectory :vector : meter
-  repeated geometry.Point  prediction_pose = 6;         //targe prediciton pose  vector       angle:°
-  optional geometry.Vector3  objsize = 7; //length width height  :meter                             
-
-}
-
-message mPredictionObjects {
-  optional common.Header header = 1;   
-  optional int32    m_nnum0 =2;   // all target number
-  optional int64    allcyclenum =3;//process cycle number
-  optional double  m_ftime=4;      //time stamp
-  optional double  fdeltat=5;      // deltatime prediciton time stamp default:0.1s
-  repeated mPredictionObject objs=6; //obj capcity 
-
-}
-
--- a/libraries/mogo-adas-data/src/main/proto/prediction2025.proto
+++ b/libraries/mogo-adas-data/src/main/proto/prediction2025.proto
@@ -0,0 +1,44 @@
+syntax = "proto2";
+
+package prediction2025;
+
+// common
+import "header.proto";
+import "geometry.proto";
+
+
+
+
+
+message mTrajectoryApp{
+  repeated geometry.Point  trajectory_points = 1;  //ego coordinate system 预测轨迹线 80个点 第一个点表示物体的位置，之后的79个点表示预测轨迹点，已自车为坐标系 只有x和y
+  optional float prediction_probability = 2;// only ego trajectory is decimal; other agents trajectory is integer 概率 自车的概率小于1；他车的概率都为1；
+}
+
+
+message mPredictionObjectApp{//ego coordinate system
+  optional int64    m_nid = 1;    //target id: 800000 + obs_id; ego_id: 800000  减去800000是障碍物的id（此id是域控未数据融合之前的id，与发给鹰眼（TrackedObjects中的uuid）的可能不一样）
+  optional int32    classtype = 2;  //target classtype 障碍物类型（与TrackedObject中的type类型不同，需要转换，转换完成后一样）
+  optional bool    is_ai_output = 3;  //1:ai_output 0:rb_out ; 输出类型：AI输出；规则输出；
+  repeated mTrajectoryApp  prediction_trajectory = 4; //ego and other agent trajectories
+}
+
+//目前仅支持Taxi
+
+//与障碍物频率相同 MsgTypeTrackedObjects = 0x10001; //障碍物信息 定频10hz
+//判断是否有数据：1.感知障碍物节点挂掉时mPredictionObjects.objs_app.size==0；预测节点挂掉时没有数据推送需要做超时判断
+
+//判断自车的方法（任意一种方式即可）：
+//1.mPredictionObjectApp.m_nid==800000表示自车
+//2.mPredictionObjectApp.prediction_trajectory.size==2时表示自车
+//3.mTrajectoryApp.prediction_probability自车的小数，他车的都是1
+
+message mPredictionObjects {
+  optional common.Header header = 1;//障碍物时间戳与障碍物数据（TrackedObjects）时间戳相同
+  optional int32   m_nnum0 = 2;   // all target number 非未知类型障碍物的个数
+  optional int64   allcyclenum = 3;//process cycle number 鹰眼无用
+  optional double  m_ftime = 4;      //time stamp 预测处理后的时间戳
+  optional double  fdeltat = 5;      // deltatime prediciton time stamp default:0.1s 鹰眼无用
+  repeated mPredictionObjectApp objs_app = 7; //objs for eagle-eye app display 总个数：AI预测障碍物输出最多31个，算自车32个；规则输出个数不固定
+
+}
--- 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
@@ -45,7 +45,7 @@ import packet_mytest_upload.CopyBag;
 import perception.FusionTrafficLightOuterClass;
 import perception.LaneMarkOuterClass;
 import planning.RoboSweeperTaskIndexOuterClass;
-import prediction.Prediction;
+import prediction2025.Prediction2025;
 import record_cache.RecordPanelOuterClass;
 import system_master.SsmInfo;
 import system_master.SystemStatusInfo;
@@ -141,12 +141,12 @@ public interface OnAdasListener {
    void onPerceptionTrafficLight(MessagePad.Header header, FusionTrafficLightOuterClass.FusionTrafficLights trafficLights);

    /**
-     * 他车轨迹预测
+     * 障碍物轨迹预测
     *
     * @param header            头
     * @param predictionObjects 他车轨迹预测数据
     */
-    void onPredictionObstacleTrajectory(MessagePad.Header header, Prediction.mPredictionObjects predictionObjects);
+    void onPredictionObstacleTrajectory(@NonNull MessagePad.Header header,@NonNull Prediction2025.mPredictionObjects predictionObjects);

    /**
     * 透传的点云数据
--- 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
@@ -121,7 +121,7 @@ public class MyMessageFactory implements IMyMessageFactory {
                perceptionTrafficLightMessage = new PerceptionTrafficLightMessage();
            }
            return perceptionTrafficLightMessage;
-        } else if (messageType == MessageType.TYPE_RECEIVE_PREDICTION_OBSTACLE_TRAJECTORY.typeCode) {
+        } else if (messageType == MessageType.TYPE_RECEIVE_PREDICTION_OBSTACLE_TRAJECTORY2025.typeCode) {
            //他车轨迹预测
            if (predictionObstacleTrajectoryMessage == null) {
                predictionObstacleTrajectoryMessage = new PredictionObstacleTrajectoryMessage();
--- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/PredictionObstacleTrajectoryMessage.java
+++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/PredictionObstacleTrajectoryMessage.java
@@ -8,23 +8,23 @@ import com.zhidao.support.adas.high.OnAdasListener;
 import com.zhidao.support.adas.high.common.CupidLogUtils;
 import com.zhidao.support.adas.high.protocol.RawData;

-import prediction.Prediction;
+import prediction2025.Prediction2025;

 /**
- * 他车轨迹预测
+ * 障碍物轨迹预测
 */
 public class PredictionObstacleTrajectoryMessage extends MyAbstractMessageHandler {

    @Override
    public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException {
-        Prediction.mPredictionObjects objects = Prediction.mPredictionObjects.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue());
-        AdasChannel.calculateTimeConsumingOnDispatchRaw("他车轨迹预测", raw.receiveTime);
+        Prediction2025.mPredictionObjects objects = Prediction2025.mPredictionObjects.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.onPredictionObstacleTrajectory(raw.getHeader(), objects);
        }
-        AdasChannel.calculateTimeConsumingBusiness("他车轨迹预测", nowTime);
+        AdasChannel.calculateTimeConsumingBusiness("障碍物轨迹预测", nowTime);
    }
 }