From 20f7c763217a19db75109efc1aa7c2c69ac40dfb Mon Sep 17 00:00:00 2001 From: zhongchao Date: Thu, 16 Jun 2022 16:01:36 +0800 Subject: [PATCH] wait to merge --- .../function/map/IdentifyDataDrawerTrack.java | 213 ------------------ .../function/map/MapIdentifySubscriber.kt | 5 +- .../map/{ => identify}/CircleQueue.java | 4 +- .../core/function/map/identify/Identify.java | 12 + .../identify/IdentifyBeautifyDataDrawer.java | 65 ++++++ .../map/identify/IdentifyFactory.java | 52 +++++ .../IdentifyOriginDataDrawer.java} | 106 ++++----- .../map/{ => identify}/KalmanFilter.java | 2 +- .../function/map/{ => identify}/ObjQueue.java | 2 +- .../map/{ => identify}/TrackManager.java | 42 +++- .../function/map/{ => identify}/TrackObj.java | 48 ++-- 11 files changed, 248 insertions(+), 303 deletions(-) delete mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java rename core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/{ => identify}/CircleQueue.java (89%) create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.java create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.java create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.java rename core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/{IdentifyDataDrawer.java => identify/IdentifyOriginDataDrawer.java} (50%) rename core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/{ => identify}/KalmanFilter.java (96%) rename core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/{ => identify}/ObjQueue.java (94%) rename core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/{ => identify}/TrackManager.java (74%) rename core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/{ => identify}/TrackObj.java (81%) diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java deleted file mode 100644 index 3edbc8c530..0000000000 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java +++ /dev/null @@ -1,213 +0,0 @@ -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.utilcode.mogo.logger.CallerLogger; -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 IdentifyDataDrawerTrack { - private static final String TAG = "IdentifyDataDrawer"; - - protected final Context mContext; - private static volatile IdentifyDataDrawerTrack sInstance; - - /** - * 上一帧数据的缓存 - */ - private static final ConcurrentHashMap mMarkersCaches = new ConcurrentHashMap<>(); - - /** - * kalman缓存数据 - */ - private static final ConcurrentHashMap algoCache = new ConcurrentHashMap<>(); - - /** - * 记录每次实际绘制的交通元素UUID - */ - private final ArrayList trafficDataUuidList = new ArrayList<>(); - /** - * 过滤后的数据集合 - */ - private final ArrayList mFilterTrafficData = new ArrayList<>(); - - private IdentifyDataDrawerTrack() { - mContext = AbsMogoApplication.getApp(); - } - - public static IdentifyDataDrawerTrack getInstance() { - if (sInstance == null) { - synchronized (IdentifyDataDrawerTrack.class) { - if (sInstance == null) { - sInstance = new IdentifyDataDrawerTrack(); - } - } - } - return sInstance; - } - - public synchronized void release() { - sInstance = null; - } - - /** - * 渲染 adas 识别的数据 - * - * @param resultList adas感知融合数据 - */ - @SuppressLint("NewApi") - public void renderAdasRecognizedResult(List 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 filterList = filterTrafficData(resultList); - - if (filterList.size() > 0) { - // 绘制新数据 - MogoMarkerManager.getInstance(mContext) - .updateBatchMarkerPosition(filterList); - } - } - - /** - * 数据过滤器 - * - * @return 过滤后的数据集合 - */ - private ArrayList filterTrafficData(List 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) { //todo 代码不要推到云端，本地测试即可！！！！！！！！！！ - MessagePad.TrackedObject correctData; - correctData = kalmanCorrectData(data); - mFilterTrafficData.add(correctData); - //更新已存在的感知物体数据 - mMarkersCaches.put(uuid, correctData); - } else { - mMarkersCaches.put(uuid, data); - } - trafficDataUuidList.add(uuid); - } - return mFilterTrafficData; - } - - //todo 相信滤波的定位点做验证，将原始data修改经纬度和航向角返回 - private MessagePad.TrackedObject kalmanCorrectData(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; - //todo 代码不要推到云端，本地测试即可！！！！！！！！！！ - if (data.getSpeed() >= 1.5 && (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE.getType())) { - double kalH = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]); - if ((cacheTrackObj.getHeading() / 360 > 0.75 && data.getHeading() / 360 < 0.25) || (cacheTrackObj.getHeading() / 360 < 0.25 && data.getHeading() / 360 > 0.75)) { - return data; - } - if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 165 && Math.abs(cacheTrackObj.getHeading() - data.getHeading()) < 195) { - return data; - } - if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 40 && (Math.abs(kalH - cacheTrackObj.getHeading())) > 40) { - if (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) { - CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , cacheH : " + cacheTrackObj.getHeading() + " , origin H : " + data.getHeading() + " , kalH H : " + kalH + " , 使用卡尔曼" + " ， speed : " + data.getSpeed()); - } - return data.toBuilder().setHeading(kalH).build(); - } - if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 40 && (Math.abs(kalH - data.getHeading())) > 40) { //存在前后帧误差 - double correct = Math.abs(cacheTrackObj.getHeading() - kalH) - Math.abs(data.getHeading() - kalH) > 0 ? data.getHeading() : cacheTrackObj.getHeading(); - if (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) { - CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , cacheH : " + cacheTrackObj.getHeading() + " , origin H : " + data.getHeading() + " , correct H : " + correct + " , 使用 : " + (Math.abs(cacheTrackObj.getHeading() - kalH) - Math.abs(data.getHeading() - kalH) > 0 ? "当前帧" : "缓存帧" + " ， speed : " + data.getSpeed())); - } - return data.toBuilder().setHeading(correct).build(); - } - } else if (data.getSpeed() <= 0.5) { - double roadAngle = MogoMap.getInstance().getMogoMap().getUIController().getRoadAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), -1); - double result = roadAngle != 0 ? roadAngle : cacheTrackObj.getHeading(); - CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType()," uuid : " + uuid + " , <0.5 result : " + result); - return data.toBuilder().setHeading(cacheTrackObj.getHeading()).setLongitude(cacheTrackObj.getLongitude()).setLatitude(cacheTrackObj.getLatitude()).build(); - } else { - return data; - } - } else { - algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005)); - return data; - } - return data; - } - - private double getAngle(double lat_a, double lng_a, double lat_b, double lng_b) { - double y = Math.sin(lng_b - lng_a) * Math.cos(lat_b); - double x = Math.cos(lat_a) * Math.sin(lat_b) - Math.sin(lat_a) * Math.cos(lat_b) * Math.cos(lng_b - lng_a); - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - if (bearing < 0) { - bearing = bearing + 360; - } - return 360 - bearing; - } - - /** - * 清除旧的 marker 数据 - */ - public void clearOldMarker() { - for (String uuid : trafficDataUuidList) { - MogoMarkerManager.getInstance(mContext) - .removeMarker(uuid); - } - trafficDataUuidList.clear(); - } -} - - - 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 5eb0df67c8..fe802a2d14 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 @@ -4,6 +4,7 @@ import com.mogo.eagle.core.data.config.FunctionBuildConfig import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotIdentifyListener import com.mogo.eagle.core.function.api.base.IMoGoSubscriber import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager +import com.mogo.eagle.core.function.map.identify.IdentifyFactory import com.mogo.eagle.core.utilcode.util.ThreadUtils import mogo.telematics.pad.MessagePad import mogo.telematics.pad.MessagePad.TrackedObject @@ -41,9 +42,9 @@ class MapIdentifySubscriber private constructor() : IMoGoSubscriber, IMoGoAutopi override fun onAutopilotIdentifyDataUpdate(trafficData: List?) { try { if (FunctionBuildConfig.isDrawIdentifyData) { - ThreadUtils.getSinglePool().execute { IdentifyDataDrawer.getInstance().renderAdasRecognizedResult(trafficData) } + ThreadUtils.getSinglePool().execute { IdentifyFactory.getInstance().renderAdasRecognizedResult(trafficData) } } else { - IdentifyDataDrawer.getInstance().clearOldMarker() + IdentifyFactory.getInstance().clearOldMarker() } } catch (e: Exception) { e.printStackTrace() diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java similarity index 89% rename from core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java rename to core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java index 921a8d829e..7e63ad99fb 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java @@ -1,4 +1,6 @@ -package com.mogo.eagle.core.function.map; +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; 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 new file mode 100644 index 0000000000..515f2c428f --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.java @@ -0,0 +1,12 @@ +package com.mogo.eagle.core.function.map.identify; + +import java.util.List; + +import mogo.telematics.pad.MessagePad; + +public interface Identify { + + void renderAdasRecognizedResult(List resultList); + + void clearOldMarker(); +} 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 new file mode 100644 index 0000000000..cc58abb4fb --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.java @@ -0,0 +1,65 @@ +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 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 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/IdentifyFactory.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.java new file mode 100644 index 0000000000..d3aa7f2a46 --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.java @@ -0,0 +1,52 @@ +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 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/IdentifyDataDrawer.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.java similarity index 50% rename from core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java rename to core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.java index aa6ed80d63..1aabe3f7d7 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.java @@ -1,17 +1,15 @@ -package com.mogo.eagle.core.function.map; +package com.mogo.eagle.core.function.map.identify; + import android.annotation.SuppressLint; import android.content.Context; -import android.util.Log; 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.AppIdentityModeUtils; import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger; import com.mogo.map.MogoMarkerManager; import com.mogo.module.common.MogoApisHandler; -import com.mogo.module.common.constants.AdasRecognizedType; import java.util.ArrayList; import java.util.List; @@ -24,79 +22,64 @@ import mogo.telematics.pad.MessagePad; * @date 2021/10/19 10:45 上午 * 域控制器识别信息绘制 */ -public class IdentifyDataDrawer { +public class IdentifyOriginDataDrawer implements Identify{ private static final String TAG = "IdentifyDataDrawer"; - protected final Context mContext; - private static volatile IdentifyDataDrawer sInstance; - /** * 上一帧数据的缓存 */ private static final ConcurrentHashMap mMarkersCaches = new ConcurrentHashMap<>(); + /** + * kalman缓存数据 + */ + private static final ConcurrentHashMap algoCache = new ConcurrentHashMap<>(); + + /** + * 记录每次实际绘制的交通元素UUID + */ + private final ArrayList trafficDataUuidList = new ArrayList<>(); /** * 过滤后的数据集合 */ private final ArrayList mFilterTrafficData = new ArrayList<>(); - private IdentifyDataDrawer() { - mContext = AbsMogoApplication.getApp(); - } - - public static IdentifyDataDrawer getInstance() { - if (sInstance == null) { - synchronized (IdentifyDataDrawer.class) { - if (sInstance == null) { - sInstance = new IdentifyDataDrawer(); - } - } - } - return sInstance; - } - - public synchronized void release() { - sInstance = null; - } - /** * 渲染 adas 识别的数据 * * @param resultList adas感知融合数据 */ @SuppressLint("NewApi") + @Override public void renderAdasRecognizedResult(List resultList) { -// if (resultList == null || resultList.isEmpty()) { -// TrackManager.getInstance().clearAll(mContext); -// CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……"); -// return; -// } -// -// if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) { -// TrackManager.getInstance().clearAll(mContext); -// CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式"); -// return; -// } + if (resultList == null || resultList.isEmpty()) { + clearOldMarker(); + CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……"); + 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); -// }); + if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) { + clearOldMarker(); + CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式"); + return; + } - TrackManager.getInstance().clearCache(mContext); + //清除缓存 + 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 filterList = filterTrafficData(resultList); -// ArrayList filterList = filterTrafficData(resultList); - ArrayList filterList = TrackManager.getInstance().filterTrafficData(resultList); - Log.i("0608arrow","size : " + filterList.size() + " , filterList : " + filterList.toString()); if (filterList.size() > 0) { // 绘制新数据 - MogoMarkerManager.getInstance(mContext) + MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) .updateBatchMarkerPosition(filterList); } } @@ -108,24 +91,26 @@ public class IdentifyDataDrawer { */ private ArrayList filterTrafficData(List 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) { - double limitSpeed = AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode) ? 0.7 : 0.5; - if (data.getSpeed() < limitSpeed) { - data = data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build(); + 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; } @@ -133,8 +118,15 @@ public class IdentifyDataDrawer { /** * 清除旧的 marker 数据 */ + @Override public void clearOldMarker() { - TrackManager.getInstance().clearAll(mContext); + 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/KalmanFilter.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/KalmanFilter.java similarity index 96% rename from core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java rename to core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/KalmanFilter.java index 9552063ede..5c55acd4ee 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/KalmanFilter.java @@ -1,4 +1,4 @@ -package com.mogo.eagle.core.function.map; +package com.mogo.eagle.core.function.map.identify; public class KalmanFilter { private final double q = 1.0E-6D; diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java similarity index 94% rename from core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java rename to core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java index f34d5599e4..2abe3db7af 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java @@ -1,4 +1,4 @@ -package com.mogo.eagle.core.function.map; +package com.mogo.eagle.core.function.map.identify; public class ObjQueue { 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/identify/TrackManager.java similarity index 74% rename from core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java rename to core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java index 994f4a96e6..916e1b9082 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/identify/TrackManager.java @@ -1,14 +1,14 @@ -package com.mogo.eagle.core.function.map; +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; import com.google.common.collect.BiMap; 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; @@ -46,6 +46,8 @@ public class TrackManager { */ private final BiMap cellIdCaches = HashBiMap.create(); + private final ArrayMap recentCaches = new ArrayMap<>(); + /** * 记录每次实际绘制的交通元素UUID */ @@ -78,8 +80,8 @@ public class TrackManager { // 判断是否有重合元素 google s2 if (cellIdCaches.containsValue(trackObj.getCellIdPos())) { String findSameValue = cellIdCaches.inverse().get(trackObj.getCellIdPos()); - if(data.getUuid() - Integer.parseInt(findSameValue) > 0){ - Log.d("0609", "uuid : " + findSameValue + " 与新感知物 : " + uuid + " , 出现相同pos : " + trackObj.getCellIdPos()); + if (data.getUuid() - Integer.parseInt(findSameValue) > 0) { +// Log.d("0609", "uuid : " + findSameValue + " 与新感知物 : " + uuid + " , 出现相同pos : " + trackObj.getCellIdPos()); uuid = findSameValue; data = data.toBuilder().setUuid(Integer.parseInt(findSameValue)).build(); data = trackObj.updateObj(data); @@ -91,11 +93,24 @@ public class TrackManager { mMarkersCaches.put(uuid, trackObj); trafficDataUuidList.add(uuid); } + //todo 将上次没被删除掉物体加入集合，造成延迟删除，对运动物体不友好 +// Iterator it = recentCaches.keySet().iterator(); +// while (it.hasNext()) { +// String key = (String) it.next(); +// TrackObj trackObj = recentCaches.get(key); +// if(trackObj == null){ +// continue; +// } +// if(!trackObj.relativeStatic()){ +// continue; +// } +// mFilterTrafficData.add(trackObj.getCache()); +// } return mFilterTrafficData; } @RequiresApi(api = Build.VERSION_CODES.N) - public void clearCache(Context mContext, List resultList) { + public void clearCache(List resultList) { //清除缓存 for (MessagePad.TrackedObject data : resultList) { if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) { @@ -106,30 +121,33 @@ public class TrackManager { mMarkersCaches.remove(uuid); cellIdCaches.remove(uuid); }); - - //todo bus存在时间回溯，将id重置，会有id复用问题，导致鹰眼展示元素缺少，此处暂时不开启。 + //todo bus存在时间回溯，将id重置，会有id复用问题，导致鹰眼展示元素缺少 // 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()) >= 1000) { -// Log.d("track", "clearCache uuid : " + key); +// if (trackObj != null && Math.abs(CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime()) >= 2000) { +//// Log.d("track", "clearCache uuid : " + key + " ， time : " + (CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime())); // mMarkersCaches.remove(key); // cellIdCaches.remove(key); -// MogoMarkerManager.getInstance(mContext) +// recentCaches.remove(key); +// MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) // .removeMarker(key); +// } else { +// recentCaches.put(key, trackObj); // } // } } - public void clearAll(Context mContext) { + public void clearAll() { trafficDataUuidList.clear(); Iterator it = mMarkersCaches.keySet().iterator(); while (it.hasNext()) { String key = (String) it.next(); mMarkersCaches.remove(key); cellIdCaches.remove(key); - MogoMarkerManager.getInstance(mContext) + recentCaches.remove(key); + MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) .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/identify/TrackObj.java similarity index 81% rename from core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java rename to core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java index 48bf638874..e452320a65 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/identify/TrackObj.java @@ -1,10 +1,7 @@ -package com.mogo.eagle.core.function.map; +package com.mogo.eagle.core.function.map.identify; -import static com.mogo.eagle.core.function.map.TrackManager.DF; -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 static com.mogo.eagle.core.function.map.identify.TrackManager.DISTANCE; +import static com.mogo.eagle.core.function.map.identify.TrackManager.LIMIT_SPEED; import com.mogo.eagle.core.function.call.map.CallerHDMapManager; import com.mogo.eagle.core.utilcode.geometry.S2CellId; @@ -23,6 +20,7 @@ public class TrackObj { private S2CellId s2CellId; //s2 id权重 private S2LatLng s2LatLng; //s2 经纬度 private long recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除 + private double roadAngle; //道路航向 private double headingDelta; //航向角德尔塔 private double typeWeight; //类型权重 private double lat; @@ -37,6 +35,10 @@ public class TrackObj { lon = data.getLongitude(); s2LatLng = S2LatLng.fromDegrees(data.getLatitude(), data.getLongitude()); s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22); //需要验证22前后 + CenterLine centerLine = CallerHDMapManager.INSTANCE.getCenterLineInfo(lon, lat, -1); + if (centerLine != null && centerLine.getAngle() != 0) { + roadAngle = centerLine.getAngle(); + } } private MessagePad.TrackedObject cacheData; @@ -48,7 +50,7 @@ public class TrackObj { correct(); recentlyTime = Double.valueOf(data.getSatelliteTime() * 1000).longValue(); - Log.d("calHeading uuid : " + cacheData.getUuid(), "result heading : " + cacheData.getHeading() + " speed : " + cacheData.getSpeed()); +// Log.d("calHeading uuid : " + cacheData.getUuid(), "result heading : " + cacheData.getHeading() + " speed : " + cacheData.getSpeed()); circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), cacheData.getType())); return cacheData; } @@ -77,7 +79,7 @@ public class TrackObj { // double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[1], lonLat[0])).distance(DISTANCE); double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude())).distance(DISTANCE); //todo 重新计算速度值(如果连续几帧distance累加到一定值，速度没变化，需要重新计算速度，防止锁死) - if (speedAverage < LIMIT_SPEED) { + if (relativeStatic()) { double tempDis = distance; if (distance >= 4) { //(150km/h) 41.6m/s x 0.1s = 4.16m 约等于 4 tempDis = 4; @@ -102,7 +104,7 @@ public class TrackObj { 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 || speedAverage < LIMIT_SPEED || calDistance * 2 < distance) { + 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(); } else { @@ -142,16 +144,22 @@ public class TrackObj { newDelta = Math.abs(cacheData.getHeading() - lastObj.getHeading()); headingDelta = newDelta; } - //更正数据,速度小于0.5使用上一帧数据 - if (speedAverage < LIMIT_SPEED) { - 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()); - cacheData = cacheData.toBuilder().setHeading(centerLine.getAngle()).build(); + //更正数据,速度小于LIMIT_SPEED使用上一帧数据 + if (relativeStatic()) { + if (roadAngle != 0.0) { + cacheData = cacheData.toBuilder().setHeading(roadAngle).build(); +// 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()); +// cacheData = cacheData.toBuilder().setHeading(centerLine.getAngle()).build(); +// }else{ +// cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build(); +// } } else { -// Log.d("calHeading uuid : " + cacheData.getUuid(), "使用上一帧heading : " + circleQueue.getLastFrame().getHeading()); + //Log.d("calHeading uuid : " + cacheData.getUuid(), "使用上一帧heading : " + circleQueue.getLastFrame().getHeading()); cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build(); } + } //速度大于LIMIT_SPEED并出现大幅度转向使用缓存帧和delta数据 if (cacheData.getSpeed() >= LIMIT_SPEED && newDelta > 10 && headingDelta != 0.0) { @@ -172,6 +180,14 @@ public class TrackObj { return s2CellId.pos(); } + public MessagePad.TrackedObject getCache() { + return cacheData; + } + + public boolean relativeStatic() { + return speedAverage < LIMIT_SPEED; + } + @Override public String toString() { return "TrackObj{" +