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 ff9e9558c2..6a6b34a15f 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 @@ -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 ) diff --git 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 index ec6337e1c7..2b2b7f827d 100644 --- 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 mMarkersCaches = new ArrayMap<>(); + /** + * marker s2 cellId缓存队列，空间换时间 + */ + private final BiMap cellIdCaches = HashBiMap.create(); + + /** + * 记录每次实际绘制的交通元素UUID + */ + private final ArrayList 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 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); } diff --git 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 index c172895484..fe687f473d 100644 --- 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{" + diff --git a/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java b/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java index d136cdc65b..4f682ea3d5 100644 --- a/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java +++ b/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java @@ -759,11 +759,11 @@ public class AMapViewWrapper implements IMogoMapView, } else { mIsFirstLocated = true; mIsDelayed = false; - try { - CallerLogger.INSTANCE.d(M_MAP + TAG, "同步定位：" + GsonUtils.toJson(location)); - } catch (Exception e) { - e.printStackTrace(); - } +// try { +// CallerLogger.INSTANCE.d(M_MAP + TAG, "同步定位：" + GsonUtils.toJson(location)); +// } catch (Exception e) { +// e.printStackTrace(); +// } } } } diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java index 7a5c0defbe..a443af288a 100644 --- a/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java @@ -201,7 +201,7 @@ public class MogoRouteOverlayManager implements if (isExcept) { RouteOverlayDrawer.getInstance(mContext).setVisible(false); } - CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "--- onLocationChanged -- cost:" + (SystemClock.elapsedRealtime() - start) + " ms"); +// CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "--- onLocationChanged -- cost:" + (SystemClock.elapsedRealtime() - start) + " ms"); } }