merge

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java

@@ -87,10 +87,11 @@ public class IdentifyDataDrawer {
             return;
         }
 
+        long start = System.currentTimeMillis();
         //清除缓存
         for (MessagePad.TrackedObject data : resultList) {
             if(trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())){
-                trafficDataUuidList.remove(data.getUuid());
+                trafficDataUuidList.remove("" + data.getUuid());
             }
         }
         trafficDataUuidList.forEach(uuid -> {
@@ -99,11 +100,16 @@ public class IdentifyDataDrawer {
 
         CallerLogger.INSTANCE.d(M_HMI + "arrow47", "origin data size : " + resultList.size());
         ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
+
+        long cost = System.currentTimeMillis() - start;
+        CallerLogger.INSTANCE.d(M_HMI + "arrow47", "cost : " + cost);
+
         if (filterList.size() > 0) {
             // 绘制新数据
             MogoMarkerManager.getInstance(mContext)
                     .updateBatchMarkerPosition(filterList);
         }
+
         CallerLogger.INSTANCE.d(M_HMI + "arrow47", "first data size  : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size());
         // 首次未添加的感知物在调用完绘制方法后再塞入cache map
         mMarkersCaches.putAll(firstData);

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTest.java

@@ -0,0 +1,203 @@
+package com.mogo.eagle.core.function.map;
+
+import static com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.M_HMI;
+
+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.function.call.map.CallerMapLocationListenerManager;
+import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
+import com.mogo.eagle.core.utilcode.util.UiThreadHandler;
+import com.mogo.map.MogoMap;
+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 IdentifyDataDrawerTest {
+    private static final String TAG = "IdentifyDataDrawer";
+
+    protected final Context mContext;
+    private static volatile IdentifyDataDrawerTest sInstance;
+
+    /**
+     * 上一帧数据的缓存
+     */
+    private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
+
+    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<>();
+
+    private IdentifyDataDrawerTest() {
+        mContext = AbsMogoApplication.getApp();
+        addPreVehicleModel();
+    }
+
+    public static IdentifyDataDrawerTest getInstance() {
+        if (sInstance == null) {
+            synchronized (IdentifyDataDrawerTest.class) {
+                if (sInstance == null) {
+                    sInstance = new IdentifyDataDrawerTest();
+                }
+            }
+        }
+        return sInstance;
+    }
+
+    public synchronized void release() {
+        sInstance = null;
+    }
+
+    /**
+     * 渲染 adas 识别的数据
+     *
+     * @param resultList adas感知融合数据
+     */
+    @SuppressLint("NewApi")
+    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(mContext)
+                    .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;
+            }
+            double heading = kalmanFilter(data);
+            MessagePad.TrackedObject correctData = data.toBuilder().setHeading(heading).build();
+            mFilterTrafficData.add(correctData);
+            //更新已存在的感知物体数据
+            mMarkersCaches.put("" + correctData.getUuid(), correctData);
+            trafficDataUuidList.add("" + correctData.getUuid());
+        }
+        return mFilterTrafficData;
+    }
+
+    private double kalmanFilter(MessagePad.TrackedObject data) {
+        String uuid = "" + data.getUuid();
+        if (algoCache.containsKey(uuid)) {
+            Object o = algoCache.get(uuid);
+            KalmanFilter kf = (KalmanFilter) o;
+            assert kf != null;
+            double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude());
+            algoCache.put(uuid, kf);
+            MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
+            assert cacheTrackObj != null;
+            double heading = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]);
+            CallerLogger.INSTANCE.d(M_HMI + "arrow47", " uuid : " + uuid + " , origin heading : " + data.getHeading() + " , correct heading : " + heading + "  ---- " + (data.getHeading() - heading));
+            return Math.abs((data.getHeading() - heading)) >= 180 ? heading : data.getHeading();
+        } else {
+            algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.00005));
+            return data.getHeading();
+        }
+    }
+
+    /**
+     * 清除旧的 marker 数据
+     */
+    public void clearOldMarker() {
+        for (String uuid : trafficDataUuidList) {
+            MogoMarkerManager.getInstance(mContext)
+                    .removeMarker(uuid);
+        }
+        trafficDataUuidList.clear();
+    }
+
+    private void addPreVehicleModel() {
+        CallerLogger.INSTANCE.d(TAG, "添加感知模型到地图中……");
+        addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI, "添加感知模型到地图中……preVehicleStrWeiZhi=");
+
+        addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_PEOPLE, "添加感知模型到地图中……preVehicleStrPeople=");
+
+        addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE, "添加感知模型到地图中……preVehicleStrBicycle=");
+
+        addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE, "添加感知模型到地图中……preVehicleStrTaChe=");
+
+        addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO, "添加感知模型到地图中……preVehicleStrMoto=");
+
+        addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS, "添加感知模型到地图中……preVehicleStrBus=");
+
+        addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK, "添加感知模型到地图中……preVehicleStrTruck=");
+    }
+
+    /**
+     * 添加模型到地图中
+     *
+     * @param typeTrafficIdWeiZhi
+     * @param s
+     */
+    private void addPreVehicleModelWeiZhi(TrafficTypeEnum typeTrafficIdWeiZhi, String s) {
+        String preVehicleStrWeiZhi = MogoMarkerManager.getInstance(mContext)
+                .addPreVehicleModel(typeTrafficIdWeiZhi.getType(),
+                        typeTrafficIdWeiZhi.getTraffic3DIconId());
+        CallerLogger.INSTANCE.d(TAG, s + preVehicleStrWeiZhi);
+
+        if (preVehicleStrWeiZhi == null) {
+            UiThreadHandler.postDelayed(new Runnable() {
+                @Override
+                public void run() {
+                    CallerLogger.INSTANCE.w(TAG, "添加感知模型到地图中失败，尝试重复添加……");
+                    addPreVehicleModelWeiZhi(typeTrafficIdWeiZhi, s);
+                }
+            }, 1000L);
+        }
+    }
+
+}

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java

@@ -0,0 +1,34 @@
+package com.mogo.eagle.core.function.map;
+
+public class KalmanFilter {
+    private final double q = 1.0E-6D;
+    double r = 5.0E-5D;
+    double[][] xhat = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
+    double[][] p = new double[][]{{1.0D, 1.0D}, {0.0D, 0.0D}};
+    double[][] xhatminus = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
+    double[][] pMinus = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
+    double[][] k = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
+    int idx = 1;
+
+    public KalmanFilter(double lon, double lat, double r) {
+        this.xhat[0][0] = lon;
+        this.xhat[0][1] = lat;
+        this.r = r;
+    }
+
+    public double[] filter(double lon, double lat) {
+        for(int i = 0; i < 2; ++i) {
+            this.xhatminus[this.idx][i] = this.xhat[1 - this.idx][i];
+            this.pMinus[this.idx][i] = this.p[1 - this.idx][i] + 1.0E-6D;
+            this.k[this.idx][i] = this.pMinus[this.idx][i] / (this.pMinus[this.idx][i] + this.r);
+            double value = i == 0 ? lon : lat;
+            this.xhat[this.idx][i] = this.xhatminus[this.idx][i] + this.k[this.idx][i] * (value - this.xhatminus[this.idx][i]);
+            this.p[this.idx][i] = (1.0D - this.k[this.idx][i]) * this.pMinus[this.idx][i];
+        }
+
+        double lon1 = this.xhat[this.idx][0];
+        double lat1 = this.xhat[this.idx][1];
+        this.idx = 1 - this.idx;
+        return new double[]{lon1, lat1};
+    }
+}

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt

@@ -41,9 +41,9 @@ class MapIdentifySubscriber private constructor() : IMoGoSubscriber, IMoGoAutopi
     override fun onAutopilotIdentifyDataUpdate(trafficData: List<TrackedObject>?) {
         try {
             if (FunctionBuildConfig.isDrawIdentifyData) {
-                ThreadUtils.getSinglePool().execute { IdentifyDataDrawer.getInstance().renderAdasRecognizedResult(trafficData) }
+                ThreadUtils.getSinglePool().execute { IdentifyDataDrawerTest.getInstance().renderAdasRecognizedResult(trafficData) }
             } else {
-                IdentifyDataDrawer.getInstance().clearOldMarker()
+                IdentifyDataDrawerTest.getInstance().clearOldMarker()
             }
         } catch (e: Exception) {
             e.printStackTrace()

libraries/mogo-map-api/src/main/java/com/mogo/map/IMogoMap.java

@@ -1,6 +1,5 @@
 package com.mogo.map;
 
-import com.mogo.eagle.core.data.traffic.TrafficData;
 import com.mogo.map.marker.IMogoMarker;
 import com.mogo.map.marker.MogoMarkerOptions;
 import com.mogo.map.overlay.IMogoPolyline;

libraries/mogo-map-api/src/main/java/com/mogo/map/uicontroller/IMogoMapUIController.java

@@ -352,4 +352,10 @@ public interface IMogoMapUIController {
      * 设置地图视线角度
      */
     void setMapDAngle(float angle);
+
+    /**
+     * 获取行车方向
+     * @return
+     */
+    float getAngle(double startLon, double startLat, double endLon, double endLat);
 }

libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java

@@ -1079,4 +1079,9 @@ public class AMapViewWrapper implements IMogoMapView,
         mMapView.getMapAutoViewHelper().setMapDAngle(angle);
     }
 
+    @Override
+    public float getAngle(double startLon, double startLat, double endLon, double endLat) {
+        return MapAutoApi.INSTANCE.getAngle(startLon,startLat,endLon,endLat);
+    }
+
 }

libraries/mogo-map/src/main/java/com/mogo/map/MogoMapUIController.java

@@ -454,4 +454,13 @@ public class MogoMapUIController implements IMogoMapUIController {
             mDelegate.setMapDAngle(angle);
         }
     }
+
+    @Override
+    public float getAngle(double startLon, double startLat, double endLon, double endLat) {
+        initDelegate();
+        if (mDelegate != null) {
+            return mDelegate.getAngle(startLon, startLat, endLon, endLat);
+        }
+        return 0.0f;
+    }
 }

libraries/mogo-map/src/main/java/com/mogo/map/uicontroller/AMapUIController.java

@@ -374,4 +374,12 @@ public class AMapUIController implements IMogoMapUIController {
             mClient.setMapDAngle(angle);
         }
     }
+
+    @Override
+    public float getAngle(double startLon, double startLat, double endLon, double endLat) {
+        if (mClient != null) {
+            return mClient.getAngle(startLon,startLat,endLon,endLat);
+        }
+        return 0;
+    }
 }