diff --git a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/IDistanceListener.kt b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/IDistanceListener.kt index ec647b77f8..53a5f0425c 100644 --- a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/IDistanceListener.kt +++ b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/IDistanceListener.kt @@ -4,9 +4,13 @@ import com.mogo.eagle.core.data.map.MogoLocation interface IDistanceListener { /** - * @param distance 距离终点坐标的距离 + * @param distance 距离终点坐标的距离(终点坐标设置错误可能为负值) */ fun distanceCallback(distance: Float){} + + /** + * 两个站点之间的距离 + */ fun stationDistanceCallback(stationDistance:Float){} } @@ -26,8 +30,8 @@ interface ITrajectoryListener{ interface ITrajectoryWithStationListener{ /** - * @param routeArrivied 已经走过的坐标 第一个是站点坐标 - * @param routeArriving 没有走过的坐标 最后一个事站点坐标 + * @param routeArrivied 已经走过的坐标 第一个是开始站点坐标 + * @param routeArriving 没有走过的坐标 最后一个是结束站点坐标 * @param location 车的坐标 * @return */ diff --git a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/TrajectoryAndDistanceManager.kt b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/TrajectoryAndDistanceManager.kt index 7683fd22a8..bccff2c869 100644 --- a/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/TrajectoryAndDistanceManager.kt +++ b/OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/TrajectoryAndDistanceManager.kt @@ -1,7 +1,6 @@ package com.mogo.och.common.module.manager.distancemamager 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.CallerAutoPilotStatusListenerManager @@ -9,7 +8,6 @@ import com.mogo.eagle.core.function.call.autopilot.CallerChassisLocationGCJ02Lis 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.eagle.core.utilcode.util.LocationUtils @@ -17,7 +15,6 @@ import com.mogo.och.common.module.biz.constant.OchCommonConst import com.mogo.och.common.module.manager.loopmanager.BizLoopManager import com.mogo.och.common.module.manager.loopmanager.LoopInfo import com.mogo.och.common.module.utils.CoordinateCalculateRouteUtil -import com.zhjt.service.chain.ChainLog import io.reactivex.schedulers.Schedulers import mogo.telematics.pad.MessagePad import java.util.concurrent.ConcurrentHashMap @@ -35,8 +32,8 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { private val trajectoryWithStationListeners: ConcurrentHashMap = ConcurrentHashMap() - const val TAG = "DistanceManager" - const val TAGDISTANCE = "BusPassengerModelDistance" + private const val TAG = "DistanceManager" + private const val DISTANCE = "BusPassengerModelDistance" fun addDistanceListener(tag: String, listener: IDistanceListener) { @@ -53,6 +50,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { trajectoryListeners[tag] = listener } + @Suppress("unused") fun addTrajectoryWithStationListener(tag: String, listener: ITrajectoryWithStationListener) { if (trajectoryWithStationListeners.containsKey(tag)) { return @@ -71,30 +69,51 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { } + /** + * 所有轨迹点 + */ @Volatile private var mRoutePoints: MutableList? = ArrayList() - // 0-1 1-2 2-3 各个轨迹点的距离 + /** + * 0-1 1-2 2-3 各个轨迹点的距离 + */ + @Volatile private var mRoutePointsDistance: MutableList? = ArrayList() - // 所有轨迹点距离的和 + /** + * 所有轨迹点距离的和 + */ + @Volatile private var maxDistanceAllPoint: Double = 0.0 - private val stationDistance: ConcurrentHashMap = ConcurrentHashMap() + /** + * 线路Id + */ @Volatile private var lineId: Long? = null + /** + * 结束站点 + */ @Volatile private var endStationInfo: StationAndIndex = StationAndIndex(null, null, null, null) + /** + * 开始站点 + */ @Volatile private var startStationInfo: StationAndIndex = StationAndIndex(null, null, null, null) - //上一次计算最近点的缓存 - private var preCarLocationIndexInTrajectory = 0 + /** + * startStationInfo endStationInfo 站点之间的距离 + */ + private val stationDistance: ConcurrentHashMap = ConcurrentHashMap() - // TODO: 计算整个轨迹的长度 - // TODO: 计算轨迹点之间的长度、及轨迹点间最大长度 + /** + * 上一次(上一个tick)计算最近点的缓存 + */ + private var preCarLocationIndexInTrajectory = 0 init { CallerPlanningRottingListenerManager.addListener(TAG, this) @@ -108,6 +127,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { M_OCHCOMMON + TAG, "收到轨迹:轨迹个数${it.size}第一个点${it[0]}最后一个点:${it.last()} 轨迹id:${globalPathResp.lineId}" ) + @Suppress("SENSELESS_COMPARISON") if (globalPathResp.lineId != null) {// 适配低版本不传递lineId if (globalPathResp.lineId == lineId && !mRoutePoints.isNullOrEmpty()) { d(M_OCHCOMMON + TAG, "重复轨迹") @@ -125,8 +145,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { } private fun updateRoutePoints(routePoints: List?) { - mRoutePoints = null - mRoutePointsDistance = null + cleanRoutePoints() val latLngModels = CoordinateCalculateRouteUtil .coordinateConverterWgsToGcjLocations(AbsMogoApplication.getApp(), routePoints!!) mRoutePoints = latLngModels @@ -165,7 +184,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { this.startStationInfo.isNext = null } - fun cleanRoutePoints() { + private fun cleanRoutePoints() { mRoutePoints = null mRoutePointsDistance = null } @@ -256,6 +275,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { endCalculateDistanceLoop() } + @Suppress("unused") fun reStartCalculate() { startCalculateDistanceLoop() } @@ -265,8 +285,10 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { */ private fun startCalculateDistanceLoop() { BizLoopManager.setLoopFunction( - TAGDISTANCE, LoopInfo( - 1, ::calculateDistance, + DISTANCE, + LoopInfo( + 1, + ::calculateDistance, scheduler = Schedulers.computation() ) ) @@ -277,7 +299,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { * 结束启动路距计算 */ private fun endCalculateDistanceLoop() { - BizLoopManager.removeLoopFunction(TAGDISTANCE) + BizLoopManager.removeLoopFunction(DISTANCE) d(M_OCHCOMMON + TAG, "结束路距计算") } @@ -351,14 +373,13 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { e(M_OCHCOMMON + TAG, "计算两个站点间的距离") } - val carLocationInfo: Triple - if (endStationInfo.isNext == true) { + val carLocationInfo: Triple = if (endStationInfo.isNext == true) { // 计算车的位置在轨迹中的信息 这个是一个变量可以缓存 - carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo( + CoordinateCalculateRouteUtil.getNearestPointInfo( preCarLocationIndexInTrajectory, endStationInfo.index!!, mRoutePoints!!, location, 2 ) } else { - carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo( + CoordinateCalculateRouteUtil.getNearestPointInfo( preCarLocationIndexInTrajectory, endStationInfo.index!! + 1, mRoutePoints!!, @@ -422,7 +443,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { * 计算站点间距离 */ private fun calculateStationDistance() { - var lastSumLength = 0f + var lastSumLength: Float val key = getKey() if (stationDistance[key] != null) { return @@ -468,13 +489,15 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { return "${startStationInfo.stationPoint!!.longitude}${startStationInfo.stationPoint!!.latitude}${endStationInfo.stationPoint!!.longitude}${endStationInfo.stationPoint!!.latitude}" } - + /** + * 到结束站点的距离 站点设置错误可能为负值 + */ private fun invokeDistance( carLocationInfo: Triple, location: MogoLocation, locationInfo: String ) { - var lastSumLength = 0f + var lastSumLength: Float val stationIndex = endStationInfo.index ?: 0 if (carLocationInfo.first < stationIndex - 1) { @@ -496,7 +519,7 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { } else { val lastPoints = endStationInfo.stationPoint lastSumLength = CoordinateUtils.calculateLineDistance( - lastPoints!!.longitude, lastPoints!!.latitude, + lastPoints!!.longitude, lastPoints.latitude, location.longitude, location.latitude ) } @@ -515,6 +538,12 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { } } + /** + * 计算站点之间的轨迹(从轨迹起点算起到轨迹结束点结束) + * routeArrivied 已经走过的轨迹点 + * routeArriving 还没有走过的轨迹 + * location 车的轨迹点 + */ private fun invokeTrajectory( carLocationInfo: Triple, location: MogoLocation @@ -539,6 +568,12 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { } } + /** + * 计算站点之间的轨迹(从开始站点算起到结束站点) + * routeArrivied 已经走过的轨迹点 + * routeArriving 还没有走过的轨迹 + * location 车的轨迹点 + */ private fun invokeTrajectoryWithStation( carLocationInfo: Triple, location: MogoLocation @@ -603,6 +638,44 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { d(M_OCHCOMMON + TAG, location) } + /** + * 返回空为可启动自驾 + * 返回其他不可启动自驾 返回为原因 + */ + fun canStartAutopilot(lineId: Long?): String { + if (lineId == null) { + return "请确认线路ID" + } + + try { + if (mRoutePoints.isNullOrEmpty()) { + // 判断距离起始站的距离 + // 没有收到过轨迹 + // 收到过轨迹 且底盘没有在自动驾驶中(没办法申请轨迹) 自驾中重启底盘的形式 + val redCatche = TrajectoryCache.redCatche(lineId.toString()) + return if (redCatche.isNullOrEmpty()) { + distanceWithStartStation() + } else { + distanceWithTrajectory(redCatche) + } + + } else { + return if (this.lineId == 0L || this.lineId == null) { + distanceWithTrajectory(mRoutePoints!!) + } else { + if (lineId != this.lineId) { + // 判断距离起始站的距离 + distanceWithStartStation() + } else { + distanceWithTrajectory(mRoutePoints!!) + } + } + } + } catch (e: Exception) { + e.printStackTrace() + } + return "" + } /** * 距离站点的距离 @@ -682,44 +755,4 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener { } } - - /** - * 返回空为可启动自驾 - * 返回其他不可启动自驾 返回为原因 - */ - fun canStartAutopilot(lineId: Long?): String { - if (lineId == null) { - return "请确认线路ID" - } - - try { - if (mRoutePoints.isNullOrEmpty()) { - // 判断距离起始站的距离 - // 没有收到过轨迹 - // 收到过轨迹 且底盘没有在自动驾驶中(没办法申请轨迹) 自驾中重启底盘的形式 - val redCatche = TrajectoryCache.redCatche(lineId.toString()) - return if (redCatche.isNullOrEmpty()) { - distanceWithStartStation() - } else { - distanceWithTrajectory(redCatche) - } - - } else { - return if (this.lineId == 0L || this.lineId == null) { - distanceWithTrajectory(mRoutePoints!!) - } else { - if (lineId != this.lineId) { - // 判断距离起始站的距离 - distanceWithStartStation() - } else { - distanceWithTrajectory(mRoutePoints!!) - } - } - } - } catch (e: Exception) { - e.printStackTrace() - } - return "" - } - } \ No newline at end of file