From 83cfab7fd906120d26ce4b4940c9456b7141cebf Mon Sep 17 00:00:00 2001 From: xuxinchao Date: Tue, 4 Mar 2025 14:44:10 +0800 Subject: [PATCH] =?UTF-8?q?[6.9.2]AI=E5=A4=A7=E6=A8=A1=E5=9E=8B=E6=95=B0?= =?UTF-8?q?=E6=8D=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../startup/stageone/HttpDnsStartUp.kt | 20 + core/mogo-core-data/build.gradle | 5 + .../mogo/eagle/core/data/ai/DataConfigs.kt | 20 + .../mogo/eagle/core/data/ai/IRepository.kt | 10 + .../mogo/eagle/core/data/ai/LocationExt.kt | 56 ++ .../eagle/core/data/ai/LocationRepository.kt | 127 +++ .../com/mogo/eagle/core/data/ai/V2XError.kt | 3 + .../mogo/eagle/core/data/ai/V2XRepository.kt | 406 ++++++++++ .../core/data/ai/V2XWarningRepository.kt | 766 ++++++++++++++++++ .../data/ai/roadplanning/PlanningUtils.java | 30 + .../interpolator/DynamicInterpolator.kt | 126 +++ .../interpolator/GeoInterpolationUtils.kt | 65 ++ core/mogo-core-utils/build.gradle | 7 + 13 files changed, 1641 insertions(+) create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/DataConfigs.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/IRepository.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationExt.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationRepository.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XError.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XRepository.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XWarningRepository.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/PlanningUtils.java create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/DynamicInterpolator.kt create mode 100644 core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/GeoInterpolationUtils.kt diff --git a/core/function-impl/mogo-core-function-startup/src/main/java/com/mogo/eagle/core/function/startup/stageone/HttpDnsStartUp.kt b/core/function-impl/mogo-core-function-startup/src/main/java/com/mogo/eagle/core/function/startup/stageone/HttpDnsStartUp.kt index be67dfc23c..606f26f3d9 100644 --- a/core/function-impl/mogo-core-function-startup/src/main/java/com/mogo/eagle/core/function/startup/stageone/HttpDnsStartUp.kt +++ b/core/function-impl/mogo-core-function-startup/src/main/java/com/mogo/eagle/core/function/startup/stageone/HttpDnsStartUp.kt @@ -4,6 +4,7 @@ import android.content.Context import android.os.Handler import android.os.Looper import android.os.Message +import android.util.Log import com.mogo.aicloud.services.locationinfo.MogoLocationInfoServices import com.mogo.aicloud.services.socket.IMogoLifecycleListener import com.mogo.aicloud.services.socket.MogoAiCloudSocketManager @@ -19,6 +20,7 @@ import com.mogo.commons.module.status.MogoStatusManager import com.mogo.commons.network.NetConfigUtils import com.mogo.commons.storage.SharedPrefsMgr import com.mogo.commons.utils.MogoAnalyticUtils +import com.mogo.eagle.core.data.ai.V2XRepository import com.mogo.eagle.core.data.config.FunctionBuildConfig import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_CODE_CLOUD_INIT import com.mogo.eagle.core.data.deva.chain.ChainConstant.Companion.CHAIN_CODE_CLOUD_PASSPORT_TOKEN @@ -41,6 +43,7 @@ import com.mogo.eagle.core.utilcode.util.DeviceUtils import com.mogo.eagle.core.utilcode.util.ProcessUtils import com.mogo.eagle.core.utilcode.util.ThreadPoolService import com.mogo.eagle.core.utilcode.util.TimeUtils +import com.mogo.service.v2n.MGV2NConst import com.rousetime.android_startup.AndroidStartup import com.zhjt.service.chain.ChainLog import ly.count.android.sdk.Countly @@ -50,6 +53,8 @@ class HttpDnsStartUp : AndroidStartup(), IMoGoCloudListener { companion object { private const val TAG = "HttpDnsStartUp" + private const val V2X_APP_ID = "lixiangCar" + private const val V2X_SECRET = "MIGfMA0GCSqGSIb3DQEBAQUAA4GNADCBiQKBgQC1z1vYOYKPhxbftTfN775Z6W8hxZEut/URomy+4aAxL7XcE1YWrXaT6C7hKjx+TFpUVmHXXW/tkeSX6HDUQOqUz5pwopoAjFmTYtgxP1uT0RTz9eryYb9BjCuuPkCehYxQV/8hLoUQ+mtnqqwILuDXYVizw9yBWmxG0tm4hoprWwIDAQAB" } // 配置云服务API @@ -97,6 +102,21 @@ class HttpDnsStartUp : AndroidStartup(), IMoGoCloudListener { this.context = context initGDLoc() preparePassportEnvironment() + //初始化NDE长链 + V2XRepository.init( + context, + SharedPrefsMgr.getInstance().sn+"PAD", + V2X_APP_ID, + V2X_SECRET, + if(DebugConfig.getNetMode() == DebugConfig.NET_MODE_DEV){ + MGV2NConst.MGEnv.QA + }else if(DebugConfig.getNetMode() == DebugConfig.NET_MODE_QA){ + MGV2NConst.MGEnv.QA + } else{ + MGV2NConst.MGEnv.SHANGHAI_PRO + }, + DebugConfig.isDebug() + ) return true } diff --git a/core/mogo-core-data/build.gradle b/core/mogo-core-data/build.gradle index 9ba5734988..4493e75a0f 100644 --- a/core/mogo-core-data/build.gradle +++ b/core/mogo-core-data/build.gradle @@ -85,6 +85,11 @@ dependencies { api 'com.zhidaoauto.machine:mapdata:1.0.0.3' api project(":libraries:mogo-adas-data") api project(':core:mogo-core-utils') + + implementation rootProject.ext.dependencies.amapnavi3dmap +// implementation rootProject.ext.dependencies.amaplocation + implementation rootProject.ext.dependencies.amapsearch +// implementation "com.mogo.service:road-planning:1.1.1-SNAPSHOT" } apply from: new File(rootProject.rootDir, "gradle/upload.gradle").toString() diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/DataConfigs.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/DataConfigs.kt new file mode 100644 index 0000000000..647eb0208e --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/DataConfigs.kt @@ -0,0 +1,20 @@ +package com.mogo.eagle.core.data.ai + +import com.amap.api.location.AMapLocation + +object DataConfigs { + /** + * 是否使用云端纠偏坐标 + */ + var useCorrectLocation = true + + /** + * 使用固定坐标 + */ + var fixedPoint: AMapLocation? = null + + + val gpsConfidence = 0.7f + + +} \ No newline at end of file diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/IRepository.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/IRepository.kt new file mode 100644 index 0000000000..169ff7d200 --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/IRepository.kt @@ -0,0 +1,10 @@ +package com.mogo.eagle.core.data.ai + +import android.content.Context + +interface IRepository { + + fun init(context: Context) + + fun release() +} \ No newline at end of file diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationExt.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationExt.kt new file mode 100644 index 0000000000..24a6c25820 --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationExt.kt @@ -0,0 +1,56 @@ +package com.mogo.eagle.core.data.ai + +import android.location.Location +import com.amap.api.location.AMapLocation +import com.mogo.service.v2n.service.socket.bean.MGLocationBean + +/** + * Create by wyy + * TIME : 2024/6/13 \ 15:05 + * Description: + */ + + +/** + * COORD_TYPE_WGS84 + * V2X 坐标转 高德坐标 慎用 + * @receiver MGLocationModel + * @return Location + */ +fun MGLocationBean.toCG02Location(): Location { + return Location(this.provider).let { + it.speed = this.speed + it.latitude = this.latitude + it.longitude = this.longitude + it.altitude = this.altitude + it.time = this.timeStamp + it.bearing = this.bearing + it.accuracy = this.accuracy + it.provider = this.provider + it + } +} + +/** + * MogoLocation 转 V2X 定位类 + * @receiver MGLocationModel + * @return MGLocationBean + */ +fun AMapLocation.toMogoLocation(sourceType:Int = 0): MGLocationBean { + val bean = MGLocationBean() + bean.lockType = locationType + bean.coordType = MGLocationBean.COORD_TYPE_GCJ02 + bean.accuracy = accuracy + bean.altitude = altitude + bean.bearing = bearing + bean.speed = speed + bean.satellites = satellites + bean.latitude = latitude + bean.longitude = longitude + bean.adCode = adCode + bean.cityCode = cityCode + bean.sourceType = sourceType + bean.gpsConfidence = DataConfigs.gpsConfidence + bean.timeStamp = System.currentTimeMillis() + return bean +} \ No newline at end of file diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationRepository.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationRepository.kt new file mode 100644 index 0000000000..79442308d2 --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/LocationRepository.kt @@ -0,0 +1,127 @@ +package com.mogo.eagle.core.data.ai + +import android.content.Context +import android.util.Log +import com.amap.api.location.AMapLocation +import com.amap.api.location.AMapLocationClient +import com.amap.api.location.AMapLocationClientOption +import com.amap.api.location.AMapLocationListener +import com.mogo.service.v2n.service.socket.bean.MGLocationBean +import com.mogo.service.v2n.utils.CoordinateUtil +import kotlinx.coroutines.flow.MutableSharedFlow +import kotlinx.coroutines.flow.SharedFlow +import kotlinx.coroutines.flow.asSharedFlow + + +/** + * 高德定位 + */ +class LocationRepository : IRepository, AMapLocationListener { + + companion object { + private const val TAG = "LocationRepository" + + @JvmStatic + val instance by lazy(mode = LazyThreadSafetyMode.SYNCHRONIZED) { + LocationRepository() + } + } + + private val _cg02Flow = MutableSharedFlow(extraBufferCapacity = 1) + val cg02Flow: SharedFlow = _cg02Flow.asSharedFlow() + private var mLocationClient: AMapLocationClient? = null + private var mLocationOption: AMapLocationClientOption? = null + var lastCityCode: String? = null + var lastAdCode: String? = null + var lastLocationGcj02: AMapLocation? = null + var lastCorrLoc84: MGLocationBean? = null + var lastLastCorrLoc84: MGLocationBean? = null + var bear: Float = 0.0f + var stopLocation = false + + override fun init(context: Context) { + mLocationOption = AMapLocationClientOption() + mLocationOption?.apply { + setInterval(800L) + setNeedAddress(true) + setLocationMode(AMapLocationClientOption.AMapLocationMode.Hight_Accuracy) + } + + mLocationClient = AMapLocationClient(context.applicationContext) + mLocationClient?.apply { + setLocationOption(mLocationOption) + setLocationListener(this@LocationRepository) + } + mLocationClient?.startLocation() + + mLocationClient?.lastKnownLocation?.let { + lastCityCode = it.cityCode + lastLocationGcj02 = it + lastAdCode = it?.adCode + } + } + + fun startLocation() { + mLocationClient?.startLocation() + //startJob() + } + + override fun release() { + mLocationClient?.stopLocation() + mLocationClient?.onDestroy() + mLocationOption = null + mLocationClient = null + } + + /** + * 高德定位返回 + * @param location MGLocationModel + */ + override fun onLocationChanged(loc: AMapLocation) { +// Log.d(TAG, "onLocationChanged:${location.toStr()}") + + var location = loc + Log.d( + TAG, + "onLocationChanged:$this , provider=${location.provider}#${location}#speed=${location.speed}#bearing=${location.bearing}" + ) + if (location.latitude <= 0 && location.longitude <= 0) return + + location.cityCode.takeIf { !it.isNullOrEmpty() }?.let { + lastCityCode = it + } + + location.adCode.takeIf { !it.isNullOrEmpty() }?.let { + lastAdCode = it + } + if (location.bearing > 0) { + bear = location.bearing + } + location.bearing = bear + + // 设置固定坐标 + if (DataConfigs.fixedPoint != null) { + location = DataConfigs.fixedPoint!! + } + + lastLocationGcj02 = location + _cg02Flow.tryEmit(location) + + + if (!DataConfigs.useCorrectLocation){ + Log.e(TAG, "CorrectLoc 本地坐标应用为纠偏坐标") + location.toMogoLocation().let { + val latLng = CoordinateUtil.gcj02LngLatToWGS84(it.longitude, it.latitude) + it.latitude = latLng[1]; + it.longitude = latLng[0] + if (!stopLocation) { + instance.lastLastCorrLoc84 = instance.lastCorrLoc84 + instance.lastCorrLoc84 = it + V2XRepository._correctLocFlow.tryEmit(it) + + } + } + return + } + } +} \ No newline at end of file diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XError.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XError.kt new file mode 100644 index 0000000000..63b9bc1f0e --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XError.kt @@ -0,0 +1,3 @@ +package com.mogo.eagle.core.data.ai + +data class V2XError(val errorCode: Int, val errorMsg: String?) diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XRepository.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XRepository.kt new file mode 100644 index 0000000000..23b8c06767 --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XRepository.kt @@ -0,0 +1,406 @@ +package com.mogo.eagle.core.data.ai + +import android.content.Context +import android.util.Log +import com.amap.api.location.AMapLocation +import com.mogo.service.v2n.MGV2N +import com.mogo.service.v2n.MGV2NConst +import com.mogo.service.v2n.bean.MGAccidentBean +import com.mogo.service.v2n.bean.MGAccidentVideoBean +import com.mogo.service.v2n.bean.MGCarDragonBean +import com.mogo.service.v2n.bean.MGCongestionBean +import com.mogo.service.v2n.bean.MGCongestionLmBean +import com.mogo.service.v2n.bean.MGConstructionBean +import com.mogo.service.v2n.bean.MGCrossRoadBean +import com.mogo.service.v2n.bean.MGCrossSpeedBean +import com.mogo.service.v2n.bean.MGDrivingAdviceDataBean +import com.mogo.service.v2n.bean.MGDynamicEventBean +import com.mogo.service.v2n.bean.MGLlmQueryBean +import com.mogo.service.v2n.bean.MGLlmQueryRespBean +import com.mogo.service.v2n.bean.MGMogoCarBean +import com.mogo.service.v2n.bean.MGNoaMapDataBean +import com.mogo.service.v2n.bean.MGObstacleBean +import com.mogo.service.v2n.bean.MGPerceptionBean +import com.mogo.service.v2n.bean.MGRoadWetBean +import com.mogo.service.v2n.bean.MGSelfMergeBean +import com.mogo.service.v2n.bean.MGTrafficLightResultBean +import com.mogo.service.v2n.bean.MGWaringCrossBean +import com.mogo.service.v2n.bean.MGWaringVehicleBean +import com.mogo.service.v2n.bean.MGWatchPedestrianBean +import com.mogo.service.v2n.bean.MGWatchTruckBean +import com.mogo.service.v2n.contract.IMGV2NListener +import com.mogo.service.v2n.service.socket.bean.MGLocationBean +import kotlinx.coroutines.channels.BufferOverflow +import kotlinx.coroutines.flow.MutableSharedFlow +import kotlinx.coroutines.flow.MutableStateFlow +import kotlinx.coroutines.flow.SharedFlow +import kotlinx.coroutines.flow.StateFlow + +object V2XRepository : IRepository, IMGV2NListener { + + + private val _v2xErrorFlow = MutableSharedFlow(extraBufferCapacity = 10) + val v2XErrorFlow: SharedFlow = _v2xErrorFlow + var testInputLocationStep = 0 + + var sn: String? = null + + var lastCorrectLocTime = System.currentTimeMillis() + var lastUploadLocationTime = System.currentTimeMillis() + + private val _lightFlow = MutableSharedFlow(extraBufferCapacity = 10) + val lightFlow: SharedFlow = _lightFlow + + /** + * 车速引导 + */ + private val _crossSpeedFlow = MutableSharedFlow(extraBufferCapacity = 1) + val crossSpeedFlow: SharedFlow = _crossSpeedFlow + + /** + * 路口预告 + */ + private val _crossRoadFlow = MutableSharedFlow(extraBufferCapacity = 1) + val crossRoadFlow: SharedFlow = _crossRoadFlow + + /** + * 他车数据 + */ +// private val _otherVehicleFlow = +// MutableSharedFlow(extraBufferCapacity = 1) +// val otherVehicleFlow: SharedFlow = _otherVehicleFlow + + + /** + * 拥堵状态 + */ + private val _congestionStateFlow = MutableStateFlow(false) + val congestionStateFlow: StateFlow = _congestionStateFlow + + + /** + * 道路施工事件 + */ + private val _constructionFlow = MutableSharedFlow(extraBufferCapacity = 10) + val constructionFlow: SharedFlow = _constructionFlow + + + /** + * 静止障碍物事件 + */ + private val _obstacleFlow = MutableSharedFlow(extraBufferCapacity = 10) + val obstacleFlow: SharedFlow = _obstacleFlow + + + /** + * 交通施工事件 + */ + private val _accidentFlow = MutableSharedFlow(extraBufferCapacity = 10) + val accidentFlow: SharedFlow = _accidentFlow + + + /** + * 拥堵事件 + */ + private val _congestionFlow = MutableSharedFlow(extraBufferCapacity = 10) + val congestionFlow: SharedFlow = _congestionFlow + + /** + * 行人横穿预警-road + */ + private val _pedestrianWarningFlow = + MutableSharedFlow( + extraBufferCapacity = 10, + onBufferOverflow = BufferOverflow.DROP_OLDEST + ) + val pedestrianWarningFlow: SharedFlow = _pedestrianWarningFlow + + + /** + * 他车逆行预警 + */ + private val _vehicleFlow = MutableSharedFlow(extraBufferCapacity = 10) + val vehicleFlow: SharedFlow = _vehicleFlow + + + /** + * 协同感知数据 + */ + private val _perceptionFlow = MutableSharedFlow(extraBufferCapacity = 10) + val perceptionFlow: SharedFlow = _perceptionFlow + + /** + * 协同感知数据 + */ + val _correctLocFlow = MutableSharedFlow(extraBufferCapacity = 10) + val correctLocFlow: SharedFlow = _correctLocFlow + + /** + * 事故视频、二维码 + */ + private val _accidentVideoFlow = + MutableSharedFlow(extraBufferCapacity = 10) + val accidentVideoFLow: SharedFlow = _accidentVideoFlow + + + /** + * 行人出没事件 + */ + private val _watchPedFlow = MutableSharedFlow(extraBufferCapacity = 10) + val watchPedFlow: SharedFlow = _watchPedFlow + + + /** + * 大车出没事件 + */ + private val _watchTruckFlow = MutableSharedFlow(extraBufferCapacity = 10) + val watchTruckFlow: SharedFlow = _watchTruckFlow + + + private val _mogoCarFlow = MutableSharedFlow?>(extraBufferCapacity = 10) + val mogoCarFlow: SharedFlow?> = _mogoCarFlow + + + private val _llmResultFlow = MutableSharedFlow(extraBufferCapacity = 10) + val llmResultFlow: SharedFlow = _llmResultFlow + + + private val _congestionLmFlow = MutableSharedFlow(extraBufferCapacity = 10) + val congestionLmFlow: SharedFlow = _congestionLmFlow + + + private val _roadWetFlow = MutableSharedFlow(extraBufferCapacity = 10) + val roadWetFlow: SharedFlow = _roadWetFlow + + private val _carDragonLmFlow = MutableSharedFlow(extraBufferCapacity = 10) + val carDragonLmFlow: SharedFlow = _carDragonLmFlow + + + override fun init(context: Context) { + MGV2N.getInstance().start() +// MGV2N.getInstance().setLogListener { +// Log.d(it) +// } + } + + fun init( + context: Context, + sn: String, + appId: String, + secret: String, + env: MGV2NConst.MGEnv, + debug: Boolean = false + ) { + this.sn = sn + MGV2N.getInstance().init(context, sn, appId, secret, env, debug) + MGV2N.getInstance().setV2NListener(this) + } + + fun uploadLlmQuery(b: MGLlmQueryBean) { + b.user = sn + MGV2N.getInstance().uploadLlmQuery(b) + } + + /** + * 提供v2x坐标 + */ + fun provideLocation(loc: AMapLocation, sourceType: Int = 0) { + loc.toMogoLocation(sourceType).also { + MGV2N.getInstance().uploadLocation(it) + } + Log.d(TAG, "provideLocation delay:${System.currentTimeMillis() - lastUploadLocationTime}") + lastUploadLocationTime = System.currentTimeMillis() + } + + override fun release() { + MGV2N.getInstance().setV2NListener(null) + } + + override fun onTrafficLightDataReceive(trafficLight: MGTrafficLightResultBean?) { + Log.d(TAG, "红绿灯：${trafficLight}") + trafficLight?.let { _lightFlow.tryEmit(it) } + } + + override fun onConstructionReceive(construction: MGConstructionBean?) { + Log.d(TAG, "道路施工：${construction}") + construction?.let { _constructionFlow.tryEmit(it) } + } + + override fun onAccidentReceive(accident: MGAccidentBean?) { + Log.d(TAG, "交通事故：${accident}") + accident?.let { _accidentFlow.tryEmit(it) } + } + + + override fun onObstacleReceive(obstacle: MGObstacleBean?) { + Log.d(TAG, "静止障碍物：${obstacle}") + obstacle?.let { _obstacleFlow.tryEmit(obstacle) } + } + + override fun onCongestionReceive(congestion: MGCongestionBean?) { + Log.d(TAG, "拥堵事件：${congestion}") +// congestion?.let { _congestionFlow.tryEmit(it) } + } + + override fun onCrossSpeedReceive(crossSpeed: MGCrossSpeedBean?) { + Log.d(TAG, "绿波：${crossSpeed}") + crossSpeed?.let { _crossSpeedFlow.tryEmit(it) } + } + + override fun onPedestrianWarningReceive(pedestrian: MGDynamicEventBean?) { +// Log.d( +// TAG, +// "Pedestrian 行人横穿 【road】 ${System.currentTimeMillis()} [${Thread.currentThread().name}]：${pedestrian}" +// ) +// pedestrian?.let { +// _pedestrianWarningFlow.tryEmit(it) +// } + } + + override fun onNdePedestrianWarningReceive(pedestrian: MGDynamicEventBean?) { + Log.d( + TAG, + "Pedestrian 行人横穿 【nde】 ${System.currentTimeMillis()} [${Thread.currentThread().name}]：${pedestrian}" + ) + V2XWarningRepository.instance.updateNdePedestrianWarning(pedestrian) + } + + override fun onVehicleWarningReceive(vehicle: MGDynamicEventBean?) { + Log.d(TAG, "他车逆行预警：${vehicle}") +// vehicle?.let { +// _vehicleFlow.tryEmit(it) +// } + } + + override fun onPerceptionReceive(perception: MGPerceptionBean?) { + Log.d(TAG, "Perception 感知数据 [${Thread.currentThread().name}]: ${perception}") + perception?.let { + _perceptionFlow.tryEmit(it) + } + } + + override fun onCrossRoadReceive(crossRoad: MGCrossRoadBean?) { + Log.d(TAG, "CrossRoad 路口预告 [${Thread.currentThread().name}]: $crossRoad") + crossRoad?.let { + _crossRoadFlow.tryEmit(it) + } + } + + override fun onCorrectLocReceive(loc: MGLocationBean?) { + Log.d(TAG, "CorrectLoc 纠正坐标 :delay:${System.currentTimeMillis() - lastCorrectLocTime}. loc: $loc") + lastCorrectLocTime = System.currentTimeMillis() + if (!DataConfigs.useCorrectLocation) { + Log.e(TAG, "CorrectLoc 纠正坐标 当前配置禁止使用纠偏坐标") + return + } + loc?.let { + +// if (testInputLocationStep == 0) { +// MGLocationBean().apply { +// cityCode = "021" +// longitude = 121.17077571918865 +// latitude = 31.285035266120133 +// bearing = 174F +// timeStamp = 1733196610278 +// }.let { +// LocationRepository.instance.lastLastCorrLoc84 = LocationRepository.instance.lastCorrLoc84 +// LocationRepository.instance.lastCorrLoc84 = it +// _correctLocFlow.tryEmit(it) +// } +// } + + + LocationRepository.instance.lastLastCorrLoc84 = + LocationRepository.instance.lastCorrLoc84 + LocationRepository.instance.lastCorrLoc84 = it + _correctLocFlow.tryEmit(it) + + } + } + + override fun onSelfMergeReceive(data: MGSelfMergeBean?) { + } + + override fun onWatchPedestrianReceive(data: MGWatchPedestrianBean?) { + data?.let { + _watchPedFlow.tryEmit(it) + } + } + + override fun onWatchTruckReceive(data: MGWatchTruckBean?) { + data?.let { + _watchTruckFlow.tryEmit(it) + } + } + + override fun onMapDataReceive(data: MGNoaMapDataBean?) { + } + + override fun onDriveAdviceReceive(data: MGDrivingAdviceDataBean?) { + } + + override fun onWarningCrossReceive(data: MGWaringCrossBean?) { + Log.d(TAG, "onWarningCrossReceive:${data}") + V2XWarningRepository.instance.updateWarningCross(data) + } + + override fun onWarningCarReceive(data: MGWaringVehicleBean?) { + Log.d(TAG, "onWarningCarReceive:${data}") + V2XWarningRepository.instance.updateWarningCar(data) + } + + override fun onWarningTruckReceive(data: MGWaringVehicleBean?) { + Log.d(TAG, "onWarningTruckReceive:${data}") + V2XWarningRepository.instance.updateWarningTruck(data) + } + + override fun onAccidentVideoReceive(data: MGAccidentVideoBean?) { + data?.let { + Log.d( + TAG, + "onAccidentVideoReceive:videoUrl=${data.videoUrl},qrCode:${data.qrCode}" + ) + _accidentVideoFlow.tryEmit(it) + } + } + + override fun onCarDragonReceive(data: MGCarDragonBean?) { + data?.let { + Log.d(TAG, "收到推荐车道, $it") + _carDragonLmFlow.tryEmit(it) + } + } + + override fun onMogoCarReceive(data: List?) { + Log.d(TAG, "onMogoCarReceive:size:${data?.size}, data:${data}") + _mogoCarFlow.tryEmit(data) + } + + override fun onLlmQueryReceive(data: MGLlmQueryRespBean?) { + data?.let { + Log.d(TAG, "onLlmQueryReceive $it") + _llmResultFlow.tryEmit(it) + } + } + + override fun onCongestionLmReceive(data: MGCongestionLmBean?) { + data?.let { + Log.d(TAG, "onCongestionLmReceive $it") + _congestionLmFlow.tryEmit(it) + } + } + + override fun onRoadWetReceive(data: MGRoadWetBean?) { + data?.let { + Log.d(TAG, "onRoadWetReceive $it") + _roadWetFlow.tryEmit(it) + } + } + + override fun onError(errorCode: Int, errorMsg: String?) { + Log.d(TAG, "onError:errorCode=${errorCode},errorMsg:${errorMsg}") + _v2xErrorFlow.tryEmit(V2XError(errorCode, errorMsg)) + } + + private const val TAG = "V2XRepository" +} \ No newline at end of file diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XWarningRepository.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XWarningRepository.kt new file mode 100644 index 0000000000..27a4769366 --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/V2XWarningRepository.kt @@ -0,0 +1,766 @@ +package com.mogo.eagle.core.data.ai + +import android.os.Handler +import android.os.Looper +import android.os.Message +import android.util.Log +import com.mogo.service.v2n.bean.MGDynamicEventBean +import com.mogo.service.v2n.bean.MGPerceptionBean +import com.mogo.service.v2n.bean.MGWaringCrossBean +import com.mogo.service.v2n.bean.MGWaringVehicleBean +import com.mogo.service.v2n.service.socket.bean.MGLocationBean +import com.mogo.service.v2n.utils.CoordinateUtil +import com.zhidaoauto.map.data.point.LonLatPoint +import kotlinx.coroutines.channels.BufferOverflow +import kotlinx.coroutines.flow.MutableSharedFlow +import kotlinx.coroutines.flow.MutableStateFlow +import kotlinx.coroutines.flow.SharedFlow +import java.util.concurrent.ConcurrentHashMap +import kotlin.math.abs +import kotlin.math.atan2 +import kotlin.math.cos +import kotlin.math.min +import kotlin.math.sin + +class V2XWarningRepository { + companion object { + val instance by lazy(mode = LazyThreadSafetyMode.SYNCHRONIZED) { + V2XWarningRepository() + } + private const val TAG = "V2XWarningRepository" + + private const val MSG_WHAT_CAR = 1 + private const val MSG_WHAT_TRUCK = 2 + private const val MSG_WHAT_CROSS = 3 + private const val EVENT_CLEAR_DELAY = 1200L + } + + + private var waringCar: MGWaringVehicleBean? = null + private var waringTruck: MGWaringVehicleBean? = null + private var waringCross: MGWaringCrossBean? = null + private val mainHandler = object : Handler(Looper.getMainLooper()) { + override fun handleMessage(msg: Message) { + + when (msg.what) { + MSG_WHAT_CAR -> { + Log.d(TAG, "clean msg, MSG_WHAT_CAR") + updateWarningCarUI(null) + updateWarningCar(null) + lastWarningCarNotify = null + } + + MSG_WHAT_TRUCK -> { + Log.d(TAG, "clean msg, MSG_WHAT_TRUCK") + updateWarningTruckUI(null) + updateWarningTruck(null) + lastWarningTruckNotify = null + } + + MSG_WHAT_CROSS -> { + Log.d(TAG, "clean msg, MSG_WHAT_CROSS") + updateWarningCrossUI(null) + updateWarningCross(null) + lastWarningCrossNotify = null + } + } + } + } + + /** + * 行人横穿预警-nde + */ + private val ndePedestrianMap = ConcurrentHashMap>() + private val _warningPedestrianNdeFlow = + MutableSharedFlow( + extraBufferCapacity = 10, + onBufferOverflow = BufferOverflow.DROP_OLDEST + ) + val warningPedestrianNdeFlow: SharedFlow = _warningPedestrianNdeFlow + + + private var updateWaringCarTime = 0L + private var updateWaringTrucTime = 0L + private var updateWaringCrossTime = 0L + + private var lastWarningCarUI: List? = null + private var lastWarningTruckUI: List? = null + private var lastWarningCrossUI: List? = null + + private var lastWarningCarNotify: Pair? = null + private var lastWarningTruckNotify: Pair? = null + private var lastWarningCrossNotify: Pair>? = null + + + private val _warningCarFlow = MutableSharedFlow?>(extraBufferCapacity = 10) + val warningCarFlow: SharedFlow?> = _warningCarFlow + + private val _warningCarNotifyFlow = MutableSharedFlow(extraBufferCapacity = 10) + val warningCarNotifyFlow: SharedFlow = _warningCarNotifyFlow + + private val _warningTruckFlow = + MutableSharedFlow?>(extraBufferCapacity = 10) + val warningTruckFlow: SharedFlow?> = _warningTruckFlow + + private val _warningTruckNotifyFlow = MutableSharedFlow(extraBufferCapacity = 10) + val warningTruckNotifyFlow: SharedFlow = _warningTruckNotifyFlow + + private val _warningCrossFlow = + MutableSharedFlow?>(extraBufferCapacity = 10) + val warningCrossFlow: SharedFlow?> = _warningCrossFlow + + private val _warningCrossOriFlow = + MutableSharedFlow>(extraBufferCapacity = 10) + val warningCrossOriFlow: SharedFlow> = _warningCrossOriFlow + + private val _warningOriFlow = MutableStateFlow(0) + val warningOriFlow: SharedFlow = _warningOriFlow + + + private val warningTruckMap = mutableMapOf() + private val warningCarMap = mutableMapOf() + private val warningCrossMap = mutableMapOf() + + + class WaringUIModel { + var uuid: String = "" + var orientation: WaringOrientationType = WaringOrientationType.none + var traj: List? = null + var lat: Double? = null + var lng: Double? = null + var dis: Double = -1.0 + var speed: Double = -1.0 + var level: Int = 3 + var isTowards: Boolean = false + override fun toString(): String { + return "WaringUIModel(uuid='$uuid', orientation=$orientation, traj=$traj, lat=$lat, lng=$lng, dis=$dis, speed=$speed, level=$level, isTowards=$isTowards)" + } + } + + private fun estimateOrientationLatLng( + otherLat: Double, otherLng: Double + ): WaringOrientationType { + val loc84 = getCurrPos() + val selfLat = loc84?.latitude ?: 0.0 + val selfLng = loc84?.longitude ?: 0.0 + val selfHeading = loc84?.bearing?.toDouble() ?: 0.0 + + if (selfLat == 0.0 || selfLng == 0.0) { + return WaringOrientationType.none + } + val orientation = getRelativeDirection(selfLat, selfLng, selfHeading, otherLat, otherLng) + + Log.d( + TAG, + "estimateOrientationLatLng: ${selfLat},${selfLng} $otherLat,$otherLng selfHeading=$selfHeading ori=$orientation" + ) + return orientation + } + + // 获取相对方位 + fun getRelativeDirection( + lat1: Double, lon1: Double, heading: Double, + lat2: Double, lon2: Double + ): WaringOrientationType { + // 计算两点之间的方位角 + val bearing = calculateBearing(lat1, lon1, lat2, lon2) + + // 计算相对角度 + val relativeAngle = (bearing - heading + 360) % 360 + + // 判断方位 + return when { + relativeAngle <= 22.5 || relativeAngle > 337.5 -> WaringOrientationType.top + relativeAngle <= 67.5 -> WaringOrientationType.right_top + relativeAngle <= 112.5 -> WaringOrientationType.right + relativeAngle <= 157.5 -> WaringOrientationType.right_bottom + relativeAngle <= 202.5 -> WaringOrientationType.bottom + relativeAngle <= 247.5 -> WaringOrientationType.left_bottom + relativeAngle <= 292.5 -> WaringOrientationType.left + else -> WaringOrientationType.left_top + } + } + + // 计算两点之间的方位角 + private fun calculateBearing(lat1: Double, lon1: Double, lat2: Double, lon2: Double): Double { + val radLat1 = Math.toRadians(lat1) + val radLat2 = Math.toRadians(lat2) + val deltaLon = Math.toRadians(lon2 - lon1) + + val y = sin(deltaLon) * cos(radLat2) + val x = cos(radLat1) * sin(radLat2) - sin(radLat1) * cos(radLat2) * cos(deltaLon) + val bearing = Math.toDegrees(atan2(y, x)) + + // 标准化为 0°~360° + return (bearing + 360) % 360 + } + +// private fun estimateTrajectoryLatLng( +// uuid: String, +// selfLat: Double, +// selfLng: Double, +// bearing: Float +// ): List? { +// +// Log.d(TAG, "estimateTrajectoryLatLng:uuid:${uuid},selfLat:${selfLat},selfLng:${selfLng} ") +// val lp = LonLatPoint().apply { +// latitude = selfLat +// longitude = selfLng +// angle = bearing.toDouble() +// } +// +// val clRoad = GuideUtils.instance.clRoad(uuid, lp, 100.0) +// Log.d( +// TAG, +// "estimateTrajectoryLatLng:uuid:${uuid},selfLat:${selfLat},selfLng:${selfLng}:${clRoad} " +// ) +// return clRoad +// } + + fun waringCheck(perceptionData: MGPerceptionBean, localBlackList: Set, serverBlackList: Set):Boolean { + val car: MutableList = mutableListOf() + val truck: MutableList = mutableListOf() + val cross: MutableList = mutableListOf() + + Log.d(TAG, "waringCheck: perception size=${perceptionData.data.size}") + var isServerBlackListExist = false + perceptionData.data.forEach { o -> + if (localBlackList.contains(o.uuid)) { + return@forEach + } + if (serverBlackList.contains(o.uuid)) { + isServerBlackListExist = true + return@forEach + } + checkWarningCar(o)?.also { car.add(it) } + checkWarningTruck(o)?.also { truck.add(it) } + checkWarningCross(o)?.also { + if (it.orientation != WaringOrientationType.none + && it.orientation != WaringOrientationType.bottom + && it.orientation != WaringOrientationType.left_bottom + && it.orientation != WaringOrientationType.right_bottom + && it.dis > 1) { + cross.add(it) + } + } + if (o.markerType == 0) { + checkWaningPedestrian(o.uuid) + } + } + updateWarningCarUI(car.takeIf { it.isNotEmpty() }) + updateWarningTruckUI(truck.takeIf { it.isNotEmpty() }) + updateWarningCrossUI(cross.takeIf { it.isNotEmpty() }) + updateNotify(car, truck, cross) + return isServerBlackListExist + } + + //TODO 逻辑重新梳理,改为flow + private fun updateNotify( + car: List?, + truck: List?, + cross: List? + ) { + Log.d(TAG, "updateNotify: car=${car?.size}, truck=${truck?.size}, cross=${cross?.size}") + + if (car?.isNotEmpty() == true) { + handleWarningCarNotif(car) + } + + if (truck?.isNotEmpty() == true) { + handleWarningTruckNotif(truck) + } + + if (cross?.isNotEmpty() == true) { + handleWarningCrossNotif(cross) + } + + val carOri = + car?.filter { it.level > 1 }?.map { it.orientation }?.distinct() ?: emptyList() + val truckOri = + truck?.filter { it.level > 1 }?.map { it.orientation }?.distinct() ?: emptyList() + val crossOri = + cross?.map { it.orientation }?.distinct() ?: emptyList() + + val carOriInt = getOriInt(carOri) + val truckOriInt = getOriInt(truckOri) + val crossOriInt = getOriInt(crossOri) + Log.d( + TAG, + "updateNotify:carOriInt=${getOriStr(carOriInt)}, truckOriInt=${getOriStr(truckOriInt)}, crossOriInt=${ + getOriStr( + crossOriInt + ) + }" + ) + val oriInt = carOriInt or truckOriInt or crossOriInt + //val r:Int = (Math.random() * 255.0).toInt() + _warningOriFlow.tryEmit(oriInt) + } + + + private fun getOriInt(carOri: List): Int { + var oriInt = 0 + carOri.forEach { + oriInt = oriInt or it.mask + } + return oriInt + } + + private fun getOriStr(oriInt: Int): String { + var oriStr = "" + listOf( + WaringOrientationType.left_top, + WaringOrientationType.top, + WaringOrientationType.right_top, + WaringOrientationType.right, + WaringOrientationType.right_bottom, + WaringOrientationType.bottom, + WaringOrientationType.left_bottom, + WaringOrientationType.left, + WaringOrientationType.top_big, + WaringOrientationType.bottom_big, + ).forEach { + oriStr += if (oriInt and it.mask != 0) { + it.text + "," + } else "" + } + return oriStr + } + + private fun handleWarningCrossNotif(cross: List) { + val oriList = cross.map { + it.orientation + }.filterNotNull().toSet() + + val lastTime = lastWarningCrossNotify?.first + val last = lastWarningCrossNotify?.second + + if (last == null || lastTime == null) { + lastWarningCrossNotify = System.currentTimeMillis() to oriList + _warningCrossOriFlow.tryEmit(oriList) + return + } + + + var newOriCount = 0 + for (type in oriList) { + if (!last.contains(type)) { + newOriCount++ + } + } + + if (newOriCount <= 0) { + Log.d(TAG, "忽略路口预警通知：无新方向") + return + } + val delay = System.currentTimeMillis() - lastTime + if (delay < 3000) { + Log.d(TAG, "忽略路口预警通知：距离上次小于3秒") + return + } + lastWarningCrossNotify = System.currentTimeMillis() to oriList + Log.d(TAG, "路口碰撞预警通知: $oriList") + _warningCrossOriFlow.tryEmit(oriList) + } + + private fun handleWarningTruckNotif(truck: List) { + val truckList = mutableListOf() + truckList.addAll(truck) + truckList.sortByLevel() + val warningTruck = truckList.first() + + val lastTime = lastWarningTruckNotify?.first + val last = lastWarningTruckNotify?.second + + if (last == null || lastTime == null) { + lastWarningTruckNotify = System.currentTimeMillis() to warningTruck + _warningTruckNotifyFlow.tryEmit(warningTruck) + return + } + val delay = System.currentTimeMillis() - lastTime + if (delay < 3000) { + Log.d(TAG, "忽略大车预警通知：离上一个通知小于3秒") + return + } + if (warningTruck.level <= last.level /*&& delay < 20000*/) { + Log.d(TAG, "忽略大车预警通知：小于或同等级当前序列不展示") + return + } + lastWarningTruckNotify = System.currentTimeMillis() to warningTruck + Log.d(TAG, "大车预警通知: $warningTruck") + _warningTruckNotifyFlow.tryEmit(warningTruck) + } + + private fun handleWarningCarNotif(car: List) { + val carList = mutableListOf() + carList.addAll(car) + carList.sortByLevel() + val warningCar = carList.first() + + val lastTime = lastWarningCarNotify?.first + val last = lastWarningCarNotify?.second + + if (last == null || lastTime == null) { + lastWarningCarNotify = System.currentTimeMillis() to warningCar + _warningCarNotifyFlow.tryEmit(warningCar) + return + } + + val delay = System.currentTimeMillis() - lastTime + if (delay < 3000) { + Log.d(TAG, "忽略快速小车预警通知：离上一个通知小于3秒") + return + } + if (warningCar.level <= last.level/* && delay < 20000*/) { + Log.d(TAG, "忽略快速小车预警通知：小于或同等级当前序列不展示") + return + } + lastWarningCarNotify = System.currentTimeMillis() to warningCar + Log.d(TAG, "快速小车预警通知: $warningCar") + _warningCarNotifyFlow.tryEmit(warningCar) + } + + private fun updateWarningCarUI(ui: List?) { + // + Log.d(TAG, "updateWarningCarUI:${ui}") + if (ui == null) { + if (lastWarningCarUI != null) { + _warningCarFlow.tryEmit(null) + } + } else { + _warningCarFlow.tryEmit(ui) + } + + lastWarningCarUI = ui + } + + private fun updateWarningTruckUI(ui: List?) { + Log.d(TAG, "updateWarningTruckUI:${ui}") + if (ui == null) { + if (lastWarningTruckUI != null) { + _warningTruckFlow.tryEmit(null) + } + } else { + _warningTruckFlow.tryEmit(ui) + } + lastWarningTruckUI = ui + } + + private fun updateWarningCrossUI(ui: List?) { + Log.d(TAG, "updateWarningCrossUI:${ui}") + if (ui == null) { + if (lastWarningCrossUI != null) { + _warningCrossFlow.tryEmit(null) + } + } else { + _warningCrossFlow.tryEmit(ui) + } + lastWarningCrossUI = ui + } + + + private fun checkWarningCar(item: MGPerceptionBean.Item): WaringUIModel? { + return waringCar?.infos?.firstOrNull { item.uuid == it.uuid }?.let { + WaringUIModel().apply { + uuid = it.uuid + orientation = WaringOrientationType.fromType(it.orientation) + lat = item.lat + lng = item.lon + //traj = estimateTrajectoryLatLng(it.uuid, item.lat, item.lon, item.rotateAngle) + dis = it.distance + speed = it.speed + level = it.level + isTowards = isTowards(item.rotateAngle) + } + } + + } + + // 取第一辆车 + private fun checkWarningTruck(item: MGPerceptionBean.Item): WaringUIModel? { + return waringTruck?.infos?.firstOrNull { item.uuid == it.uuid }?.let { + WaringUIModel().apply { + uuid = it.uuid + orientation = WaringOrientationType.fromType(it.orientation) + lat = item.lat + lng = item.lon + //traj = estimateTrajectoryLatLng(it.uuid, item.lat, item.lon, item.rotateAngle) + dis = it.distance + speed = it.speed + level = it.level + } + } + } + + // 取第一辆车 + private fun checkWarningCross(item: MGPerceptionBean.Item): WaringUIModel? { + return waringCross?.targetInfo?.firstOrNull { item.uuid == it.uuid }?.let { + WaringUIModel().apply { + uuid = it.uuid + orientation = estimateOrientationLatLng(item.lat, item.lon) + lat = item.lat + lng = item.lon + traj = it.traj?.map { it.toTrajectoryLatLonPoint() } + dis = culDisWithSelf(item) + speed = item.speed * 3.6 + level = 3 + } + } + } + + private fun checkWaningPedestrian(uuid: String) { + Log.d(TAG, "checkWaningPedestrian:uuid:${uuid}") + ndePedestrianMap[uuid]?.second?.let { + _warningPedestrianNdeFlow.tryEmit(it) + } + } + + + private fun isTowards(rotateAngle: Float): Boolean { + val loc84 = LocationRepository.instance.lastCorrLoc84 ?: return false + var angle: Double = abs(loc84.bearing - rotateAngle.toDouble()) + if (angle > 180) { + angle = min(angle, 360 - angle) + } + return angle > 135 + } + + private fun culDisWithSelf(item: MGPerceptionBean.Item): Double { + val loc84 = LocationRepository.instance.lastCorrLoc84 + + if (loc84 == null || loc84.latitude == 0.0 || loc84.longitude == 0.0) { + return -1.0 + } + + return CoordinateUtil.calculateLineDistanceLatLon( + item.lat, + item.lon, + loc84.latitude, + loc84.longitude + ).toDouble() + } + + private fun MGLocationBean.toTrajectoryLatLonPoint(): LonLatPoint { + return LonLatPoint().let { + it.latitude = this.latitude + it.longitude = this.longitude + it + } + } + + fun updateWarningCar(waringCar: MGWaringVehicleBean?) { + Log.d(TAG, "updateWarningCar :${waringCar}") + updateWaringCarTime = System.currentTimeMillis() + this.waringCar = waringCar + val t = System.currentTimeMillis() + warningCarMap.clear() + waringCar?.let { car -> + car.infos.forEach { + warningCarMap[it.uuid] = t + } + } + mainHandler.removeMessages(MSG_WHAT_CAR) + mainHandler.sendEmptyMessageDelayed(MSG_WHAT_CAR, EVENT_CLEAR_DELAY) + } + + fun updateWarningCross(waringCross: MGWaringCrossBean?) { + Log.d(TAG, "updateWarningCross :${waringCross}") + updateWaringCrossTime = System.currentTimeMillis() + this.waringCross = waringCross + val t = System.currentTimeMillis() + warningCrossMap.clear() + waringCross?.let { cross -> + cross.targetInfo.forEach { + warningCrossMap[it.uuid] = t + } + } + mainHandler.removeMessages(MSG_WHAT_CROSS) + mainHandler.sendEmptyMessageDelayed(MSG_WHAT_CROSS, EVENT_CLEAR_DELAY) + } + + fun updateWarningTruck(waringTruck: MGWaringVehicleBean?) { + Log.d(TAG, "updateWarningTruck :${waringTruck}") + updateWaringTrucTime = System.currentTimeMillis() + this.waringTruck = waringTruck + val t = System.currentTimeMillis() + warningTruckMap.clear() + waringTruck?.let { truck -> + truck.infos.forEach { + warningTruckMap[it.uuid] = t + } + } + mainHandler.removeMessages(MSG_WHAT_TRUCK) + mainHandler.sendEmptyMessageDelayed(MSG_WHAT_TRUCK, EVENT_CLEAR_DELAY) + } + + fun updateNdePedestrianWarning(waringPedestrian: MGDynamicEventBean?) { + waringPedestrian?.let { + if (it.targetIds.isNotEmpty()) { + val uuid = it.targetIds[0] + ndePedestrianMap[uuid] = System.currentTimeMillis() to it + } + } + } + + fun isWarningTruck(uuid: String): Boolean { + return warningTruckMap[uuid] != null + } + + fun isWarningCar(uuid: String): Boolean { + return warningCarMap[uuid] != null + } + + fun isWarningCross(uuid: String): Boolean { + return warningCrossMap[uuid] != null + } + + fun isWarningPedestrian(uuid: String): Boolean { + return ndePedestrianMap[uuid] != null + } + + + fun clearWarning() { + Log.d(TAG, "clearWarning") + updateWarningCarUI(null) + updateWarningTruckUI(null) + updateWarningCrossUI(null) + updateWarningCar(null) + updateWarningTruck(null) + updateWarningCross(null) + + lastWarningCarNotify = null + lastWarningTruckNotify = null + lastWarningCrossNotify = null + } + + + fun MutableList.sortByLevel() { + this.sortWith(Comparator { a, b -> + val l = a.level - b.level + if (l != 0) { + return@Comparator l + } + return@Comparator if (a.dis > b.dis) -1 else if (a.dis == b.dis) 0 else 1 + }) + } + + fun removeOvertime(map: ConcurrentHashMap>) { + val iterator = map.iterator() + while (iterator.hasNext()) { + val entry = iterator.next() + val time = System.currentTimeMillis() - entry.value.first + // 1分钟移除缓存 + if (time > 60_000) { + Log.d(TAG, "pedestriansRefresh remove: ${entry.key}") + iterator.remove() + } + } + } + + fun pedestriansRefresh() { + removeOvertime(ndePedestrianMap) + Log.d(TAG, "pedestriansRefresh nde: size=${ndePedestrianMap.size}") + } + + private fun getCurrPos(): MGLocationBean? { + val lr = LocationRepository.instance + val start = lr.lastLastCorrLoc84 + val end = lr.lastCorrLoc84 + +// Log.d( +// TAG, +// "getCurrPos: start=${start?.longitude},${start?.latitude},${start?.bearing}, end=${end?.longitude},${end?.latitude},${end?.bearing}" +// ) + + if (start == null) { + return end + } + if (end == null) { + return start + } + return interpolatePoints(start, end) + } +} + +fun interpolateHeading(startHeading: Double, endHeading: Double, fraction: Double): Double { + val clampedFraction = fraction.coerceIn(0.0, 1.0) + + // 将航向角转换到 [-180, 180) + val normalizedStart = ((startHeading + 180) % 360) - 180 + val normalizedEnd = ((endHeading + 180) % 360) - 180 + + // 计算最短差值 + val delta = normalizedEnd - normalizedStart + val shortestDelta = if (delta > 180) delta - 360 else if (delta < -180) delta + 360 else delta + + // 插值并转换回 [0, 360) + val interpolated = normalizedStart + shortestDelta * clampedFraction + return (interpolated + 360) % 360 +} + +fun interpolatePoints(start: MGLocationBean, end: MGLocationBean): MGLocationBean { + val clampedFraction = 0.5 + + // 经纬度线性插值 + val interpolatedLat = start.latitude + (end.latitude - start.latitude) * clampedFraction + val interpolatedLon = start.longitude + (end.longitude - start.longitude) * clampedFraction + + // 修正后的航向角插值 + val interpolatedHeading = interpolateHeading(start.bearing.toDouble(), end.bearing.toDouble(), clampedFraction) + + return MGLocationBean().apply { + longitude = interpolatedLon + latitude = interpolatedLat + bearing = interpolatedHeading.toFloat() + } +} + +enum class WaringOrientationType(val text: String, val mask: Int) { + left_top("左前方", 1 shl 0),// 00000001 + top("前方", 1 shl 1),// 00000010 + right_top("右前方", 1 shl 2),//00000100 + right("右侧", 1 shl 3),//00001000 + right_bottom("右后方", 1 shl 4),//00010000 + bottom("后方", 1 shl 5),//00100000 + left_bottom("左后方", 1 shl 6),//01000000 + left("左侧", 1 shl 7),//10000000 + top_big("前方", 1 shl 8),//100000000 + bottom_big("后方", 1 shl 9),//1000000000 + none("未知", 0); + + companion object { + //a上左，b上中，c上右， d中左，e中，f中右， g下左，h下中，i下右 + fun fromType(type: String?): WaringOrientationType { + return when (type) { + "a" -> left_top + "b" -> top + "c" -> right_top + "d" -> left + "e" -> top + "f" -> right + "g" -> left_bottom + "h" -> bottom + "i" -> right_bottom + "front" -> top_big + "rear" -> bottom_big + else -> none + } + } + + fun fromMask(mask: Int): WaringOrientationType { + return when (mask) { + left_top.mask -> left_top + top.mask -> top + right_top.mask -> right_top + right.mask -> right + right_bottom.mask -> right_bottom + bottom.mask -> bottom + left_bottom.mask -> left_bottom + left.mask -> left + top_big.mask -> top_big + bottom_big.mask -> bottom_big + else -> none + } + } + } +} \ No newline at end of file diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/PlanningUtils.java b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/PlanningUtils.java new file mode 100644 index 0000000000..b1e9b7364b --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/PlanningUtils.java @@ -0,0 +1,30 @@ +package com.mogo.eagle.core.data.ai.roadplanning; + +import com.mogo.service.v2n.utils.CoordinateUtil; + +import java.util.ArrayList; +import java.util.List; + +public class PlanningUtils { + public static List setNavigationGcj(List gpsList) { + if (gpsList == null || gpsList.isEmpty()) { + return null; + } + List list = new ArrayList<>(); + double preLon = 0; + double preLat = 0; + for (double[] gps : gpsList) { + if (String.format("%.6f", gps[0]).equals(String.format("%.6f", preLon)) && + String.format("%.6f", gps[1]).equals(String.format("%.6f", preLat))) { + continue; + } + double[] doubles = CoordinateUtil.gcj02LngLatToWGS84(gps[0], gps[1]); + //double[] doubles = CoordinateUtils.transformGcj02toWgs84(, ); + list.add(doubles); + preLon = gps[0]; + preLat = gps[1]; + } + return list; + } + +} diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/DynamicInterpolator.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/DynamicInterpolator.kt new file mode 100644 index 0000000000..6a784a64e7 --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/DynamicInterpolator.kt @@ -0,0 +1,126 @@ +import android.util.Log +import com.mogo.service.v2n.utils.CoordinateUtil +import com.zhidaoauto.map.data.point.LonLatPoint +import kotlinx.coroutines.* +import kotlinx.coroutines.channels.Channel + + +class DynamicInterpolator { + + companion object { + private const val TAG = "DynamicInterpolator" + } + + private val channel = Channel(Channel.CONFLATED) // 最新输入的坐标 + private var job: Job? = null // 当前插值任务 + var lastCoordinate: LonLatPoint? = null + var lastLineCoordinate: LonLatPoint? = null + private fun areNumbersEqual(num1: Double, num2: Double): Boolean { + // 将 num1 和 num2 乘以 1,000,000 然后转换为整数 + val num1Scaled = (num1 * 1_000_000).toLong() + val num2Scaled = (num2 * 1_000_000).toLong() + // 直接比较两个整数 + return num1Scaled == num2Scaled + } + private fun isLocationEqual(l1:LonLatPoint, l2:LonLatPoint):Boolean { + return areNumbersEqual(l1.latitude, l2.latitude) && areNumbersEqual(l1.longitude, l2.longitude) + } + + fun clear() { + lastCoordinate = null + lastLineCoordinate = null + } + + fun start(onInterpolated: (LonLatPoint) -> Unit) { // 回调函数) { + CoroutineScope(Dispatchers.Default).launch { + for (newCoordinate in channel) { + // 如果有未完成的任务，取消并立即回调最后一个插值点 + job?.let { + if (it.isActive) { + it.cancel() // 取消当前任务 + Log.d(TAG, "job is active, cancelAndJoin, call back lastCoordinate") +// lastCoordinate?.let { +// onInterpolated(it) // 回调当前任务的最后一个点 +// } + } + } + // 如果有前一个坐标，则进行插值计算 + lastCoordinate?.let { previous -> + val locationEqual = isLocationEqual(previous, newCoordinate) + var isNeedUpdate = true + lastLineCoordinate?.let { + val dis = CoordinateUtil.calculateLineDistanceLatLon( + it.latitude, + it.longitude, + newCoordinate.latitude, + newCoordinate.longitude + ) + if (dis < 1.0) { + isNeedUpdate = false + } + } + + if (!locationEqual && isNeedUpdate) { + lastLineCoordinate = newCoordinate + job = launch { + calculateInterpolations(previous, newCoordinate, onInterpolated) + } + } + } + // 更新最后坐标 + lastCoordinate = newCoordinate + } + } + } + + // 启动监听输入数据 + // 计算插值并回调 + private suspend fun calculateInterpolations( + start: LonLatPoint, + end: LonLatPoint, + onInterpolated: (LonLatPoint) -> Unit + ) { + val times = end.satelliteTime - start.satelliteTime + //val numPoints = (i/intervalMillis).toInt() // 设置插值点的数量 + var newTime = times + if (newTime > 1000) { + newTime = 1000 + } + newTime -= 150 + val numPoints = 2 + val delay = newTime / (numPoints + 1) + //val t = i/numPoints + val interpolatedPoints = GeoInterpolationUtils.greatCircleInterpolateWithLatLon( + start.latitude, start.longitude, start.angle, + end.latitude, end.longitude, end.angle, + numPoints + ) + Log.d(TAG, "start:${start},end:${end}, times:${times},numPoints:${numPoints},delay:${delay}, interpolatedPoints:${interpolatedPoints}") + // 按照时间间隔回调每个插值点1 + for (point in interpolatedPoints) { + val (latLon, azimuth) = point + LonLatPoint().apply { + latitude = latLon.first + longitude = latLon.second + angle = azimuth + speed = end.speed + }.let { + onInterpolated(it) + delay(delay) + } + // 回调 + // 延迟时间间隔 + } + + onInterpolated(end) + } + + // 外部调用来传入新的数据 + fun inputData(data: LonLatPoint) { + + CoroutineScope(Dispatchers.Default).launch { + Log.d(TAG, "inputData location:${data}, time:${data.satelliteTime}, delay:${System.currentTimeMillis() - data.satelliteTime}") + channel.send(data) + } + } +} diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/GeoInterpolationUtils.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/GeoInterpolationUtils.kt new file mode 100644 index 0000000000..806bda65cb --- /dev/null +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/ai/roadplanning/interpolator/GeoInterpolationUtils.kt @@ -0,0 +1,65 @@ +import kotlin.math.* + +object GeoInterpolationUtils { + + // 计算两点之间的大圆距离（弧长） + private fun haversineDistance(lat1: Double, lon1: Double, lat2: Double, lon2: Double): Double { + val earthRadius = 6371.0 // 地球半径，单位：公里 + val dLat = Math.toRadians(lat2 - lat1) + val dLon = Math.toRadians(lon2 - lon1) + val a = sin(dLat / 2).pow(2) + cos(Math.toRadians(lat1)) * cos(Math.toRadians(lat2)) * sin(dLon / 2).pow(2) + val c = 2 * atan2(sqrt(a), sqrt(1 - a)) + return earthRadius * c // 返回距离，单位：公里 + } + + // 计算方位角的最短差值，处理跨越0度的情况 + private fun calculateAzimuthDifference(startAzimuth: Double, endAzimuth: Double): Double { + var delta = endAzimuth - startAzimuth + if (delta > 180) { + delta -= 360 // 跨越 0 度的情况 + } else if (delta < -180) { + delta += 360 // 跨越 0 度的情况 + } + return delta + } + + // 使用球面线性插值（Slerp）进行大圆路径插值（经纬度） + fun greatCircleInterpolateWithLatLon( + startLat: Double, startLon: Double, startAzimuth: Double, + endLat: Double, endLon: Double, endAzimuth: Double, + numPoints: Int + ): List, Double>> { + + val interpolatedPoints = mutableListOf, Double>>() + + // 计算大圆距离 + val distance = haversineDistance(startLat, startLon, endLat, endLon) + + // 插值 + for (i in 1..numPoints) { + val fraction = i.toDouble() / (numPoints + 1) // 插值比例，确保均匀分布 + + // 使用球面线性插值（Slerp）计算经纬度 + val a = sin((1 - fraction) * distance) / sin(distance) + val b = sin(fraction * distance) / sin(distance) + + val x = a * cos(Math.toRadians(startLat)) * cos(Math.toRadians(startLon)) + + b * cos(Math.toRadians(endLat)) * cos(Math.toRadians(endLon)) + val y = a * cos(Math.toRadians(startLat)) * sin(Math.toRadians(startLon)) + + b * cos(Math.toRadians(endLat)) * sin(Math.toRadians(endLon)) + val z = a * sin(Math.toRadians(startLat)) + b * sin(Math.toRadians(endLat)) + + val interpolatedLat = Math.toDegrees(atan2(z, sqrt(x * x + y * y))) + val interpolatedLon = Math.toDegrees(atan2(y, x)) + + // 计算角度差并进行线性插值 + val azimuthDifference = calculateAzimuthDifference(startAzimuth, endAzimuth) + val interpolatedAzimuth = startAzimuth + fraction * azimuthDifference + + // 保存插值结果 + interpolatedPoints.add(Pair(Pair(interpolatedLat, interpolatedLon), interpolatedAzimuth)) + } + + return interpolatedPoints + } +} \ No newline at end of file diff --git a/core/mogo-core-utils/build.gradle b/core/mogo-core-utils/build.gradle index d8941ad776..5d8d385937 100644 --- a/core/mogo-core-utils/build.gradle +++ b/core/mogo-core-utils/build.gradle @@ -89,6 +89,13 @@ dependencies { api rootProject.ext.dependencies.mogochainbase api rootProject.ext.dependencies.mogoservicebiz + + api ('com.mogo.service:mogo-v2x-enhanced:1.3.8'){ + exclude group: 'com.google.protobuf', module: 'protoc' + exclude group: 'com.google.protobuf', module: 'protobuf-java' + exclude group: 'com.google.protobuf', module: 'protobuf-java-util' + exclude group: 'com.google.protobuf', module: 'protobuf-javalite' + } } apply from: new File(rootProject.rootDir, "gradle/upload.gradle").toString()