fix bug

2022-06-10 18:13:43 +08:00
parent 650d43f914
commit 0f1019645a
5 changed files with 90 additions and 47 deletions
--- 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
@@ -120,7 +120,7 @@ class MoGoAdasListenerImpl : OnAdasListener {
                CallerMapUIServiceManager.getMapUIController()?.syncLocation2Map(gnssInfo)
                // 同步更新经纬度和系统时间至 AutoPilotStatusListener
                CallerAutoPilotStatusListenerManager.updateAutoPilotLatLon(
-                    gnssInfo.satelliteTime.toLong(),
+                    gnssInfo.satelliteTime.toLong() * 1000,
                    gnssInfo.longitude,
                    gnssInfo.latitude
                )
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java
@@ -1,15 +1,20 @@
 package com.mogo.eagle.core.function.map;

 import android.content.Context;
+import android.os.Build;
 import android.util.ArrayMap;
 import android.util.Log;

+import androidx.annotation.RequiresApi;
+
+import com.google.common.collect.BiMap;
+import com.google.common.collect.HashBiMap;
 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;
 import java.util.ArrayList;
 import java.util.Iterator;
 import java.util.List;
@@ -26,14 +31,24 @@ public class TrackManager {
        return TrackOwner.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;

    /**
     * marker缓存队列
     */
    private final ArrayMap<String, TrackObj> mMarkersCaches = new ArrayMap<>();

+    /**
+     * marker s2 cellId缓存队列，空间换时间
+     */
+    private final BiMap<String, Long> cellIdCaches = HashBiMap.create();
+
+    /**
+     * 记录每次实际绘制的交通元素UUID
+     */
+    private final ArrayList<String> trafficDataUuidList = new ArrayList<>();
+
    /**
     * 过滤后的数据集合
     */
@@ -58,32 +73,55 @@ public class TrackManager {
                mFilterTrafficData.add(data);
            } else {
                trackObj = new TrackObj(data);
-                //todo 判断是否有重合元素 google s2
+                // 判断是否有重合元素 google s2
+//                if (cellIdCaches.containsValue(trackObj.getCellIdPos())) {
+//                    String findSameValue = cellIdCaches.inverse().get(trackObj.getCellIdPos());
+//                    Log.d("0609", "uuid : " + findSameValue + " 与新感知物 : " + uuid + " , 出现相同pos : " + trackObj.getCellIdPos());
+//                    data = data.toBuilder().setUuid(Integer.parseInt(findSameValue)).build();
+//                    data = trackObj.updateObj(data);
+//                    mFilterTrafficData.add(data);
+//                }
            }
+            cellIdCaches.forcePut(uuid, trackObj.getCellIdPos());
            mMarkersCaches.put(uuid, trackObj);
+            trafficDataUuidList.add(uuid);
        }
        return mFilterTrafficData;
    }

-    public void clearCache(Context mContext) {
-        Iterator it = mMarkersCaches.keySet().iterator();
-        while (it.hasNext()) {
-            String key = (String) it.next();
-            TrackObj trackObj = mMarkersCaches.get(key);
-            if (CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime() > 1000) {
-                Log.d("track","clearCache uuid : " + key);
-                mMarkersCaches.remove(key);
-                MogoMarkerManager.getInstance(mContext)
-                        .removeMarker(key);
+    @RequiresApi(api = Build.VERSION_CODES.N)
+    public void clearCache(Context mContext, List<MessagePad.TrackedObject> resultList) {
+        //清除缓存
+        for (MessagePad.TrackedObject data : resultList) {
+            if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
+                trafficDataUuidList.remove("" + data.getUuid());
            }
        }
+        trafficDataUuidList.forEach(uuid -> {
+            mMarkersCaches.remove(uuid);
+            cellIdCaches.remove(uuid);
+        });
+//        Iterator it = mMarkersCaches.keySet().iterator();
+//        while (it.hasNext()) {
+//            String key = (String) it.next();
+//            TrackObj trackObj = mMarkersCaches.get(key);
+//            if (trackObj != null && Math.abs(CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime()) >= 500) {
+//                Log.d("track", "clearCache uuid : " + key);
+//                mMarkersCaches.remove(key);
+//                cellIdCaches.remove(key);
+//                MogoMarkerManager.getInstance(mContext)
+//                        .removeMarker(key);
+//            }
+//        }
    }

    public void clearAll(Context mContext) {
+        trafficDataUuidList.clear();
        Iterator it = mMarkersCaches.keySet().iterator();
        while (it.hasNext()) {
            String key = (String) it.next();
            mMarkersCaches.remove(key);
+            cellIdCaches.remove(key);
            MogoMarkerManager.getInstance(mContext)
                    .removeMarker(key);
        }
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java
@@ -1,13 +1,12 @@
 package com.mogo.eagle.core.function.map;

-import static com.mogo.eagle.core.function.map.TrackManager.DISTANCE;
+import static com.mogo.eagle.core.function.map.TrackManager.LIMIT_SPEED;

 import android.util.Log;

 import com.mogo.eagle.core.utilcode.geometry.S2CellId;
 import com.mogo.eagle.core.utilcode.geometry.S2LatLng;

-import java.math.BigDecimal;
 import java.util.List;

 import mogo.telematics.pad.MessagePad;
@@ -21,14 +20,16 @@ public class TrackObj {
    private long recentlyTime;  //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除
    private double headingDelta; //航向角德尔塔
    private double typeWeight;   //类型权重
-    private String uuid;
+    private double lat;
+    private double lon;

    public TrackObj(MessagePad.TrackedObject data) {
-        uuid = "" + data.getUuid();
        kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
        circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getType()));
-        recentlyTime = BigDecimal.valueOf(data.getSatelliteTime()).longValue();
-        S2LatLng s2LatLng = S2LatLng.fromDegrees(data.getLatitude(), data.getLongitude());
+        recentlyTime = Double.valueOf(data.getSatelliteTime() * 1000).longValue();
+        lat = data.getLatitude();
+        lon = data.getLongitude();
+        s2LatLng = S2LatLng.fromDegrees(data.getLatitude(), data.getLongitude());
        s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22); //需要验证22前后
    }

@@ -36,13 +37,13 @@ public class TrackObj {

    //先处理kalman数据，将经纬度校准后，放入缓存队列，然后基于后序策略将各个项进行校准
    public MessagePad.TrackedObject updateObj(MessagePad.TrackedObject data) {
-        cacheData = data.toBuilder().build();
-        recentlyTime = BigDecimal.valueOf(data.getSatelliteTime()).longValue();//todo 毫秒没有了
+        cacheData = data;

        correct();

+        recentlyTime = Double.valueOf(data.getSatelliteTime() * 1000).longValue();
        circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), cacheData.getType()));
-        return cacheData.toBuilder().build();
+        return cacheData;
    }

    private void correct() {
@@ -53,27 +54,27 @@ public class TrackObj {

    private void calLoc() {
        //距离计算,位置修正
-        double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude());
-        if (s2LatLng != null) {
-            double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[0], lonLat[1])).distance(DISTANCE);
-//            DF.format(distance)
-            //速度小于0.5m/s,并且距离在15米范围内，则认为是相对静止状态(注意调整阈值)，不更新缓存点信息
-            if (cacheData.getSpeed() < 0.5 && distance < 15) {
-                Log.i("track", "uuid : " + uuid + " , 静止物体相对上一帧 distance : " + distance + " lon : " + s2LatLng.latDegrees() + " lat : " + s2LatLng.lngDegrees());
-                cacheData = cacheData.toBuilder().setLongitude(s2LatLng.latDegrees()).setLatitude(s2LatLng.lngDegrees()).build();
-            } else {
-                //不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据
-                s2LatLng = S2LatLng.fromDegrees(lonLat[0], lonLat[1]);
-                s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
-                cacheData = cacheData.toBuilder().setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
-            }
+        //todo  bus250  taxi上测试下面注释掉内容
+        //        double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude());
+//        double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[1], lonLat[0])).distance(DISTANCE);
+//        double calDistance = (cacheData.getSpeed() * (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime)) / 1000.0;
+        //速度小于0.5m/s,并且距离在计算合理范围内超出2倍，则认为是相对静止状态(注意调整阈值)，不更新缓存点信息
+//        if (cacheData.getSpeed() < LIMIT_SPEED || calDistance * 2 < distance) {
+//        Log.i("uuid : " + uuid , cacheData.getSpeed() < LIMIT_SPEED ? " 减速到静止" : "拉回来了 cal :" + calDistance + " , distance  : " + distance +  " , lon : "  + lonLat[0]  + " , lat : " + lonLat[1]);
+        if (cacheData.getSpeed() < LIMIT_SPEED) {
+//            cacheData = cacheData.toBuilder().setLongitude(s2LatLng.lngDegrees()).setLatitude(s2LatLng.latDegrees()).build();
+            cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
        } else {
-            //首次更新
-            s2LatLng = S2LatLng.fromDegrees(lonLat[0], lonLat[1]);
+            //不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据
+            lat = cacheData.getLatitude();
+            lon = cacheData.getLongitude();
+            s2LatLng = S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude());
            s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
+//            cacheData = cacheData.toBuilder().setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
        }
    }

+    //todo 对航向角做 10帧 原数据缓存，取众数
    private void calHeading() {
        double speedAverage;
        double newDelta;
@@ -105,14 +106,14 @@ public class TrackObj {
            headingDelta = newDelta;
        }
        //更正数据,速度小于0.5使用上一帧数据
-        if (speedAverage < 0.5) {
+        if (speedAverage < LIMIT_SPEED && circleQueue.size() != 1) {
            cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build();
        }
        //速度大于1.5并出现大幅度转向使用缓存帧和delta数据
        if (cacheData.getSpeed() > 1.5 && newDelta > 10 && headingDelta != 0.0) {
+            Log.i("0609", "uuid : " + cacheData.getUuid() + " 修正航向角 last : " + lastObj.getHeading() + " , 增益 : " + headingDelta);
            cacheData = cacheData.toBuilder().setHeading(lastObj.getHeading() + headingDelta).build();
        }
-
    }

    private void calType() {
@@ -123,6 +124,10 @@ public class TrackObj {
        return recentlyTime;
    }

+    public long getCellIdPos() {
+        return s2CellId.pos();
+    }
+
    @Override
    public String toString() {
        return "TrackObj{" +