diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt index 9a6f593406..53f3dfac50 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt @@ -1,13 +1,18 @@ package com.mogo.eagle.core.function.map.identify +import com.mogo.eagle.core.data.traffic.TrafficData import mogo.telematics.pad.MessagePad import mogo.telematics.pad.MessagePad.TrackedObject interface Identify { + fun renderAdasRecognizedResult(resultList: List?) + fun renderPlanningWarningObj(planningObjects: List?) - fun renderAdasRecognizedResult(resultList: List?) + fun renderOBUWarningObj(exist: Boolean, obuTrafficData: TrafficData) { + + } fun clearOldMarker() } \ No newline at end of file diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt index 7206c6fcc8..3af4a12431 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt @@ -2,6 +2,7 @@ package com.mogo.eagle.core.function.map.identify import android.annotation.SuppressLint import com.mogo.commons.AbsMogoApplication +import com.mogo.eagle.core.data.traffic.TrafficData import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger.w import com.mogo.map.MogoMarkerManager import com.mogo.module.common.MogoApisHandler @@ -21,6 +22,10 @@ class IdentifyBeautifyDataDrawer : Identify { TrackManager.getInstance().renderPlanningWarningObj(planningObjects) } + override fun renderOBUWarningObj(exist: Boolean, obuTrafficData: TrafficData) { + TrackManager.getInstance().renderOBUWarningObj(exist, obuTrafficData) + } + /** * 渲染 adas 识别的数据 * @@ -56,4 +61,5 @@ class IdentifyBeautifyDataDrawer : Identify { TrackManager.getInstance().clearAll() } -} \ No newline at end of file +} + diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.kt index 99959f1f4d..b68a400241 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyFactory.kt @@ -3,11 +3,17 @@ package com.mogo.eagle.core.function.map.identify import android.os.Handler import android.os.Message import com.mogo.eagle.core.data.config.FunctionBuildConfig +import com.mogo.eagle.core.data.traffic.TrafficData +import com.mogo.eagle.core.function.api.obu.IMoGoObuStatusListener +import com.mogo.eagle.core.function.call.obu.CallerObuListenerManager import com.mogo.eagle.core.utilcode.mogo.thread.WorkThreadHandler import mogo.telematics.pad.MessagePad import mogo.telematics.pad.MessagePad.TrackedObject -object IdentifyFactory : Identify { +object IdentifyFactory : Identify, IMoGoObuStatusListener { + + private const val TAG = "IdentifyFactory" + object DriverIdentify { internal val originDataDrawer = IdentifyOriginDataDrawer() } @@ -24,11 +30,14 @@ object IdentifyFactory : Identify { } else { DriverIdentify.originDataDrawer } + CallerObuListenerManager.addListener(TAG, this) } private const val MSG_DATA_TRACK = 0 private const val MSG_DATA_WARNING = 1 - private const val MSG_DATA_CLEAR = 2 + private const val MSG_DATA_OBU_WARNING_UPDATE = 2 + private const val MSG_DATA_OBU_WARNING_REMOVE = 3 + private const val MSG_DATA_CLEAR = 4 // 维护一个线程定时轮询数据进行地图绘制 private val mDrawerHandler: Handler = @@ -59,6 +68,16 @@ object IdentifyFactory : Identify { identify!!.renderPlanningWarningObj(msg.obj as List?) } } + MSG_DATA_OBU_WARNING_UPDATE -> { + if (msg.obj is TrafficData) { + identify!!.renderOBUWarningObj(true, msg.obj as TrafficData) + } + } + MSG_DATA_OBU_WARNING_REMOVE -> { + if (msg.obj is TrafficData) { + identify!!.renderOBUWarningObj(false, msg.obj as TrafficData) + } + } MSG_DATA_CLEAR -> { identify!!.clearOldMarker() } @@ -86,4 +105,19 @@ object IdentifyFactory : Identify { mDrawerHandler.sendMessage(message) } + override fun updateTrackerWarningInfo(trafficData: TrafficData) { + super.updateTrackerWarningInfo(trafficData) + val message = Message.obtain() + message.what = MSG_DATA_OBU_WARNING_UPDATE + message.obj = trafficData + mDrawerHandler.sendMessage(message) + } + + override fun removeTrackerWarningInfo(trafficData: TrafficData) { + super.removeTrackerWarningInfo(trafficData) + val message = Message.obtain() + message.what = MSG_DATA_OBU_WARNING_REMOVE + message.obj = trafficData + mDrawerHandler.sendMessage(message) + } } \ No newline at end of file diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt index 68370910cf..f0b7f6e935 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt @@ -8,6 +8,8 @@ import com.mogo.commons.AbsMogoApplication import com.mogo.eagle.core.data.autopilot.AutopilotStatusInfo import com.mogo.eagle.core.data.config.FunctionBuildConfig import com.mogo.eagle.core.data.enums.TrafficTypeEnum +import com.mogo.eagle.core.data.traffic.TrafficData +import com.mogo.eagle.core.data.traffic.threatLevelColor import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotStatusListener import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger.w @@ -56,11 +58,16 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { /** * planning 感知物预警缓存，用于重置color状态 */ - private val colorTrafficData = ConcurrentHashMap() + private val pncWarningTrafficData = ConcurrentHashMap() + + /** + * obu 感知物预警缓存，用于重置color状态 + */ + private val obuWarningTrafficData = ConcurrentHashMap() @SuppressLint("NewApi") override fun renderPlanningWarningObj(planningObjects: List?) { - colorTrafficData.clear() + pncWarningTrafficData.clear() if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || mAutopilotStatus == 2) { if (planningObjects == null) { return @@ -72,7 +79,7 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { if (planningObj.type == 0) { //0是leading障碍物，障碍物车身红色提示 trackObj?.let { - colorTrafficData[trackId] = PlanningTrack( + pncWarningTrafficData[trackId] = PlanningTrack( "#D65D5AFF", CallerAutoPilotStatusListenerManager.getCurWgs84SatelliteTime() ) @@ -80,7 +87,7 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { } else if (planningObj.type == 1) { //1是避障和择机的障碍物，障碍物车身黄色提示 trackObj?.let { - colorTrafficData[trackId] = PlanningTrack( + pncWarningTrafficData[trackId] = PlanningTrack( "#E4DD94FF", CallerAutoPilotStatusListenerManager.getCurWgs84SatelliteTime() ) @@ -91,6 +98,17 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { } } + override fun renderOBUWarningObj(exist: Boolean, obuTrafficData: TrafficData) { + if (obuTrafficData.uuid.isNullOrEmpty()) { + return + } + if (exist) { + obuWarningTrafficData[obuTrafficData.uuid!!] = obuTrafficData + } else { + obuWarningTrafficData.remove(obuTrafficData.uuid!!) + } + } + /** * 渲染 adas 识别的数据 * @@ -157,16 +175,6 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { val cacheData = mMarkersCaches[uuid] if (cacheData != null) { - //预警颜色变化 - if (colorTrafficData.containsKey(uuid)) { - val planningTrack = colorTrafficData[uuid] - if (!timeOut(planningTrack!!.time)) { - temp = data.toBuilder().setColor(planningTrack.color).build() - } else { - colorTrafficData.remove(uuid) - } - } - //OBU数据颜色标记 val first = data.trackedSourceList.stream() .filter { trackedSource: TrackedSource -> trackedSource.source == 4 } @@ -178,12 +186,28 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { if (subFirst.isPresent) { val subID = subFirst.get().id if (!TextUtils.isEmpty(subID)) { - Log.d("emarrow", "存在subsource为obu的数据") - temp = data.toBuilder().setColor("FF1493").build() + Log.d("emArrow-Test", "存在subSource为obu的数据") + temp = data.toBuilder().setColor("FF0000FF").build() + if (obuWarningTrafficData.containsKey(subID)) { + Log.d("emArrow-Test", "obu数据预警") + temp = data.toBuilder() + .setColor(obuWarningTrafficData[subID]!!.threatLevelColor()) + .build() + } } } } + //预警颜色变化 + if (pncWarningTrafficData.containsKey(uuid)) { + val planningTrack = pncWarningTrafficData[uuid] + if (!timeOut(planningTrack!!.time)) { + temp = data.toBuilder().setColor(planningTrack.color).build() + } else { + pncWarningTrafficData.remove(uuid) + } + } + mFilterTrafficData[uuid] = temp } mMarkersCaches[uuid] = temp @@ -202,6 +226,7 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { .removeMarker(uuid) } trafficDataUuidList.clear() + pncWarningTrafficData.clear() } override fun onAutopilotStatusResponse(autoPilotStatusInfo: AutopilotStatusInfo) { diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java index 2abe3db7af..0835a5ea5a 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/ObjQueue.java @@ -4,11 +4,15 @@ public class ObjQueue { private double heading; private double speed; + private double lat; + private double lon; private int type; - public ObjQueue(double heading, double speed, int type) { + public ObjQueue(double heading, double speed, double lat, double lon, int type) { this.heading = heading; this.speed = speed; + this.lat = lat; + this.lon = lon; this.type = type; } @@ -28,6 +32,22 @@ public class ObjQueue { this.speed = speed; } + public double getLat() { + return lat; + } + + public void setLat(double lat) { + this.lat = lat; + } + + public double getLon() { + return lon; + } + + public void setLon(double lon) { + this.lon = lon; + } + public int getType() { return type; } @@ -41,6 +61,8 @@ public class ObjQueue { return "ObjQueue{" + "heading=" + heading + ", speed=" + speed + + ", lat=" + lat + + ", lon=" + lon + ", type=" + type + '}'; } diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java index ee0d0c8809..aca57d9e16 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java @@ -12,6 +12,8 @@ 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.data.traffic.TrafficData; +import com.mogo.eagle.core.data.traffic.TrafficDataKt; import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager; import com.mogo.eagle.core.utilcode.geometry.S2CellId; import com.mogo.eagle.core.utilcode.geometry.S2LatLng; @@ -66,11 +68,16 @@ public class TrackManager { /** * planning 感知物预警缓存，用于重置color状态 */ - private final ConcurrentHashMap colorTrafficData = new ConcurrentHashMap<>(); + private final ConcurrentHashMap pncWarningTrafficData = new ConcurrentHashMap<>(); + + /** + * obu 感知物预警缓存，用于重置color状态 + */ + private final ConcurrentHashMap obuWarningTrafficData = new ConcurrentHashMap<>(); @SuppressLint("NewApi") public synchronized void renderPlanningWarningObj(List planningObjects) { - colorTrafficData.clear(); + pncWarningTrafficData.clear(); //处于美化模式或者自动驾驶状态下展示 if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || CallerAutoPilotStatusListenerManager.INSTANCE.getAutoPilotStatusInfo().getState() == 2) { if (planningObjects == null) { @@ -84,12 +91,12 @@ public class TrackManager { return; } if (planningObj.getType() == 0) { - colorTrafficData.put(trackId, new PlanningTrack( + pncWarningTrafficData.put(trackId, new PlanningTrack( "#D65D5AFF", CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() )); } else if (planningObj.getType() == 1) { - colorTrafficData.put(trackId, new PlanningTrack( + pncWarningTrafficData.put(trackId, new PlanningTrack( "#E4DD94FF", CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() )); @@ -99,6 +106,17 @@ public class TrackManager { } } + public synchronized void renderOBUWarningObj(boolean exist, TrafficData obuTrafficData) { + if (obuTrafficData.getUuid() == null || obuTrafficData.getUuid().isEmpty()) { + return; + } + if (exist) { + obuWarningTrafficData.put(obuTrafficData.getUuid(), obuTrafficData); + } else { + obuWarningTrafficData.remove(obuTrafficData.getUuid()); + } + } + /** * 过滤后的数据集合 */ @@ -114,16 +132,7 @@ public class TrackManager { String uuid = "" + data.getUuid(); TrackObj trackObj = mMarkersCaches.get(uuid); - // 判断物体是否与预警信息 - if (colorTrafficData.containsKey(uuid)) { - PlanningTrack planningTrack = colorTrafficData.get(uuid); - if (planningTrack != null && !timeOut(planningTrack.getTime())) { - data = data.toBuilder().setColor(planningTrack.getColor()).build(); - } else { - colorTrafficData.remove(uuid); - } - } - + //OBU数据颜色标记 Optional first = data.getTrackedSourceList().stream() .filter(trackedSource -> trackedSource.getSource() == 4).findFirst(); if (first.isPresent()) { @@ -131,12 +140,28 @@ public class TrackManager { if (subFirst.isPresent()) { String subID = subFirst.get().getId(); if (!TextUtils.isEmpty(subID)) { - Log.d("emarrow","存在subsource为obu的数据"); - data = data.toBuilder().setColor("FF1493").build(); + Log.d("emArrow-Test", "存在subSource为obu的数据"); + data = data.toBuilder().setColor("FF0000FF").build(); + if (obuWarningTrafficData.containsKey(subID)) { + Log.d("emArrow-Test", "obu数据预警"); + data = data.toBuilder() + .setColor(TrafficDataKt.threatLevelColor(obuWarningTrafficData.get(subID))) + .build(); + } } } } + // 判断物体是否与预警信息 + if (pncWarningTrafficData.containsKey(uuid)) { + PlanningTrack planningTrack = pncWarningTrafficData.get(uuid); + if (planningTrack != null && !timeOut(planningTrack.getTime())) { + data = data.toBuilder().setColor(planningTrack.getColor()).build(); + } else { + pncWarningTrafficData.remove(uuid); + } + } + if (trackObj != null) { trackObj.updateObj(data); } else { @@ -155,6 +180,9 @@ public class TrackManager { if (cache != null) { //相对静止物体并且非obu数据，则不改变，为感知融合同位置物体，使用缓存数据做覆盖 if (cacheTrack.relativeStatic()) { + if(data.getColor()!= null && !data.getColor().isEmpty()){ + cache = cache.toBuilder().setColor(data.getColor()).build(); + } data = cache; } } @@ -167,7 +195,7 @@ public class TrackManager { trackObj = new TrackObj(data); } } - Log.d("0823-arrow", "uuid : " + uuid); + Log.d("emArrow-Test", "uuid : " + uuid); mFilterTrafficData.put(uuid, trackObj.getCache()); cellIdCaches.forcePut(uuid, trackObj.getCellIdPos()); mMarkersCaches.put(uuid, trackObj); @@ -209,6 +237,7 @@ public class TrackManager { @SuppressLint("NewApi") public void clearAll() { trafficDataUuid.clear(); + pncWarningTrafficData.clear(); mMarkersCaches.forEach((uuid, trackObj) -> removeKey(uuid)); } diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java index 7bdd70b422..0647a4895e 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java @@ -5,13 +5,12 @@ import static com.mogo.eagle.core.function.map.identify.TrackManager.LIMIT_SPEED import android.annotation.SuppressLint; import android.util.Log; +import com.mogo.cloud.commons.utils.CoordinateUtils; import com.mogo.eagle.core.data.enums.TrafficTypeEnum; import com.mogo.eagle.core.data.map.CenterLine; import com.mogo.eagle.core.function.call.map.CallerHDMapManager; import com.mogo.eagle.core.utilcode.geometry.S2CellId; import com.mogo.eagle.core.utilcode.geometry.S2LatLng; -import com.mogo.map.utils.PointInterpolatorUtil; -import com.zhidaoauto.map.sdk.open.data.MapDataApi; import java.util.Arrays; import java.util.Comparator; @@ -21,23 +20,23 @@ import mogo.telematics.pad.MessagePad; public class TrackObj { - private final CircleQueue circleQueue = new CircleQueue(10); + private final CircleQueue circleQueue = new CircleQueue(6); // private final KalmanFilter kalmanFilter; //卡尔曼结果 private S2CellId s2CellId; //s2 id权重 private S2LatLng s2LatLng; //s2 经纬度 private double recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除 private double roadAngle; //道路航向 - private double headingDelta; //航向角德尔塔 +// private double headingDelta; //航向角德尔塔 private int[] typeArray = new int[3]; private int typeWeight; //类型权重 private double lat; private double lon; private double speedAverage; - private CenterLine centerLineInfo = null; +// private CenterLine centerLineInfo = null; public TrackObj(MessagePad.TrackedObject data) { // kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005); - circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getType())); + circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getLatitude(), data.getLongitude(), data.getType())); recentlyTime = data.getSatelliteTime(); lat = data.getLatitude(); lon = data.getLongitude(); @@ -47,6 +46,7 @@ public class TrackObj { if (centerLine != null && centerLine.getAngle() != 0) { roadAngle = centerLine.getAngle(); } + } private MessagePad.TrackedObject cacheData; @@ -56,7 +56,7 @@ public class TrackObj { cacheData = data; correct(); recentlyTime = data.getSatelliteTime(); - circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), cacheData.getType())); + circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), data.getLatitude(), data.getLongitude(), data.getType())); } private void correct() { @@ -125,7 +125,7 @@ public class TrackObj { // } cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build(); } else { - centerLineInfo = null; +// centerLineInfo = null; //不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据 //double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude()); lat = cacheData.getLatitude(); @@ -175,11 +175,26 @@ public class TrackObj { } public boolean relativeStatic() { - return (speedAverage < LIMIT_SPEED && cacheData.getSpeed() < LIMIT_SPEED) | isInRange() ; + if(speedAverage < LIMIT_SPEED){ + Log.d("emArrow-Track","relativeStatic return" + " , uuid : " + cacheData.getUuid()); + return true; + }else{ + return isInRange(); + } } - private boolean isInRange(){ - return s2LatLng.getDistance(S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude())).distance(6371000) < 0.2; + private boolean isInRange() { + if (circleQueue.size() < 3) { + return false; + } + List objQueueList = circleQueue.getPreFrame(); + double[] center = getCenterPoint(objQueueList); + if (center[0] == 0.0 || center[1] == 0.0) { + return false; + } + double dis = CoordinateUtils.calculateLineDistance(center[0], center[1], cacheData.getLongitude(), cacheData.getLatitude()); + Log.d("emArrow-Track", "uuid : " + cacheData.getUuid() + " , list size : " + objQueueList.size() + " , dis : " + dis); + return dis < 0.2; } public boolean isFourWheelType() { @@ -188,6 +203,35 @@ public class TrackObj { && typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO.getType(); } + private double[] getCenterPoint(List objQueueList) { + int total = objQueueList.size(); + double X = 0, Y = 0, Z = 0; + for (int i = 0; i < objQueueList.size() - 1; i++) { + ObjQueue objQueue = objQueueList.get(i); + if (objQueue != null) { + double lat, lon, x, y, z; + lat = objQueue.getLat() * Math.PI / 180; + lon = objQueue.getLon() * Math.PI / 180; + x = Math.cos(lat) * Math.cos(lon); + y = Math.cos(lat) * Math.sin(lon); + z = Math.sin(lat); + X += x; + Y += y; + Z += z; + } + } + X = X / total; + Y = Y / total; + Z = Z / total; + double Lon = Math.atan2(Y, X); + double Hyp = Math.sqrt(X * X + Y * Y); + double Lat = Math.atan2(Z, Hyp); + double[] d = new double[2]; + d[0] = Lon * 180 / Math.PI; + d[1] = Lat * 180 / Math.PI; + return d; + } + @Override public String toString() { return "TrackObj{" + diff --git a/core/function-impl/mogo-core-function-obu-mogo/src/main/java/com/mogo/eagle/core/function/obu/mogo/MogoPrivateObuManager.kt b/core/function-impl/mogo-core-function-obu-mogo/src/main/java/com/mogo/eagle/core/function/obu/mogo/MogoPrivateObuManager.kt index b79d01c83f..e7cfcaa6dd 100644 --- a/core/function-impl/mogo-core-function-obu-mogo/src/main/java/com/mogo/eagle/core/function/obu/mogo/MogoPrivateObuManager.kt +++ b/core/function-impl/mogo-core-function-obu-mogo/src/main/java/com/mogo/eagle/core/function/obu/mogo/MogoPrivateObuManager.kt @@ -359,6 +359,7 @@ class MogoPrivateObuManager private constructor() { ) // 更新数据 TrafficDataConvertUtils.cvxRtiThreatIndInfo2TrafficData(info)?.let { + CallerObuListenerManager.invokeTrackerWarningInfo(it) TrafficMarkerDrawer.updateITrafficThreatLevelInfo(it) } } @@ -370,9 +371,10 @@ class MogoPrivateObuManager private constructor() { CallerHmiManager.disableWarningV2X((appId + direction.direction).toString()) // 更新数据 TrafficDataConvertUtils.cvxRtiThreatIndInfo2TrafficData(info)?.let { + CallerObuListenerManager.removeTrackerWarningInfo(it) // 事件结束，还原车辆颜色 it.threatLevel = 0x01 - TrafficMarkerDrawer.updateITrafficInfo(it) + TrafficMarkerDrawer.updateITrafficThreatLevelInfo(it) } } } @@ -468,7 +470,8 @@ class MogoPrivateObuManager private constructor() { // } // 更新数据 TrafficDataConvertUtils.cvxPtcThreatIndInfo2TrafficData(info)?.let { - TrafficMarkerDrawer.updateITrafficInfo(it) + CallerObuListenerManager.invokeTrackerWarningInfo(it) + TrafficMarkerDrawer.updateITrafficThreatLevelInfo(it) } } // 删除 @@ -477,6 +480,7 @@ class MogoPrivateObuManager private constructor() { CallerHmiManager.showWarning(WarningDirectionEnum.ALERT_WARNING_NON) // 更新数据 TrafficDataConvertUtils.cvxPtcThreatIndInfo2TrafficData(info)?.let { + CallerObuListenerManager.removeTrackerWarningInfo(it) // 事件结束，还原交通参与者颜色 it.threatLevel = 0x01 TrafficMarkerDrawer.updateITrafficThreatLevelInfo(it) @@ -880,6 +884,7 @@ class MogoPrivateObuManager private constructor() { } //更新周边车辆进行预警颜色变换，车辆实时移动和变色 TrafficDataConvertUtils.cvxV2vThreatIndInfo2TrafficData(info)?.let { + CallerObuListenerManager.invokeTrackerWarningInfo(it) TrafficMarkerDrawer.updateITrafficThreatLevelInfo(it) } } @@ -891,6 +896,7 @@ class MogoPrivateObuManager private constructor() { CallerHmiManager.disableWarningV2X((appId + direction.direction).toString()) //更新周边车辆进行预警颜色变换，车辆实时移动和变色 TrafficDataConvertUtils.cvxV2vThreatIndInfo2TrafficData(info)?.let { + CallerObuListenerManager.removeTrackerWarningInfo(it) it.threatLevel = 0x01 TrafficMarkerDrawer.updateITrafficThreatLevelInfo(it) } diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/traffic/TrafficData.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/traffic/TrafficData.kt index fc19bb997b..b141f13369 100644 --- a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/traffic/TrafficData.kt +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/traffic/TrafficData.kt @@ -2,6 +2,23 @@ package com.mogo.eagle.core.data.traffic import com.mogo.eagle.core.data.enums.TrafficTypeEnum +fun TrafficData.threatLevelColor():String{ + return when (threatLevel) { + 1 -> { + "#D8D8D8FF" + } + 2 -> { + "#FFD53EFF" + } + 3 -> { + "#FF3C45FF" + } + else -> { + "#D8D8D8FF" + } + } +} + /** * @author xiaoyuzhou * @date 2021/8/17 8:41 下午 diff --git a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/obu/IMoGoObuStatusListener.kt b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/obu/IMoGoObuStatusListener.kt index a2037a4ebe..7e39f9e31c 100644 --- a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/obu/IMoGoObuStatusListener.kt +++ b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/obu/IMoGoObuStatusListener.kt @@ -1,6 +1,7 @@ package com.mogo.eagle.core.function.api.obu import com.mogo.eagle.core.data.obu.ObuStatusInfo +import com.mogo.eagle.core.data.traffic.TrafficData /** * @author xiaoyuzhou @@ -12,11 +13,29 @@ interface IMoGoObuStatusListener { * 检查OBU连链接信息 * @param obuStatusInfo OBU 状态信息 */ - fun onObuStatusResponse(obuStatusInfo: ObuStatusInfo) + fun onObuStatusResponse(obuStatusInfo: ObuStatusInfo){ + + } /** * v2i时延 */ - fun onObuV2iDelayTime(delayTime: Long) + fun onObuV2iDelayTime(delayTime: Long){ + + } + + /** + * 更新obu Tracker 预警信息 + */ + fun updateTrackerWarningInfo(trafficData: TrafficData){ + + } + + /** + * 移除obu Tracker 预警信息 + */ + fun removeTrackerWarningInfo(trafficData: TrafficData){ + + } } \ No newline at end of file diff --git a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/obu/CallerObuListenerManager.kt b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/obu/CallerObuListenerManager.kt index b396fd75ec..34a4b63b32 100644 --- a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/obu/CallerObuListenerManager.kt +++ b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/obu/CallerObuListenerManager.kt @@ -2,6 +2,7 @@ package com.mogo.eagle.core.function.call.obu import androidx.annotation.Nullable import com.mogo.eagle.core.data.obu.ObuStatusInfo +import com.mogo.eagle.core.data.traffic.TrafficData import com.mogo.eagle.core.function.api.obu.IMoGoObuStatusListener import com.mogo.eagle.core.function.call.base.CallerBase import com.mogo.eagle.core.utilcode.util.GsonUtils @@ -105,5 +106,18 @@ object CallerObuListenerManager : CallerBase() { } } + fun invokeTrackerWarningInfo(trafficData: TrafficData){ + mObuStatusListeners.forEach { + val listener = it.value + listener.updateTrackerWarningInfo(trafficData) + } + } + + fun removeTrackerWarningInfo(trafficData: TrafficData){ + mObuStatusListeners.forEach { + val listener = it.value + listener.removeTrackerWarningInfo(trafficData) + } + } } \ No newline at end of file diff --git a/libraries/mogo-map/src/main/java/com/mogo/map/AMapWrapper.java b/libraries/mogo-map/src/main/java/com/mogo/map/AMapWrapper.java index 135de98e90..85e43f8edb 100644 --- a/libraries/mogo-map/src/main/java/com/mogo/map/AMapWrapper.java +++ b/libraries/mogo-map/src/main/java/com/mogo/map/AMapWrapper.java @@ -3,6 +3,7 @@ package com.mogo.map; import android.annotation.SuppressLint; import android.content.Context; import android.graphics.Point; +import android.util.Log; import com.mogo.eagle.core.data.config.FunctionBuildConfig; import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger; @@ -113,6 +114,7 @@ public class AMapWrapper implements IMogoMap { MarkerSimpleData markerOptions = ObjectUtils.fromTrafficData(trackedObject); if (markerOptions != null) { markerOptionsArrayList.add(markerOptions); + Log.d("emArrow-Test","batch : " + markerOptions.toString()); } }); MarkerHelper.INSTANCE.updateBatchMarkerPositon(markerOptionsArrayList, false, FunctionBuildConfig.isBeautyMode ? 8.0f : 0f, 1, 100, 1); diff --git a/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/TrafficMarkerDrawer.kt b/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/TrafficMarkerDrawer.kt index bb45bb1c5d..152e8dec3b 100644 --- a/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/TrafficMarkerDrawer.kt +++ b/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/TrafficMarkerDrawer.kt @@ -8,6 +8,7 @@ import android.text.TextUtils import com.mogo.commons.AbsMogoApplication import com.mogo.eagle.core.data.enums.TrafficTypeEnum import com.mogo.eagle.core.data.traffic.TrafficData +import com.mogo.eagle.core.data.traffic.threatLevelColor import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger import com.mogo.eagle.core.utilcode.mogo.thread.WorkThreadHandler import com.mogo.map.MogoMarkerManager @@ -17,19 +18,19 @@ import com.mogo.module.common.constants.DataTypes import java.util.concurrent.ConcurrentHashMap /** - * @author xiaoyuzhou - * @date 2021/8/18 11:12 上午 - * 交通元素 2D\3D绘制 + * 此类用于obu/v2x预警绘制交通元素 2D\3D绘制 + * obu:与其进行调试单独展示，与debugview上感知开关相关联 + * v2x:绘制云端预警marker，不会做感知控制管理，后续根据注解的方式来控制v2x业务 */ @SuppressLint("StaticFieldLeak") object TrafficMarkerDrawer { - private val TAG = "TrafficMarkerDrawer" + private const val TAG = "TrafficMarkerDrawer" private var mContext: Context? = null // 动画持续时间 - private val stepTime = 150L + private const val stepTime = 150L // 维护Obu识别的他车集合 private val mTrafficMap = ConcurrentHashMap() @@ -57,15 +58,6 @@ object TrafficMarkerDrawer { mDrawerHandler.sendEmptyMessageDelayed(1, 0L) } - /** - * 添加识别的数据 - */ - fun addTrafficInfo(trafficData: TrafficData) { - if (trafficData.uuid != null) { - mTrafficMap[trafficData.uuid!!] = trafficData - } - } - /** * 更新识别数据,V2V预警的时候需要修改车辆颜色 */ @@ -159,8 +151,8 @@ object TrafficMarkerDrawer { */ private fun drawObuRecognizedDataMarker(trafficData: TrafficData) { CallerLogger.d( - TAG, - "trafficData.type = " + trafficData.type + "---trafficData.threatLevel = " + trafficData.threatLevel + TAG, + "trafficData.type = " + trafficData.type + "---trafficData.threatLevel = " + trafficData.threatLevel ) if (trafficData.type != null) { val resId: Int = trafficData.type.traffic3DIconId @@ -177,30 +169,18 @@ object TrafficMarkerDrawer { .icon3DRes(resId) .rotate(trafficData.heading.toFloat()) .position( - com.mogo.eagle.core.data.map.MogoLatLng( - trafficData.lat, - trafficData.lon - ) + com.mogo.eagle.core.data.map.MogoLatLng( + trafficData.lat, + trafficData.lon + ) ) if (trafficData.type != TrafficTypeEnum.TYPE_TRAFFIC_ID_SPECIAL_VEHICLE) { // 修改颜色 - when (trafficData.threatLevel) { - 1 -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#D8D8D8FF") - } - 2 -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#FFD53EFF") - } - 3 -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#FF3C45FF") - } - else -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#D8D8D8FF") - } - } + mMarkersCaches[trafficData.uuid]?.setAnchorColor(trafficData.threatLevelColor()) } - val marker = MogoMarkerManager.getInstance(mContext).addMarker(DataTypes.TYPE_MARKER_OBU_DATA, options) + val marker = MogoMarkerManager.getInstance(mContext) + .addMarker(DataTypes.TYPE_MARKER_OBU_DATA, options) // 缓存3D资源 mMarkerCachesResMd5Values[resIdVal] = marker.markerResName @@ -215,39 +195,25 @@ object TrafficMarkerDrawer { * 带动画的修改Marker */ private fun changeDynamicMarker( - marker: IMogoMarker, - trafficData: TrafficData + marker: IMogoMarker, + trafficData: TrafficData ) { CallerLogger.d( - TAG, - "trafficData.type = " + trafficData.type + "---trafficData.threatLevel = " + trafficData.threatLevel + TAG, + "trafficData.type = " + trafficData.type + "---trafficData.threatLevel = " + trafficData.threatLevel ) if (trafficData.type != TrafficTypeEnum.TYPE_TRAFFIC_ID_SPECIAL_VEHICLE) { - // 修改颜色 - when (trafficData.threatLevel) { - 1 -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#D8D8D8FF") - } - 2 -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#FFD53EFF") - } - 3 -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#FF3C45FF") - } - else -> { - mMarkersCaches[trafficData.uuid]?.setAnchorColor("#D8D8D8FF") - } - } + mMarkersCaches[trafficData.uuid]?.setAnchorColor(trafficData.threatLevelColor()) } try { marker.addDynamicAnchorPosition( - com.mogo.eagle.core.data.map.MogoLatLng( - trafficData.lat, - trafficData.lon - ), - trafficData.heading.toFloat(), - stepTime + com.mogo.eagle.core.data.map.MogoLatLng( + trafficData.lat, + trafficData.lon + ), + trafficData.heading.toFloat(), + stepTime ) } catch (e: Exception) { e.printStackTrace()