From 6ee8fdab68623c6b29dbc45c435e29a1024182e2 Mon Sep 17 00:00:00 2001 From: yangyakun Date: Tue, 20 Jun 2023 13:56:12 +0800 Subject: [PATCH] =?UTF-8?q?[3.3.0]=20[feature]=20[=E8=BE=B9=E7=95=8C?= =?UTF-8?q?=E5=A4=84=E7=90=86=E3=80=81=E8=AE=BE=E7=BD=AE=E7=AB=99=E7=82=B9?= =?UTF-8?q?=E5=92=8C=E6=B8=85=E7=90=86=E7=AB=99=E7=82=B9]?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../TrajectoryAndDistanceManager.kt | 90 +++++++--- .../utils/CoordinateCalculateRouteUtil.kt | 162 ++++++++++++++---- .../com/mogo/och/taxi/model/TaxiModel.java | 18 ++ .../och/taxi/presenter/TaxiPresenter.java | 3 + .../core/data/deva/chain/ChainConstant.kt | 1 + 5 files changed, 219 insertions(+), 55 deletions(-) diff --git a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/TrajectoryAndDistanceManager.kt b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/TrajectoryAndDistanceManager.kt index be6369dc1b..6cb98e3de3 100644 --- a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/TrajectoryAndDistanceManager.kt +++ b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/TrajectoryAndDistanceManager.kt @@ -1,19 +1,21 @@ package com.mogo.och.common.module.manager.trajectorymamager import com.mogo.commons.AbsMogoApplication +import com.mogo.eagle.core.data.deva.chain.ChainConstant import com.mogo.eagle.core.data.map.MogoLocation import com.mogo.eagle.core.function.api.autopilot.IMoGoPlanningRottingListener import com.mogo.eagle.core.function.call.autopilot.CallerChassisLocationGCJ02ListenerManager import com.mogo.eagle.core.function.call.autopilot.CallerPlanningRottingListenerManager import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger.d +import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger.e import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.Companion.M_OCHCOMMON import com.mogo.eagle.core.utilcode.util.CoordinateUtils import com.mogo.och.common.module.manager.loopmanager.BusPassengerModelLoopManager import com.mogo.och.common.module.manager.loopmanager.LoopInfo import com.mogo.och.common.module.utils.CoordinateCalculateRouteUtil +import com.zhjt.service.chain.ChainLog import mogo.telematics.pad.MessagePad -import java.util.ArrayList import java.util.concurrent.ConcurrentHashMap object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ @@ -54,6 +56,9 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ @Volatile private var mRoutePoints: MutableList? = ArrayList() + @Volatile + private var lineId:Long? = null + @Volatile private var endStationInfo: StationAndIndex = StationAndIndex(null, null, null, null) @@ -72,7 +77,13 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ globalPathResp?.wayPointsList?.let { if (it.size > 0) { d(M_OCHCOMMON + TAG, "收到轨迹:${it.size}第一个点${it[0]}最后一个点:${it.last()}") + if(globalPathResp.lineId == lineId){ + d(M_OCHCOMMON + TAG, "重复轨迹") + startCalculateDistanceLoop() + return + } endCalculateDistanceLoop() + this.lineId = globalPathResp.lineId updateRoutePoints(it) preCarLocationIndexInTrajectory = 0 startCalculateDistanceLoop() @@ -128,7 +139,6 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ endCalculateDistanceLoop() return } - d(M_OCHCOMMON + TAG, "站点坐标：${endStationInfo.stationPoint}") calculateRouteSumLength(it) } } @@ -151,7 +161,7 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ @Synchronized - private fun calculateRouteSumLength( + fun calculateRouteSumLength( location: MogoLocation, ) { if (mRoutePoints.isNullOrEmpty()) return @@ -163,25 +173,15 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ ) { //要前往的站在轨迹中对应的点的信息 val startStationInfoInTra = CoordinateCalculateRouteUtil.getNearestPointInfo( - preCarLocationIndexInTrajectory, mRoutePoints!!, startStationInfo.stationPoint!! + preCarLocationIndexInTrajectory, mRoutePoints!!.size,mRoutePoints!!, startStationInfo.stationPoint!!,1 ) startStationInfo.isNext = startStationInfoInTra.second startStationInfo.index = startStationInfoInTra.first startStationInfo.distance = startStationInfoInTra.third preCarLocationIndexInTrajectory = startStationInfoInTra.first + writeLog(startStationInfoInTra,location) } - // 计算车的位置在轨迹中的信息 这个是一个变量可以缓存 - val carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo( - preCarLocationIndexInTrajectory, mRoutePoints!!, location - ) - if(carLocationInfo.second==null||carLocationInfo.third>1_000){ - preCarLocationIndexInTrajectory = 0 - return - } - d(M_OCHCOMMON+ TAG,"距离车最近的点:${carLocationInfo.first} 点下后面的轨迹：${carLocationInfo.second} 距离点的距离：${carLocationInfo.third}") - preCarLocationIndexInTrajectory = carLocationInfo.first - // 计算结束站点在轨迹中的信息 这个是一个常量可以缓存 if (endStationInfo.stationPoint != null && endStationInfo.isNext == null @@ -190,24 +190,58 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ ) { //要前往的站在轨迹中对应的点 val stationPointInRouteIndex = CoordinateCalculateRouteUtil.getNearestPointInfo( - preCarLocationIndexInTrajectory, mRoutePoints!!, endStationInfo.stationPoint!! + preCarLocationIndexInTrajectory,mRoutePoints!!.size, mRoutePoints!!, endStationInfo.stationPoint!!,3 ) endStationInfo.isNext = stationPointInRouteIndex.second endStationInfo.index = stationPointInRouteIndex.first endStationInfo.distance = stationPointInRouteIndex.third + writeLog(stationPointInRouteIndex, location) } + + val carLocationInfo:Triple + if(endStationInfo.isNext==true){ + // 计算车的位置在轨迹中的信息 这个是一个变量可以缓存 + carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo( + preCarLocationIndexInTrajectory,endStationInfo.index!!, mRoutePoints!!, location,2 + ) + }else{ + carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo( + preCarLocationIndexInTrajectory,endStationInfo.index!!+1, mRoutePoints!!, location,2 + ) + } + writeLog(carLocationInfo, location) + if(carLocationInfo.second==null||carLocationInfo.third>1_000){ + preCarLocationIndexInTrajectory = 0 + return + } + preCarLocationIndexInTrajectory = carLocationInfo.first + // 距离回调 - if(distanceListeners.size>0) { - invokeDistance(carLocationInfo, location) + try { + if(distanceListeners.size>0) { + invokeDistance(carLocationInfo, location) + } + }catch (e:Exception){ + e(M_OCHCOMMON+ TAG,"距离计算错误") } // 不带站点轨迹回调 - if(trajectoryListeners.size>0) { - invokeTrajectory(carLocationInfo, location) + try { + if(trajectoryListeners.size>0) { + invokeTrajectory(carLocationInfo, location) + } + }catch (e:Exception){ + e(M_OCHCOMMON+ TAG,"轨迹线(轨迹两头)计算错误") } + // 只展示站点之间轨迹 - if(trajectoryWithStationListeners.size>0) { - invokeTrajectoryWithStation(carLocationInfo, location) + try { + if(trajectoryWithStationListeners.size>0) { + invokeTrajectoryWithStation(carLocationInfo, location) + } + }catch (e:Exception){ + e(M_OCHCOMMON+ TAG,"轨迹线(站点两头)计算错误") } + } @@ -303,6 +337,7 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ endCut = endStationInfo.index!!+1 } } + d(M_OCHCOMMON+ TAG,"根据站点切个：第一个点：$fromCut 最后一个点$endCut") if(carLocationInfo.second==true){// isNext = true routeArrivied.addAll(mRoutePoints!!.subList(fromCut,carLocationInfo.first)) routeArriving.addAll(mRoutePoints!!.subList(carLocationInfo.first,endCut)) @@ -323,4 +358,15 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{ } } + @ChainLog( + linkChainLog = ChainConstant.CHAIN_TYPE_SOCKET_AUTOPILOT, + linkCode = ChainConstant.CHAIN_SOURCE_ADAS, + nodeAliasCode = ChainConstant.CHAIN_CODE_OCH_COMMON_DISTANCE, + paramIndexes = [0,1] + ) + fun writeLog(carLocationInfo: Triple, location: MogoLocation) { + d(M_OCHCOMMON+ TAG,"距离车最近的点:${carLocationInfo.first} 当前位置在点的前后：${carLocationInfo.second} 距离点的距离：${carLocationInfo.third}") + d(M_OCHCOMMON+ TAG,"定位信息:${location}") + } + } \ No newline at end of file diff --git a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/utils/CoordinateCalculateRouteUtil.kt b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/utils/CoordinateCalculateRouteUtil.kt index 7afbb43109..b73d57e2fb 100644 --- a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/utils/CoordinateCalculateRouteUtil.kt +++ b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/utils/CoordinateCalculateRouteUtil.kt @@ -12,6 +12,7 @@ import com.mogo.och.common.module.manager.trajectorymamager.DistanceDegree import mogo.telematics.pad.MessagePad import java.util.TreeMap import kotlin.math.acos +import kotlin.math.atan2 import kotlin.math.cos import kotlin.math.pow import kotlin.math.sin @@ -547,23 +548,44 @@ object CoordinateCalculateRouteUtil { } fun getHeadingAngle(location: MogoLocation, nextPoint: MogoLocation): Double { - val y = Math.sin(nextPoint.longitude - location.longitude) * Math.cos(nextPoint.latitude) - val x = Math.cos(location.latitude) * Math.sin(nextPoint.latitude) - Math.sin( - location.latitude - ) * - Math.cos(nextPoint.latitude) * Math.cos(nextPoint.longitude - location.longitude) - var bearing = Math.atan2(y, x) + return getHeadingAngle( + location.longitude, + location.latitude, + nextPoint.longitude, + nextPoint.latitude + ) + } + + fun getHeadingAngle(location: LatLng, nextPoint: LatLng): Double { + return getHeadingAngle( + location.longitude, + location.latitude, + nextPoint.longitude, + nextPoint.latitude + ) + } + fun getHeadingAngle(locationLongitude: Double, locationLatitude: Double, + nextPointLongitude: Double, nextPointLatitude: Double): Double { + val y = sin(nextPointLongitude - locationLongitude) * cos(nextPointLatitude) + val x = cos(locationLatitude) * sin(nextPointLatitude) - sin(locationLatitude) * + cos(nextPointLatitude) * cos(nextPointLongitude - locationLongitude) + var bearing = atan2(y, x) bearing = Math.toDegrees(bearing) if (bearing < 0) { bearing += 360.0 } - return bearing + return 360-bearing } + + /** + * 支持带航行角的和不带航向角的 + * 带航向角的会删除 航向角差大于90°的点 * @param preIndex 上次计算缓存 * @param mRoutePoints 轨迹点 * @param location 目标坐标 + * @param type 1 开始站点 2 定位点 3 结束站点 * @return Triple * 距离目标坐标最近的轨迹点下标、 * 最近点坐标是目标坐标的上一个点还是下一个点 @@ -572,13 +594,16 @@ object CoordinateCalculateRouteUtil { @JvmStatic fun getNearestPointInfo( preIndex: Int, + endIndex: Int, mRoutePoints: List, location: MogoLocation, + type:Int, // + size:Int = 8 ): Triple { var currentIndex:Int = preIndex //记录疑似点 //基础点 // 轨迹中的点和定位点的距离集合 val distanceMap: TreeMap = TreeMap() - for (index in preIndex until mRoutePoints.size) { + for (index in preIndex until endIndex) { val latLngIndex = mRoutePoints[index] val distance = CoordinateUtils.calculateLineDistance( location.longitude, @@ -587,7 +612,7 @@ object CoordinateCalculateRouteUtil { latLngIndex.latitude ) distanceMap[DistanceDegree(distance, null,null)] = index - if (distanceMap.size > 4) { + if (distanceMap.size > size) { distanceMap.pollLastEntry() } } @@ -600,49 +625,81 @@ object CoordinateCalculateRouteUtil { if (pointIndex + 1 < mRoutePoints.size) { val nextPoint = mRoutePoints[pointIndex + 1] - nextDegree = CoordinateCalculateRouteUtil.getDegree(location, currentPoint, nextPoint) + nextDegree = getDegree(location, currentPoint, nextPoint) + }else{ + nextDegree = 135.0 } // 需要减距离 和上一个轨迹点成钝角 if (pointIndex - 1 >= 0) { val prePoint = mRoutePoints[pointIndex - 1] - preDegree = CoordinateCalculateRouteUtil.getDegree( + preDegree = getDegree( location, currentPoint, prePoint ) + }else{// 第一个个点处理 + preDegree = 135.0 } - if(nextDegree>90){ + fun getDegreeNext(){ if (pointIndex + 1 < mRoutePoints.size) { val nextPoint = mRoutePoints[pointIndex + 1] - val headingAngle = CoordinateCalculateRouteUtil.getHeadingAngle(currentPoint, nextPoint) + val headingAngle = getHeadingAngle(currentPoint, nextPoint) + distanceDegree.degree = headingAngle + distanceDegree.isNext = false + }else{ + val prePoint = mRoutePoints[pointIndex - 1] + val headingAngle = getHeadingAngle(prePoint,currentPoint) + distanceDegree.degree = headingAngle + distanceDegree.isNext = true + } + } + fun getDegreePre(){ + if (pointIndex - 1 >= 0) { + val prePoint = mRoutePoints[pointIndex - 1] + val headingAngle = getHeadingAngle(prePoint, currentPoint) + distanceDegree.degree = headingAngle; + distanceDegree.isNext = true + }else{ + val nextPoint = mRoutePoints[pointIndex + 1] + val headingAngle = getHeadingAngle(currentPoint,nextPoint) distanceDegree.degree = headingAngle distanceDegree.isNext = false } } - if(preDegree>90){ - if (pointIndex - 1 >= 0) { - val prePoint = mRoutePoints[pointIndex - 1] - val headingAngle = CoordinateCalculateRouteUtil.getHeadingAngle(prePoint, currentPoint) - distanceDegree.degree = headingAngle; - distanceDegree.isNext = true - } + if(nextDegree>90){ + getDegreeNext() } - // 距离过近直接返回 - if(distanceDegree.distance<1){ - currentIndex = pointIndex - return Triple(currentIndex,distanceDegree.isNext,distanceDegree.distance) + if(preDegree>90){ + getDegreePre() + } + if(preDegree<90&&nextDegree<90&&preDegree+nextDegree>90){ + if (pointIndex + 1 < mRoutePoints.size&&pointIndex - 1 >= 0) { + val nextPoint = mRoutePoints[pointIndex + 1] + val prePoint = mRoutePoints[pointIndex - 1] + val degree = getDegree(currentPoint, prePoint, nextPoint) + if(degree>90) { + getDegreePre() + // isNext 值无所谓了 就 ture和false都行 + // 通过航向角过滤一遍 + if(distanceDegree.degree!=null&&DrivingDirectionUtils.getAngleDiff(location.heading, distanceDegree.degree!!)<90){ + currentIndex = pointIndex + return Triple(currentIndex,distanceDegree.isNext,distanceDegree.distance) + } + } + } } } + // 根据角度来排除一些点 val iterator = distanceMap.iterator() // 有航向角比较航向角 - if(location.heading!=0.0) { - while (iterator.hasNext()) { - val next = iterator.next() - val key = next.key - if (key.degree == null) { - iterator.remove() - } else { + while (iterator.hasNext()) { + val next = iterator.next() + val key = next.key + if (key.degree == null) { + iterator.remove() + } else { + if (location.heading != 0.0) { key.degree?.let { val dexAngle = DrivingDirectionUtils.getAngleDiff(location.heading, it) if (dexAngle > 90) { @@ -653,7 +710,7 @@ object CoordinateCalculateRouteUtil { } } - // 上一次查询此次最近点包含上个点和下个点 + // 最近点中包含上次计算的点和上次计算的最近的一个点 if(distanceMap.containsValue(preIndex)&&distanceMap.containsValue(preIndex+1)){ var preIndexDistance:DistanceDegree?=null var preIndexNextDistance:DistanceDegree?=null @@ -677,7 +734,45 @@ object CoordinateCalculateRouteUtil { } } - // 最近的点 只有一个前面的点 + var maxIndex = 0 + var minIndex = Int.MAX_VALUE + + distanceMap.iterator().forEach { en -> + val value = en.value + if(valuemaxIndex){ + maxIndex = value + } + } + + val middleVale =minIndex + (maxIndex-minIndex)/2 + when (type) { + 1 -> {// 求开始站点 + val iteratorRemove = distanceMap.iterator() + while (iteratorRemove.hasNext()) { + val next = iteratorRemove.next() + val key = next.key + if(next.value>middleVale){ + iteratorRemove.remove() + } + } + } + 3 -> {// 求结束站点 + val iteratorRemove = distanceMap.iterator() + while (iteratorRemove.hasNext()) { + val next = iteratorRemove.next() + val key = next.key + if(next.value {} + } + + // 根据距离来计算 最近的点 只有一个前面的点 var tempDistance = Float.MAX_VALUE var isNext:Boolean? = null distanceMap.iterator().forEach { en -> @@ -696,6 +791,7 @@ object CoordinateCalculateRouteUtil { } } } + return Triple(currentIndex,isNext,tempDistance) } } \ No newline at end of file diff --git a/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java b/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java index d9684e0c27..c754678d1e 100644 --- a/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java +++ b/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java @@ -49,6 +49,7 @@ import com.mogo.och.common.module.biz.provider.LoginService; import com.mogo.och.common.module.callback.OchAdasStartFailureCallback; import com.mogo.och.common.module.manager.AbnormalFactorsLoopManager; import com.mogo.och.common.module.manager.OCHAdasAbilityManager; +import com.mogo.och.common.module.manager.trajectorymamager.TrajectoryAndDistanceManager; import com.mogo.och.common.module.map.AmapNaviToDestinationModel; import com.mogo.och.common.module.utils.CoordinateCalculateRouteUtil; import com.mogo.och.common.module.utils.PinYinUtil; @@ -1634,4 +1635,21 @@ public class TaxiModel { } } + + public void setStation(){ + OrderQueryRespBean.Result currentOCHOrder = getCurrentOCHOrder(); + if(currentOCHOrder!=null){ + MogoLocation startStation = new MogoLocation(); + startStation.setLongitude(currentOCHOrder.startSiteGcjPoint.get(0)); + startStation.setLatitude(currentOCHOrder.startSiteGcjPoint.get(1)); + MogoLocation endStation = new MogoLocation(); + endStation.setLongitude(currentOCHOrder.endSiteGcjPoint.get(0)); + endStation.setLatitude(currentOCHOrder.endSiteGcjPoint.get(1)); + TrajectoryAndDistanceManager.INSTANCE.setStationPoint(startStation,endStation); + } + } + + public void cleanStation(){ + TrajectoryAndDistanceManager.INSTANCE.setStationPoint(null,null); + } } diff --git a/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/presenter/TaxiPresenter.java b/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/presenter/TaxiPresenter.java index fde991f2ac..3b08eb1527 100644 --- a/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/presenter/TaxiPresenter.java +++ b/OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/presenter/TaxiPresenter.java @@ -240,10 +240,12 @@ public class TaxiPresenter extends Presenter implements ITaxiADASS CallerLogger.INSTANCE.d(M_TAXI + TAG,"order = "+order.toString()); if (TaxiOrderStatusEnum.UserArriveAtStart.getCode() == order.orderStatus){ TaxiModel.getInstance().queryAutopilotStatus(true); + TaxiModel.getInstance().setStation(); } if (TaxiOrderStatusEnum.OnTheWayToEnd.getCode() == order.orderStatus){ TaxiModel.getInstance().startDynamicCalculateRouteInfo(); + TaxiModel.getInstance().setStation(); runOnUIThread(() -> { mView.updateCtvAutopilotStatusTag(true); CallerOrderListenerManager.INSTANCE.invokeOrderStatus(true); @@ -256,6 +258,7 @@ public class TaxiPresenter extends Presenter implements ITaxiADASS TaxiOrderStatusEnum.JourneyCompleted.getCode() == order.orderStatus){ TaxiModel.getInstance().startOrStopCalculateRouteInfo(false); TaxiModel.getInstance().setRouteLineMarker(null); + TaxiModel.getInstance().cleanStation(); runOnUIThread(() -> { if(TaxiOrderStatusEnum.ArriveAtEnd.getCode() == order.orderStatus){ CallerOrderListenerManager.INSTANCE.invokeOrderStatus(false); diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt index 53af54db0f..d0cde4d8bf 100644 --- a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt @@ -100,6 +100,7 @@ class ChainConstant { //operation by user const val CHAIN_CODE_OCH_TAXI_START_AUTOPILOT = "CHAIN_CODE_OCH_TAXI_START_AUTOPILOT" + const val CHAIN_CODE_OCH_COMMON_DISTANCE = "CHAIN_CODE_OCH_COMMON_DISTANCE" const val CHAIN_CODE_EAGLE_START_AUTOPILOT = "CHAIN_CODE_EAGLE_START_AUTOPILOT" const val CHAIN_CODE_EAGLE_START_AUTOPILOT_RESULT = "CHAIN_CODE_EAGLE_START_AUTOPILOT_RESULT"