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

[6.1.0]

Browse Source 
[代码解释]
This commit is contained in:
yangyakun
2023-09-25 20:55:55 +08:00
parent 9df1b4bfa8
commit 4e6ad6dc9c
2 changed files with 106 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/IDistanceListener.kt
View File

@@ -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
*/

165
OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/distancemamager/TrajectoryAndDistanceManager.kt
View File

@@ -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<String, ITrajectoryWithStationListener> =
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<MogoLocation>? = ArrayList()
// 0-1 1-2 2-3 各个轨迹点的距离
/**
* 0-1 1-2 2-3 各个轨迹点的距离
*/
@Volatile
private var mRoutePointsDistance: MutableList<Float>? = ArrayList()
// 所有轨迹点距离的和
/**
* 所有轨迹点距离的和
*/
@Volatile
private var maxDistanceAllPoint: Double = 0.0
private val stationDistance: ConcurrentHashMap<String, Float> = 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<String, Float> = 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<MessagePad.Location>?) {
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<Int, Boolean?, Float>
if (endStationInfo.isNext == true) {
val carLocationInfo: Triple<Int, Boolean?, Float> = 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<Int, Boolean?, Float>,
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<Int, Boolean?, Float>,
location: MogoLocation
@@ -539,6 +568,12 @@ object TrajectoryAndDistanceManager : IMoGoPlanningRottingListener {
}
}
/**
* 计算站点之间的轨迹(从开始站点算起到结束站点)
* routeArrivied 已经走过的轨迹点
* routeArriving 还没有走过的轨迹
* location 车的轨迹点
*/
private fun invokeTrajectoryWithStation(
carLocationInfo: Triple<Int, Boolean?, Float>,
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 ""
}
}