diff --git a/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt b/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt
index 6a6b34a15f..7baf14e26f 100644
--- a/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt
+++ b/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt
@@ -14,12 +14,14 @@ import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_ALIAS_C
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_ALIAS_CODE_ADAS_MESSAGE_AUTOPILOT_VEHICLE
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_ALIAS_CODE_ADAS_MESSAGE_CAR_CONFIG
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_ALIAS_CODE_ADAS_MESSAGE_CAR_STATE
+import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_ALIAS_CODE_ADAS_MESSAGE_PLANNING_OBJECTS
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_ALIAS_CODE_ADAS_MESSAGE_RECT_DATA
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_ADAS
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_CONNECT_STATUS
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_WEB_SOCKET_AUTOPILOT
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_WEB_SOCKET_DATA_TRACKED
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_WEB_SOCKET_GNSSINFO
+import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_WEB_SOCKET_PLANNING_OBJECTS
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_WEB_SOCKET_TRAFFIC_LIGHT
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_WEB_SOCKET_TRAJECTORY
 import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_LINK_LOG_WEB_SOCKET_VEHICLE
@@ -33,6 +35,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotCarConfigListe
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotCarStatusListenerManager.invokeAutopilotCarStateData
 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.invokeAutopilotWarnMessage
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotPlanningListenerManager.invokeAutopilotRotting
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotPlanningListenerManager.invokeAutopilotTrajectory
@@ -102,7 +105,6 @@ class MoGoAdasListenerImpl : OnAdasListener {
         }
     }
 
-
     //自车定位信息
     @ChainLog(
         linkChainLog = CHAIN_LINK_LOG_WEB_SOCKET_GNSSINFO,
@@ -252,11 +254,22 @@ class MoGoAdasListenerImpl : OnAdasListener {
         //点云数据透传
     }
 
+    //planning障碍物
+    @ChainLog(
+        linkChainLog = CHAIN_LINK_LOG_WEB_SOCKET_PLANNING_OBJECTS,
+        linkCode = CHAIN_LINK_ADAS,
+        endpoint = PAD,
+        nodeAliasCode = CHAIN_ALIAS_CODE_ADAS_MESSAGE_PLANNING_OBJECTS,
+        paramIndexes = [0, 1],
+        clientPkFileName = "sn"
+    )
     override fun onPlanningObjects(
         header: MessagePad.Header?,
-        planningObjects: MessagePad.PlanningObjects?
+        planningObjects: MessagePad.PlanningObjects
     ) {
-        //planning障碍物
+        if (HdMapBuildConfig.isMapLoaded) {
+            invokeAutopilotIdentifyPlanningObj(planningObjects.objsList as List<MessagePad.PlanningObject>)
+        }
     }
 
     override fun onBasicInfoReq(
diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/trace/TraceManager.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/trace/TraceManager.kt
index 07af6d7a33..fe9561dbcc 100644
--- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/trace/TraceManager.kt
+++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/trace/TraceManager.kt
@@ -49,6 +49,8 @@ class TraceManager {
             FwBuild(false, 30,pkgName + ChainConstant.CHAIN_LINK_LOG_ADAS_VEHICLE)
         fwBuildMap[ChainConstant.CHAIN_LINK_LOG_WEB_SOCKET_TRAFFIC_LIGHT] =
             FwBuild(false, 30,pkgName + ChainConstant.CHAIN_LINK_LOG_ADAS_TRAFFIC_LIGHT)
+        fwBuildMap[ChainConstant.CHAIN_LINK_LOG_WEB_SOCKET_PLANNING_OBJECTS] =
+            FwBuild(false, 30,pkgName + ChainConstant.CHAIN_LINK_LOG_ADAS_PLANNING_OBJECTS)
 
         traceInfoCache[ChainConstant.CHAIN_LINK_LOG_CONNECT_STATUS] =
             ChainLogParam(true, "ADAS连接状态")
@@ -62,6 +64,8 @@ class TraceManager {
             ChainLogParam(false, "ADAS车前引导线")
         traceInfoCache[ChainConstant.CHAIN_LINK_LOG_WEB_SOCKET_VEHICLE] =
             ChainLogParam(false, "ADAS车辆底盘数据")
+        traceInfoCache[ChainConstant.CHAIN_LINK_LOG_WEB_SOCKET_PLANNING_OBJECTS] =
+            ChainLogParam(false, "ADAS PLANNING 感知障碍物")
 
         FileWriteManager.getInstance()
             .init(context, MoGoAiCloudClientConfig.getInstance().sn, pkgName, fwBuildMap)
diff --git a/core/function-impl/mogo-core-function-hmi/src/main/java/com/mogo/eagle/core/function/hmi/ui/setting/DebugSettingView.kt b/core/function-impl/mogo-core-function-hmi/src/main/java/com/mogo/eagle/core/function/hmi/ui/setting/DebugSettingView.kt
index d28909d5e6..de04584c9c 100644
--- a/core/function-impl/mogo-core-function-hmi/src/main/java/com/mogo/eagle/core/function/hmi/ui/setting/DebugSettingView.kt
+++ b/core/function-impl/mogo-core-function-hmi/src/main/java/com/mogo/eagle/core/function/hmi/ui/setting/DebugSettingView.kt
@@ -138,7 +138,7 @@ class DebugSettingView @JvmOverloads constructor(
     private var clickListener: ClickListener? = null
 
     //剪切板
-    private var clipboardManager: ClipboardManager ?= null
+    private var clipboardManager: ClipboardManager? = null
 
     private val simpleDateFormat by lazy {
         SimpleDateFormat("yyyy-MM-dd HH:mm:ss.SSS")
@@ -292,10 +292,10 @@ class DebugSettingView @JvmOverloads constructor(
          * 开发者模式
          */
         swDevelopMode.setOnCheckedChangeListener { _, isChecked ->
-            if(isChecked){
+            if (isChecked) {
                 controlCenterLayout.visibility = View.VISIBLE
                 commonLayout.visibility = View.GONE
-            }else{
+            } else {
                 controlCenterLayout.visibility = View.GONE
                 commonLayout.visibility = View.VISIBLE
             }
@@ -340,12 +340,12 @@ class DebugSettingView @JvmOverloads constructor(
                 buttonView.setCompoundDrawables(null, null, iconDown, null)
                 //展示OBU控制中心
                 obuControllerLayout.visibility = View.VISIBLE
-                tbVehicleStateController.isChecked  = true
+                tbVehicleStateController.isChecked = true
             } else {
                 buttonView.setCompoundDrawables(null, null, iconRight, null)
                 //隐藏OBU控制中心
                 obuControllerLayout.visibility = View.GONE
-                tbVehicleStateController.isChecked  = false
+                tbVehicleStateController.isChecked = false
             }
         }
 
@@ -550,17 +550,17 @@ class DebugSettingView @JvmOverloads constructor(
             FunctionBuildConfig.isRainMode = isChecked
         }
         //雨天模式按钮只在司机屏生效，乘客屏不显示
-        if(AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)){
+        if (AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)) {
             tbIsRainMode.visibility = View.GONE
         }
 
         //重启工控机所有节点
-        btnIpcReboot.onClick{
+        btnIpcReboot.onClick {
             CallerAutoPilotManager.sendIpcReboot()
             ToastUtils.showLong("重启命令已发送")
         }
         //只在司机端设置工控机节点重启功能
-        if(AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)){
+        if (AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)) {
             btnIpcReboot.visibility = View.GONE
         }
 
@@ -636,15 +636,15 @@ class DebugSettingView @JvmOverloads constructor(
         }
 
         //设置点云大小
-        btnPointCloudSize.setOnClickListener{
+        btnPointCloudSize.setOnClickListener {
             val cloudSize = etPointCloudSize.text.toString()
-            if(cloudSize.isEmpty()){
+            if (cloudSize.isEmpty()) {
                 ToastUtils.showShort("请输入正确的点云大小")
-            }else{
+            } else {
                 try {
                     val cloudSizeFloat = cloudSize.toFloat()
                     CallerHDMapManager.setPointCloudSize(cloudSizeFloat)
-                }catch (e: Exception){
+                } catch (e: Exception) {
                     ToastUtils.showShort("点云大小格式输入不正确")
                 }
             }
@@ -652,12 +652,12 @@ class DebugSettingView @JvmOverloads constructor(
         //设置点云颜色
         btnPointCloudColor.setOnClickListener {
             val cloudColor = etPointCloudColor.text.toString()
-            if(cloudColor.isEmpty()){
+            if (cloudColor.isEmpty()) {
                 ToastUtils.showShort("请输入正确的点云颜色")
-            }else{
+            } else {
                 try {
                     CallerHDMapManager.setPointCloudColor(cloudColor)
-                }catch (e: Exception){
+                } catch (e: Exception) {
                     ToastUtils.showShort("点云大小颜色输入不正确")
                 }
             }
@@ -695,34 +695,47 @@ class DebugSettingView @JvmOverloads constructor(
 
         //SN复制按钮
         tvPadSnClip.setOnClickListener {
-            if(clipboardManager==null){
+            if (clipboardManager == null) {
                 //获取剪贴板管理器：
-                clipboardManager = context.getSystemService(Context.CLIPBOARD_SERVICE) as ClipboardManager
+                clipboardManager =
+                    context.getSystemService(Context.CLIPBOARD_SERVICE) as ClipboardManager
             }
             // 创建普通字符型ClipData ,将ClipData内容放到系统剪贴板里
-            clipboardManager?.setPrimaryClip(ClipData.newPlainText("MoGoSN",AppConfigInfo.mogoSN))
+            clipboardManager?.setPrimaryClip(ClipData.newPlainText("MoGoSN", AppConfigInfo.mogoSN))
             ToastUtils.showLong("SN复制成功")
         }
 
         //工控机镜像复制按钮
-        tvIpcVersionInfoClip.setOnClickListener{
-            if(clipboardManager==null){
+        tvIpcVersionInfoClip.setOnClickListener {
+            if (clipboardManager == null) {
                 //获取剪贴板管理器：
-                clipboardManager = context.getSystemService(Context.CLIPBOARD_SERVICE) as ClipboardManager
+                clipboardManager =
+                    context.getSystemService(Context.CLIPBOARD_SERVICE) as ClipboardManager
             }
             // 创建普通字符型ClipData ,将ClipData内容放到系统剪贴板里
-            clipboardManager?.setPrimaryClip(ClipData.newPlainText("DockVersion",mAutoPilotStatusInfo?.dockVersion))
+            clipboardManager?.setPrimaryClip(
+                ClipData.newPlainText(
+                    "DockVersion",
+                    mAutoPilotStatusInfo?.dockVersion
+                )
+            )
             ToastUtils.showLong("docker版本复制成功")
         }
 
         //经纬度复制按钮
-        tvCarInfoCopyClip.setOnClickListener{
-            if(clipboardManager==null){
+        tvCarInfoCopyClip.setOnClickListener {
+            if (clipboardManager == null) {
                 //获取剪贴板管理器：
-                clipboardManager = context.getSystemService(Context.CLIPBOARD_SERVICE) as ClipboardManager
+                clipboardManager =
+                    context.getSystemService(Context.CLIPBOARD_SERVICE) as ClipboardManager
             }
             // 创建普通字符型ClipData ,将ClipData内容放到系统剪贴板里
-            clipboardManager?.setPrimaryClip(ClipData.newPlainText("LonAndLat","${mGnssInfo?.longitude},${mGnssInfo?.latitude}"))
+            clipboardManager?.setPrimaryClip(
+                ClipData.newPlainText(
+                    "LonAndLat",
+                    "${mGnssInfo?.longitude},${mGnssInfo?.latitude}"
+                )
+            )
             ToastUtils.showLong("经纬度复制成功")
         }
 
@@ -1135,7 +1148,7 @@ class DebugSettingView @JvmOverloads constructor(
                 if (logTimeStr.isNullOrEmpty()) {
                     logTimeStr = "10"
                 }
-                try{
+                try {
                     val logCatchTime = logTimeStr.toInt()
                     if (logCatchTime > 60) {
                         tbLogCatch.isChecked = false
@@ -1143,7 +1156,7 @@ class DebugSettingView @JvmOverloads constructor(
                         return@setOnCheckedChangeListener
                     }
                     CallerDevaToolsManager.startCatchLog(logCatchTime)
-                }catch (e: Exception){
+                } catch (e: Exception) {
                     ToastUtils.showLong("输入格式错误，请重新输入正确时间数字，最长抓取时间为60分钟")
                     etLogCatch.setText("")
                 }
@@ -1267,7 +1280,7 @@ class DebugSettingView @JvmOverloads constructor(
         }
 
         /**
-         * ADAS车辆地盘数据
+         * ADAS车辆底盘数据
          */
         cbAdasVehicle.setOnCheckedChangeListener { _, isChecked ->
             val map = CallerDevaToolsManager.getTraceInfo()
@@ -1278,6 +1291,20 @@ class DebugSettingView @JvmOverloads constructor(
                 CallerDevaToolsManager.refreshTraceInfo(map)
             }
         }
+
+        /**
+         * ADAS PLANNING OBJ 感知障碍物
+         */
+        cbAdasPlanningObj.setOnCheckedChangeListener { _, isChecked ->
+            val map = CallerDevaToolsManager.getTraceInfo()
+            val param = map[ChainConstant.CHAIN_LINK_LOG_WEB_SOCKET_PLANNING_OBJECTS]
+            param?.let {
+                it.record = isChecked
+                map[ChainConstant.CHAIN_LINK_LOG_WEB_SOCKET_PLANNING_OBJECTS] = param
+                CallerDevaToolsManager.refreshTraceInfo(map)
+            }
+        }
+
     }
 
     private fun refreshTraceInfo() {
@@ -1524,9 +1551,9 @@ class DebugSettingView @JvmOverloads constructor(
             }"
         )
 
-        if(AppConfigInfo.isConnectAutopilot){
+        if (AppConfigInfo.isConnectAutopilot) {
             tvIpcConnectStatus.minLines = 1
-        }else{
+        } else {
             tvIpcConnectStatus.minLines = 4
         }
 
@@ -1546,7 +1573,7 @@ class DebugSettingView @JvmOverloads constructor(
             }"
         )
         //如果是乘客端，则不显示工控机连接状态
-        if(AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)){
+        if (AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)) {
             tvIpcConnectStatus.visibility = View.GONE
             tvAutopilotConnectStatus.visibility = View.GONE
         }
@@ -1856,8 +1883,8 @@ class DebugSettingView @JvmOverloads constructor(
     /**
      * 初始化上报
      */
-    fun reportInit(reportList: ArrayList<ReportEntity>){
-        if(reportList.size > 0){
+    fun reportInit(reportList: ArrayList<ReportEntity>) {
+        if (reportList.size > 0) {
             reportMsgLayout.visibility = View.VISIBLE
             reportList[0].let {
                 tvReportSrc.text = "src:${it.src}"
diff --git a/core/function-impl/mogo-core-function-hmi/src/main/res/layout/view_debug_setting.xml b/core/function-impl/mogo-core-function-hmi/src/main/res/layout/view_debug_setting.xml
index 8a00091c6e..b1a26a0e13 100644
--- a/core/function-impl/mogo-core-function-hmi/src/main/res/layout/view_debug_setting.xml
+++ b/core/function-impl/mogo-core-function-hmi/src/main/res/layout/view_debug_setting.xml
@@ -1967,6 +1967,16 @@
                         android:text="ADAS底盘数据"
                         android:textColor="#000"
                         android:textSize="@dimen/dp_24" />
+
+                    <CheckBox
+                        android:id="@+id/cbAdasPlanningObj"
+                        android:layout_width="wrap_content"
+                        android:layout_height="wrap_content"
+                        android:checked="false"
+                        android:padding="@dimen/dp_10"
+                        android:text="ADAS PLANNING 感知障碍物"
+                        android:textColor="#000"
+                        android:textSize="@dimen/dp_24" />
                 </LinearLayout>
             </LinearLayout>
 
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt
index fe802a2d14..357b370784 100644
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt
@@ -42,9 +42,19 @@ class MapIdentifySubscriber private constructor() : IMoGoSubscriber, IMoGoAutopi
     override fun onAutopilotIdentifyDataUpdate(trafficData: List<TrackedObject>?) {
         try {
             if (FunctionBuildConfig.isDrawIdentifyData) {
-                ThreadUtils.getSinglePool().execute { IdentifyFactory.getInstance().renderAdasRecognizedResult(trafficData) }
+                ThreadUtils.getSinglePool().execute { IdentifyFactory.renderAdasRecognizedResult(trafficData) }
             } else {
-                IdentifyFactory.getInstance().clearOldMarker()
+                IdentifyFactory.clearOldMarker()
+            }
+        } catch (e: Exception) {
+            e.printStackTrace()
+        }
+    }
+
+    override fun onAutopilotIdentifyPlanningObj(planningObjects: List<MessagePad.PlanningObject>?) {
+        try {
+            if (FunctionBuildConfig.isDrawIdentifyData) {
+                ThreadUtils.getSinglePool().execute { IdentifyFactory.renderPlanningWarningObj(planningObjects) }
             }
         } catch (e: Exception) {
             e.printStackTrace()
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java
index 7e63ad99fb..5673c2ed2d 100644
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java
@@ -1,7 +1,5 @@
 package com.mogo.eagle.core.function.map.identify;
 
-import com.mogo.eagle.core.function.map.identify.ObjQueue;
-
 import java.util.List;
 import java.util.Vector;
 
@@ -34,6 +32,14 @@ public class CircleQueue {
         return objQueue.subList(objQueue.size() - 3, objQueue.size());
     }
 
+    public List<ObjQueue> getLastFiveFrame() {
+        return objQueue.subList(objQueue.size() - 5, objQueue.size());
+    }
+
+    public List<ObjQueue> getPreFrame() {
+        return objQueue.subList(0, objQueue.size());
+    }
+
     public ObjQueue getLastFrame() {
         return objQueue.lastElement();
     }
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.java
deleted file mode 100644
index 515f2c428f..0000000000
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.java
+++ /dev/null
@@ -1,12 +0,0 @@
-package com.mogo.eagle.core.function.map.identify;
-
-import java.util.List;
-
-import mogo.telematics.pad.MessagePad;
-
-public interface Identify {
-
-    void renderAdasRecognizedResult(List<MessagePad.TrackedObject> resultList);
-
-    void clearOldMarker();
-}
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt
new file mode 100644
index 0000000000..9a6f593406
--- /dev/null
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt
@@ -0,0 +1,13 @@
+package com.mogo.eagle.core.function.map.identify
+
+import mogo.telematics.pad.MessagePad
+import mogo.telematics.pad.MessagePad.TrackedObject
+
+interface Identify {
+
+    fun renderPlanningWarningObj(planningObjects: List<MessagePad.PlanningObject>?)
+
+    fun renderAdasRecognizedResult(resultList: List<TrackedObject>?)
+
+    fun clearOldMarker()
+}
\ No newline at end of file
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.java
deleted file mode 100644
index cc58abb4fb..0000000000
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.java
+++ /dev/null
@@ -1,65 +0,0 @@
-package com.mogo.eagle.core.function.map.identify;
-
-import android.annotation.SuppressLint;
-import android.util.Log;
-
-import com.mogo.commons.AbsMogoApplication;
-import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
-import com.mogo.map.MogoMarkerManager;
-import com.mogo.module.common.MogoApisHandler;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import mogo.telematics.pad.MessagePad;
-
-/**
- * @author xiaoyuzhou
- * @date 2021/10/19 10:45 上午
- * 域控制器识别信息绘制
- */
-public class IdentifyBeautifyDataDrawer implements Identify{
-    private static final String TAG = "IdentifyDataDrawer";
-
-    /**
-     * 渲染 adas 识别的数据
-     *
-     * @param resultList adas感知融合数据
-     */
-    @SuppressLint("NewApi")
-    @Override
-    public void renderAdasRecognizedResult(List<MessagePad.TrackedObject> resultList) {
-        if (resultList == null || resultList.isEmpty()) {
-            TrackManager.getInstance().clearAll();
-            CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……");
-            return;
-        }
-
-        if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) {
-            TrackManager.getInstance().clearAll();
-            CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式");
-            return;
-        }
-
-        //清除缓存
-        TrackManager.getInstance().clearCache(resultList);
-
-        long cost = System.nanoTime();
-        ArrayList<MessagePad.TrackedObject> filterList = TrackManager.getInstance().filterTrafficData(resultList);
-        Log.d("time cost", " " + (System.nanoTime() - cost) / 1000000 + " , 处理了" + resultList.size() + "条数据");
-        if (filterList.size() > 0) {
-            // 绘制新数据
-            MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
-                    .updateBatchMarkerPosition(filterList);
-        }
-    }
-
-    /**
-     * 清除旧的 marker 数据
-     */
-    @Override
-    public void clearOldMarker() {
-        TrackManager.getInstance().clearAll();
-    }
-}
-
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt
new file mode 100644
index 0000000000..6d7dbf42c9
--- /dev/null
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt
@@ -0,0 +1,64 @@
+package com.mogo.eagle.core.function.map.identify
+
+import android.annotation.SuppressLint
+import com.mogo.commons.AbsMogoApplication
+import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger.w
+import com.mogo.map.MogoMarkerManager
+import com.mogo.module.common.MogoApisHandler
+import mogo.telematics.pad.MessagePad
+import mogo.telematics.pad.MessagePad.TrackedObject
+
+/**
+ * 域控制器识别信息绘制
+ */
+class IdentifyBeautifyDataDrawer : Identify {
+
+    companion object {
+        private const val TAG = "IdentifyDataDrawer"
+    }
+
+    override fun renderPlanningWarningObj(planningObjects: List<MessagePad.PlanningObject>?) {
+        TrackManager.getInstance().filterWarningData(planningObjects)
+    }
+
+    /**
+     * 渲染 adas 识别的数据
+     *
+     * @param resultList adas感知融合数据
+     */
+    @SuppressLint("NewApi")
+    override fun renderAdasRecognizedResult(resultList: List<TrackedObject>?) {
+        if (resultList == null || resultList.isEmpty()) {
+            TrackManager.getInstance().clearAll()
+            w(TAG, "感知数据为空无需渲染……")
+            return
+        }
+        if (!MogoApisHandler.getInstance().apis.statusManagerApi.isVrMode) {
+            TrackManager.getInstance().clearAll()
+            w(TAG, "渲染 adas 识别的数据 当前不是VR模式")
+            return
+        }
+
+        //清除缓存
+        TrackManager.getInstance().clearCache(resultList)
+//        val cost = System.nanoTime()
+        val filterList = TrackManager.getInstance().filterTrafficData(resultList)
+//        Log.d(
+//            "time cost",
+//            " " + (System.nanoTime() - cost) / 1000000 + " , 处理了" + resultList.size + "条数据"
+//        )
+        if (filterList.size > 0) {
+            // 绘制新数据
+            MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
+                .updateBatchMarkerPosition(filterList)
+        }
+    }
+
+    /**
+     * 清除旧的 marker 数据
+     */
+    override fun clearOldMarker() {
+        TrackManager.getInstance().clearAll()
+    }
+
+}
\ No newline at end of file
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.java
deleted file mode 100644
index d3aa7f2a46..0000000000
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.java
+++ /dev/null
@@ -1,52 +0,0 @@
-package com.mogo.eagle.core.function.map.identify;
-
-import com.mogo.eagle.core.data.config.FunctionBuildConfig;
-import com.mogo.eagle.core.utilcode.mogo.AppIdentityModeUtils;
-
-import java.util.List;
-
-import mogo.telematics.pad.MessagePad;
-
-public class IdentifyFactory implements Identify {
-
-    private static IdentifyFactory identifyFactory;
-    private static Identify identify;
-    private static final byte[] obj = new byte[0];
-
-    public static final class DriverIdentify {
-        private static final IdentifyOriginDataDrawer originDataDrawer = new IdentifyOriginDataDrawer();
-    }
-
-    public static final class UserIdentify {
-        private static final IdentifyBeautifyDataDrawer beautifyDataDrawer = new IdentifyBeautifyDataDrawer();
-    }
-
-    private IdentifyFactory() { //todo 还得加开关做判断
-        if (AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode)) {
-            identify = UserIdentify.beautifyDataDrawer;
-        } else {
-            identify = DriverIdentify.originDataDrawer;
-        }
-    }
-
-    public static IdentifyFactory getInstance() {
-        if (identifyFactory == null) {
-            synchronized (obj) {
-                if (identifyFactory == null) {
-                    identifyFactory = new IdentifyFactory();
-                }
-            }
-        }
-        return identifyFactory;
-    }
-
-    @Override
-    public void renderAdasRecognizedResult(List<MessagePad.TrackedObject> resultList) {
-        identify.renderAdasRecognizedResult(resultList);
-    }
-
-    @Override
-    public void clearOldMarker() {
-        identify.clearOldMarker();
-    }
-}
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.kt
new file mode 100644
index 0000000000..b459127ab8
--- /dev/null
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.kt
@@ -0,0 +1,38 @@
+package com.mogo.eagle.core.function.map.identify
+
+import com.mogo.eagle.core.data.config.FunctionBuildConfig
+import com.mogo.eagle.core.utilcode.mogo.AppIdentityModeUtils
+import mogo.telematics.pad.MessagePad
+import mogo.telematics.pad.MessagePad.TrackedObject
+
+object IdentifyFactory : Identify {
+    object DriverIdentify {
+        internal val originDataDrawer = IdentifyOriginDataDrawer()
+    }
+
+    object UserIdentify {
+        internal val beautifyDataDrawer = IdentifyBeautifyDataDrawer()
+    }
+
+    override fun renderPlanningWarningObj(planningObjects: List<MessagePad.PlanningObject>?) {
+        identify!!.renderPlanningWarningObj(planningObjects)
+    }
+
+    override fun renderAdasRecognizedResult(resultList: List<TrackedObject>?) {
+        identify!!.renderAdasRecognizedResult(resultList)
+    }
+
+    override fun clearOldMarker() {
+        identify!!.clearOldMarker()
+    }
+
+    private var identify: Identify? = null
+
+    init { //todo 还得加开关做判断
+        identify = if (AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode)) {
+            UserIdentify.beautifyDataDrawer
+        } else {
+            DriverIdentify.originDataDrawer
+        }
+    }
+}
\ No newline at end of file
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.java
deleted file mode 100644
index 1aabe3f7d7..0000000000
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.java
+++ /dev/null
@@ -1,132 +0,0 @@
-package com.mogo.eagle.core.function.map.identify;
-
-
-import android.annotation.SuppressLint;
-import android.content.Context;
-
-import com.mogo.commons.AbsMogoApplication;
-import com.mogo.eagle.core.data.config.FunctionBuildConfig;
-import com.mogo.eagle.core.data.enums.TrafficTypeEnum;
-import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
-import com.mogo.map.MogoMarkerManager;
-import com.mogo.module.common.MogoApisHandler;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.concurrent.ConcurrentHashMap;
-
-import mogo.telematics.pad.MessagePad;
-
-/**
- * @author xiaoyuzhou
- * @date 2021/10/19 10:45 上午
- * 域控制器识别信息绘制
- */
-public class IdentifyOriginDataDrawer implements Identify{
-    private static final String TAG = "IdentifyDataDrawer";
-
-    /**
-     * 上一帧数据的缓存
-     */
-    private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
-
-    /**
-     * kalman缓存数据
-     */
-    private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
-
-    /**
-     * 记录每次实际绘制的交通元素UUID
-     */
-    private final ArrayList<String> trafficDataUuidList = new ArrayList<>();
-    /**
-     * 过滤后的数据集合
-     */
-    private final ArrayList<MessagePad.TrackedObject> mFilterTrafficData = new ArrayList<>();
-
-    /**
-     * 渲染 adas 识别的数据
-     *
-     * @param resultList adas感知融合数据
-     */
-    @SuppressLint("NewApi")
-    @Override
-    public void renderAdasRecognizedResult(List<MessagePad.TrackedObject> resultList) {
-        if (resultList == null || resultList.isEmpty()) {
-            clearOldMarker();
-            CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……");
-            return;
-        }
-
-        if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) {
-            clearOldMarker();
-            CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式");
-            return;
-        }
-
-        //清除缓存
-        for (MessagePad.TrackedObject data : resultList) {
-            if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
-                trafficDataUuidList.remove("" + data.getUuid());
-            }
-        }
-        trafficDataUuidList.forEach(uuid -> {
-            mMarkersCaches.remove(uuid);
-            algoCache.remove(uuid);
-        });
-
-        ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
-
-        if (filterList.size() > 0) {
-            // 绘制新数据
-            MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
-                    .updateBatchMarkerPosition(filterList);
-        }
-    }
-
-    /**
-     * 数据过滤器
-     *
-     * @return 过滤后的数据集合
-     */
-    private ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> trafficData) {
-        mFilterTrafficData.clear();
-        trafficDataUuidList.clear();
-        for (MessagePad.TrackedObject data : trafficData) {
-            // 过滤掉未知感知数据
-            if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) {
-                //CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染");
-                continue;
-            }
-
-            String uuid = "" + data.getUuid();
-            //首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map
-            MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
-            if (cacheData != null) {
-                if (data.getSpeed() < 0.5) {
-                    data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build();
-                }
-                mFilterTrafficData.add(data);
-                //更新已存在的感知物体数据
-            }
-            mMarkersCaches.put(uuid, data);
-            trafficDataUuidList.add(uuid);
-        }
-        return mFilterTrafficData;
-    }
-
-    /**
-     * 清除旧的 marker 数据
-     */
-    @Override
-    public void clearOldMarker() {
-        for (String uuid : trafficDataUuidList) {
-            MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
-                    .removeMarker(uuid);
-        }
-        trafficDataUuidList.clear();
-    }
-}
-
-
-
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt
new file mode 100644
index 0000000000..51c8ad06bf
--- /dev/null
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt
@@ -0,0 +1,165 @@
+package com.mogo.eagle.core.function.map.identify
+
+import android.annotation.SuppressLint
+import com.mogo.commons.AbsMogoApplication
+import com.mogo.eagle.core.data.config.FunctionBuildConfig
+import com.mogo.eagle.core.data.enums.TrafficTypeEnum
+import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger.w
+import com.mogo.map.MogoMarkerManager
+import com.mogo.module.common.MogoApisHandler
+import mogo.telematics.pad.MessagePad
+import mogo.telematics.pad.MessagePad.TrackedObject
+import java.util.*
+import java.util.concurrent.ConcurrentHashMap
+import java.util.function.Consumer
+
+/**
+ * @author xiaoyuzhou
+ * @date 2021/10/19 10:45 上午
+ * 域控制器识别信息绘制
+ */
+class IdentifyOriginDataDrawer : Identify {
+
+    companion object {
+        private const val TAG = "IdentifyDataDrawer"
+    }
+
+    /**
+     * 上一帧数据的缓存
+     */
+    private val mMarkersCaches = ConcurrentHashMap<String, TrackedObject>()
+
+    /**
+     * kalman缓存数据
+     */
+    private val algoCache = ConcurrentHashMap<String, KalmanFilter>()
+
+    /**
+     * 记录每次实际绘制的交通元素UUID
+     */
+    private val trafficDataUuidList = ArrayList<String>()
+
+    /**
+     * 过滤后的数据集合
+     */
+    private val mFilterTrafficData = ArrayList<TrackedObject>()
+
+    /**
+     * planning 感知物预警缓存，用于重置color状态
+     */
+    private val colorTrafficData = ArrayList<String>()
+
+    //todo reset color
+    override fun renderPlanningWarningObj(planningObjects: List<MessagePad.PlanningObject>?) {
+//        if (planningObjects == null) {
+//            if (colorTrafficData.size == 0) {
+//                return
+//            }
+//            colorTrafficData.forEach {
+//                val cacheData = mMarkersCaches[it] //todo 是否要直接绘制 还是等下一帧
+//                if (cacheData != null) {
+//                    mMarkersCaches[it] = cacheData.toBuilder().setColor("#D8D8D8FF").build()
+//                }
+//            }
+//            colorTrafficData.clear()
+//            return
+//        }
+//        val tempTrafficData = ArrayList<TrackedObject>()
+//        planningObjects.forEach {
+//            val trackId = it.uuid.toString()
+//            val cacheData = mMarkersCaches[trackId]
+//            if (cacheData != null) {
+//                colorTrafficData.add(trackId)
+//                when (it.type) {
+//                    0 -> {
+//                        tempTrafficData.add(cacheData.toBuilder().setColor("#FF3C45FF").build())
+//                    }
+//                    1 -> {
+//                        tempTrafficData.add(cacheData.toBuilder().setColor("#FFD53EFF").build())
+//                    }
+//                }
+//            }
+//            MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
+//                .updateBatchMarkerPosition(tempTrafficData)
+//        }
+    }
+
+    /**
+     * 渲染 adas 识别的数据
+     *
+     * @param resultList adas感知融合数据
+     */
+    @SuppressLint("NewApi")
+    override fun renderAdasRecognizedResult(resultList: List<TrackedObject>?) {
+        if (resultList == null || resultList.isEmpty()) {
+            clearOldMarker()
+            w(TAG, "感知数据为空无需渲染……")
+            return
+        }
+        if (!MogoApisHandler.getInstance().apis.statusManagerApi.isVrMode) {
+            clearOldMarker()
+            w(TAG, "渲染 adas 识别的数据 当前不是VR模式")
+            return
+        }
+
+        //清除缓存
+        for (data in resultList) {
+            if (trafficDataUuidList.size > 0 && trafficDataUuidList.contains("" + data.uuid)) {
+                trafficDataUuidList.remove("" + data.uuid)
+            }
+        }
+        trafficDataUuidList.forEach(Consumer { uuid: String ->
+            mMarkersCaches.remove(uuid)
+            algoCache.remove(uuid)
+        })
+        val filterList = filterTrafficData(resultList)
+        if (filterList.size > 0) {
+            // 绘制新数据
+            MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
+                .updateBatchMarkerPosition(filterList)
+        }
+    }
+
+    /**
+     * 数据过滤器
+     *
+     * @return 过滤后的数据集合
+     */
+    private fun filterTrafficData(trafficData: List<TrackedObject>): ArrayList<TrackedObject> {
+        mFilterTrafficData.clear()
+        trafficDataUuidList.clear()
+        for (data in trafficData) {
+            // 过滤掉未知感知数据
+            if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.type == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.type) {
+                //CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染");
+                continue
+            }
+            val uuid = "" + data.uuid
+            //首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map
+            val cacheData = mMarkersCaches[uuid]
+            if (cacheData != null) {
+                if (data.speed < 0.5) {
+                    data.toBuilder().setHeading(cacheData.heading).setLongitude(cacheData.longitude)
+                        .setLatitude(cacheData.latitude).build()
+                }
+                mFilterTrafficData.add(data)
+                //更新已存在的感知物体数据
+            }
+            mMarkersCaches[uuid] = data
+            trafficDataUuidList.add(uuid)
+        }
+        return mFilterTrafficData
+    }
+
+    /**
+     * 清除旧的 marker 数据
+     */
+    override fun clearOldMarker() {
+        for (uuid in trafficDataUuidList) {
+            MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
+                .removeMarker(uuid)
+        }
+        trafficDataUuidList.clear()
+    }
+
+}
\ No newline at end of file
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java
index 916e1b9082..fea37135ee 100644
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java
@@ -1,8 +1,8 @@
 package com.mogo.eagle.core.function.map.identify;
 
-import android.content.Context;
 import android.os.Build;
 import android.util.ArrayMap;
+import android.util.Log;
 
 import androidx.annotation.RequiresApi;
 
@@ -11,8 +11,6 @@ import com.google.common.collect.HashBiMap;
 import com.mogo.commons.AbsMogoApplication;
 import com.mogo.eagle.core.data.config.FunctionBuildConfig;
 import com.mogo.eagle.core.data.enums.TrafficTypeEnum;
-import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager;
-import com.mogo.eagle.core.utilcode.mogo.AppIdentityModeUtils;
 import com.mogo.map.MogoMarkerManager;
 
 import java.text.DecimalFormat;
@@ -34,7 +32,7 @@ public class TrackManager {
 
     public static final DecimalFormat DF = new DecimalFormat("0.000000");
     public static final int DISTANCE = 6371000;
-    public static double LIMIT_SPEED = AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode) ? 0.7 : 0.5;
+    public static double LIMIT_SPEED = 0.5;
 
     /**
      * marker缓存队列
@@ -109,6 +107,11 @@ public class TrackManager {
         return mFilterTrafficData;
     }
 
+    //todo reset color
+    public void filterWarningData(List<MessagePad.PlanningObject> planningObjects) {
+
+    }
+
     @RequiresApi(api = Build.VERSION_CODES.N)
     public void clearCache(List<MessagePad.TrackedObject> resultList) {
         //清除缓存
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java
index 6e4cee017a..e4f8b94915 100644
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java
@@ -26,6 +26,8 @@ public class TrackObj {
     private double lat;
     private double lon;
     private double speedAverage;
+//    private SinglePointRoadInfo singlePointRoadInfo;
+//    private double[] matchedPoint;
 
     public TrackObj(MessagePad.TrackedObject data) {
         kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
@@ -64,11 +66,17 @@ public class TrackObj {
 
     private void calAverageSpeed() {
         //计算平均速度
-        if (circleQueue.size() >= 3) {
-            List<ObjQueue> objQueueList = circleQueue.getLastThreeFrame();
-            speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3;
+        if (circleQueue.size() >= 5) {
+            List<ObjQueue> objQueueList = circleQueue.getLastFiveFrame();
+            speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed() + objQueueList.get(3).getSpeed() + objQueueList.get(4).getSpeed()) / 5;
         } else {
-            speedAverage = circleQueue.getLastFrame().getSpeed();
+            double cal = 0;
+            List<ObjQueue> objQueueList = circleQueue.getPreFrame();
+            for (ObjQueue obj : objQueueList) {
+                cal += obj.getSpeed();
+            }
+            speedAverage = cal / objQueueList.size();
+//            speedAverage = circleQueue.getLastFrame().getSpeed();
         }
     }
 
@@ -77,36 +85,60 @@ public class TrackObj {
         //todo  bus250  taxi上测试下面注释掉内容
         //double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude());
 //        double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[1], lonLat[0])).distance(DISTANCE);
-        double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude())).distance(DISTANCE);
+//        double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude())).distance(DISTANCE);
         //todo 重新计算速度值(如果连续几帧distance累加到一定值，速度没变化，需要重新计算速度，防止锁死)
-        if (relativeStatic()) {
-            double tempDis = distance;
-            if (distance >= 4) {  //(150km/h) 41.6m/s x 0.1s = 4.16m 约等于 4
-                tempDis = 4;
-            }
-            double calSpeed = cacheData.getSpeed();
-            if (cacheData.getSpeed() != 0.0) {
-                calSpeed = tempDis / ((Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) / 1000.0);
-//                Log.d("calSpeed uuid : " + cacheData.getUuid(), " tempDis : " + tempDis + " , 重新赋值 calSpeed : " + DF.format(calSpeed) + " , time : " + (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) + " , 原速度 : " + cacheData.getSpeed());
-                if (calSpeed > cacheData.getSpeed()) {
-                    calSpeed = cacheData.getSpeed();
-//                    Log.d("calSpeed uuid : " + cacheData.getUuid(), "  二次重新赋值 calSpeed : " + DF.format(calSpeed));
-                }
-                if (calSpeed > 2) {
-                    calSpeed = 2;
-//                    Log.d("calSpeed uuid : " + cacheData.getUuid(), "  三次重新赋值 calSpeed : " + DF.format(calSpeed));
-                }
-            }
-            cacheData = cacheData.toBuilder().setSpeed(calSpeed).build();
-        }
+//        if (relativeStatic()) {
+//            double tempDis = distance;
+//            if (distance >= 4) {  //(150km/h) 41.6m/s x 0.1s = 4.16m 约等于 4
+//                tempDis = 4;
+//            }
+//            double calSpeed = cacheData.getSpeed();
+//            if (cacheData.getSpeed() != 0.0) {
+//                calSpeed = tempDis / ((Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) / 1000.0);
+////                Log.d("calSpeed uuid : " + cacheData.getUuid(), " tempDis : " + tempDis + " , 重新赋值 calSpeed : " + DF.format(calSpeed) + " , time : " + (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) + " , 原速度 : " + cacheData.getSpeed());
+//                if (calSpeed > cacheData.getSpeed()) {
+//                    calSpeed = cacheData.getSpeed();
+////                    Log.d("calSpeed uuid : " + cacheData.getUuid(), "  二次重新赋值 calSpeed : " + DF.format(calSpeed));
+//                }
+////                if (calSpeed > 2) {
+////                    calSpeed = 2;
+//////                    Log.d("calSpeed uuid : " + cacheData.getUuid(), "  三次重新赋值 calSpeed : " + DF.format(calSpeed));
+////                }
+//            }
+//            cacheData = cacheData.toBuilder().setSpeed(calSpeed).build();
+//        }
         //todo 等后序速度优化结果值可用，使用计算结果
 //        double calDistance = (cacheData.getSpeed() * (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime)) / 1000.0;
-        double calDistance = Utils.calculateLineDistance(lon, lat, cacheData.getLongitude(), cacheData.getLatitude());
+//        double calDistance = Utils.calculateLineDistance(lon, lat, cacheData.getLongitude(), cacheData.getLatitude());
 //        Log.d("calLoc uuid : " + cacheData.getUuid() + "  calDistance : " + DF.format(calDistance), (calDistance * 2 < distance) ? "超出范围" : "正常值");
         //速度小于0.5m/s,并且距离在计算合理范围内超出2倍，则认为是相对静止状态(注意调整阈值)，不更新缓存点信息
-        if (cacheData.getSpeed() < LIMIT_SPEED || relativeStatic() || calDistance * 2 < distance) {
-//            Log.i("calLoc uuid : " + cacheData.getUuid(), cacheData.getSpeed() < LIMIT_SPEED ? " 减速到静止" : "拉回来了 cal :" + calDistance + " , distance  : " + distance + " , lon : " + lon + " , lat : " + lat);
-            cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
+//        if (cacheData.getSpeed() < LIMIT_SPEED || relativeStatic() || calDistance * 2 < distance) {
+        if (relativeStatic()) {
+//            if (singlePointRoadInfo == null) {
+//                double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading();
+//                long cost = System.nanoTime();
+//                singlePointRoadInfo = MapDataApi.INSTANCE.getSinglePointMatchRoad(lon, lat, (float) angle, true, true);
+//                Log.d("hy create cost", " " + (System.nanoTime() - cost) / 1000000);
+//            }
+//            if (singlePointRoadInfo != null && singlePointRoadInfo.getCoords() != null && !singlePointRoadInfo.getCoords().isEmpty()) {
+//                if(matchedPoint == null || matchedPoint.length == 0){
+//                    long cost = System.nanoTime();
+//                    matchedPoint = PointInterpolatorUtil.mergeToRoad(cacheData.getLongitude(), cacheData.getLatitude(), singlePointRoadInfo.getCoords());
+//                    Log.d("hy matchedPoint cost", " " + (System.nanoTime() - cost) / 1000000);
+//                    Log.d("hy uuid : " + cacheData.getUuid(), "道路经纬度 lon : " + matchedPoint[0] + " lat : " + matchedPoint[1] + " distance : " + matchedPoint[2] + " , 原数据 lon : " + lon + " lat : " + lat);
+//                }else{
+//                    if(matchedPoint[0] == 0 || matchedPoint[1] == 0){
+//                        cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
+//                    }else{
+//                        cacheData = cacheData.toBuilder().setLongitude(matchedPoint[0]).setLatitude(matchedPoint[1]).build();
+//                    }
+//                }
+//                lat = matchedPoint[1];
+//                lon = matchedPoint[0];
+//            } else {
+//                Log.d("hy uuid : " + cacheData.getUuid(), "未匹配到道路数据,使用原数据 lon : " + lon + " lat : " + lat);
+//            }
+                cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
         } else {
             //不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据
             lat = cacheData.getLatitude();
@@ -117,7 +149,6 @@ public class TrackObj {
         }
     }
 
-    //todo 对航向角做 10帧 原数据缓存，取众数
     private void calHeading() {
         double newDelta;
         ObjQueue lastObj;
@@ -148,14 +179,14 @@ public class TrackObj {
         if (relativeStatic()) {
             if (roadAngle != 0.0) {
                 CenterLine centerLine = CallerHDMapManager.INSTANCE.getCenterLineInfo(lon, lat, -1);
-                if (centerLine != null && centerLine.getAngle() != 0 && Math.abs(centerLine.getAngle() - circleQueue.getLastFrame().getHeading()) >= 10) {
-                    //Log.d("calHeading uuid : " + cacheData.getUuid(), " road angle : " + centerLine.getAngle() + " , last heading : " + circleQueue.getLastFrame().getHeading() + " queue size : " + circleQueue.size());
+                if (centerLine != null && centerLine.getAngle() != 0) {
                     cacheData = cacheData.toBuilder().setHeading(centerLine.getAngle()).build();
-                }else{
+                } else {
+//                    Log.d("hy uuid : " + cacheData.getUuid(), "未获取到道路航向,使用上一帧 : " + circleQueue.getLastFrame().getHeading());
                     cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build();
                 }
             } else {
-                //Log.d("calHeading uuid : " + cacheData.getUuid(), "使用上一帧heading : " + circleQueue.getLastFrame().getHeading());
+//                Log.d("hy uuid : " + cacheData.getUuid(), "未获取到道路航向,使用上一帧 : " + circleQueue.getLastFrame().getHeading());
                 cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build();
             }
 
diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt
index b61b1f1e96..d08ccfb392 100644
--- a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt
+++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt
@@ -18,6 +18,7 @@ class ChainConstant {
         const val CHAIN_LINK_LOG_WEB_SOCKET_TRAJECTORY = 4
         const val CHAIN_LINK_LOG_WEB_SOCKET_VEHICLE = 5
         const val CHAIN_LINK_LOG_WEB_SOCKET_TRAFFIC_LIGHT = 6
+        const val CHAIN_LINK_LOG_WEB_SOCKET_PLANNING_OBJECTS = 7
 
         const val CHAIN_LINK_LOG_ADAS_INIT = "-eagleInitStatus"
         const val CHAIN_LINK_LOG_ADAS_GNSS = "-adasWsGnssInfo"
@@ -26,6 +27,7 @@ class ChainConstant {
         const val CHAIN_LINK_LOG_ADAS_TRAJECTORY = "-adasWsTrajectory"
         const val CHAIN_LINK_LOG_ADAS_VEHICLE = "-adasWsVehicle"
         const val CHAIN_LINK_LOG_ADAS_TRAFFIC_LIGHT = "-adasWsTrafficLight"
+        const val CHAIN_LINK_LOG_ADAS_PLANNING_OBJECTS = "-adasWsPlanningObj"
 
         const val CHAIN_ALIAS_CODE_MULTI_CONNECT = "CHAIN_ALIAS_CODE_MULTI_CONNECT"
         const val CHAIN_ALIAS_CODE_ADAS_MESSAGE_CAR_CONFIG = "CHAIN_ALIAS_CODE_CAR_CONFIG"
@@ -39,6 +41,7 @@ class ChainConstant {
         const val CHAIN_ALIAS_CODE_ADAS_MESSAGE_AUTOPILOT_RECORD = "PAD_ADAS_MESSAGE_AUTOPILOT_RECORD"
         const val CHAIN_ALIAS_CODE_ADAS_MESSAGE_AUTOPILOT_VEHICLE = "PAD_ADAS_MESSAGE_AUTOPILOT_VEHICLE"
         const val CHAIN_ALIAS_CODE_ADAS_MESSAGE_AUTOPILOT_TRAFFIC_LIGHT = "PAD_ADAS_MESSAGE_AUTOPILOT_TRAFFIC_LIGHT"
+        const val CHAIN_ALIAS_CODE_ADAS_MESSAGE_PLANNING_OBJECTS = "CHAIN_ALIAS_CODE_ADAS_MESSAGE_PLANNING_OBJECTS"
         const val CHAIN_ALIAS_CODE_ADAS_MESSAGE_AUTOPILOT_WARN = "PAD_ADAS_MESSAGE_AUTOPILOT_WARN"
 
         const val CHAIN_ALIAS_CODE_CLOUD_CONNECT_FAIL = "CHAIN_ALIAS_CODE_CLOUD_CONNECT_FAIL"
diff --git 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
index 5306cda882..b9dad6c968 100644
--- 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
@@ -17,6 +17,11 @@ interface IMoGoAutopilotIdentifyListener {
      */
     fun onAutopilotIdentifyDataUpdate(trafficData: List<MessagePad.TrackedObject>?) {}
 
+    /**
+     * planning识别感知预警物体
+     */
+    fun onAutopilotIdentifyPlanningObj(planningObjects: List<MessagePad.PlanningObject>?){}
+
     /**
      * 报警信息
      *
diff --git 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
index 097adbfc58..5e606b0fa7 100644
--- 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
@@ -68,6 +68,15 @@ object CallerAutopilotIdentifyListenerManager : CallerBase() {
         }
     }
 
+    @Synchronized
+    fun invokeAutopilotIdentifyPlanningObj(planningObjects: List<MessagePad.PlanningObject>?){
+        M_AUTOPILOT_IDENTIFY_LISTENERS.forEach {
+            val tag = it.key
+            val listener = it.value
+            listener.onAutopilotIdentifyPlanningObj(planningObjects)
+        }
+    }
+
     /**
      * 报警信息 回调
      */
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 0184d1fabf..1c890d2aba 100644
--- a/libraries/mogo-adas-data/src/main/proto/message_pad.proto
+++ b/libraries/mogo-adas-data/src/main/proto/message_pad.proto
@@ -142,7 +142,7 @@ message AutopilotState
 message PlanningObject
 {
     uint32 uuid = 1; 
-    uint32 type = 2; //影响自车决策的类型, 和感知的障碍物类型不是一回事
+    uint32 type = 2; //影响自车决策的类型, 和感知的障碍物类型不是一回事   0是leading障碍物，1是避障和择机的障碍物
 }
 
 message PlanningObjects
diff --git a/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java b/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java
index 79aeacc109..206e780bd6 100644
--- a/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java
+++ b/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java
@@ -112,7 +112,7 @@ public class PointInterpolatorUtil {
      * @param x  target point lon
      * @param y  target point lat
      * @param x1 线段起点 lon
-     * @param y1 起点起点 lat
+     * @param y1 线段起点 lat
      * @param x2 线段终点 lon
      * @param y2 线段终点 lat
      * @return double[]{footLon,footLat,minDistance} if(footLon == -1) => no foot or foot not in line