yangyakun921/MoGoEagleEye
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wait to merge

Browse Source
This commit is contained in:
zhongchao
2022-06-16 16:01:36 +08:00
parent 52bd1df733
commit 20f7c76321
11 changed files with 248 additions and 303 deletions
Download Patch File Download Diff File Expand all files Collapse all files

213
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java
View File

@@ -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<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<>();
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<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;
}
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();
}
}

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

@@ -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<TrackedObject>?) {
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()

4
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java → core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/CircleQueue.java
View File

@@ -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;

12
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.java Normal file
View File

@@ -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<MessagePad.TrackedObject> resultList);
void clearOldMarker();
}

65
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.java Normal file
View File

@@ -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<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();
}
}

52
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.java Normal file
View File

@@ -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<MessagePad.TrackedObject> resultList) {
identify.renderAdasRecognizedResult(resultList);
}
@Override
public void clearOldMarker() {
identify.clearOldMarker();
}
}

106
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java → core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.java
View File

@@ -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<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<>();
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<MessagePad.TrackedObject> 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<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
// ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
ArrayList<MessagePad.TrackedObject> 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<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) {
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();
}
}

2
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java → core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/KalmanFilter.java
View File

@@ -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;

2
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java → core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java
View File

@@ -1,4 +1,4 @@
package com.mogo.eagle.core.function.map;
package com.mogo.eagle.core.function.map.identify;
public class ObjQueue {

42
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java → core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java
View File

@@ -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<String, Long> cellIdCaches = HashBiMap.create();
private final ArrayMap<String, TrackObj> 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<MessagePad.TrackedObject> resultList) {
public void clearCache(List<MessagePad.TrackedObject> 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);
}
}

48
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java → core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java
View File

@@ -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{" +