[6.0.0]

[distance] [当前位置是否可启动自驾]
2023-09-04 14:55:53 +08:00
parent a6eea5aa53
commit a45f9cc025
3 changed files with 117 additions and 77 deletions
--- a/OCH/mogo-och-common-module/src/debug/java/com/mogo/och/common/module/debug/DebugDataDispatch.kt
+++ b/OCH/mogo-och-common-module/src/debug/java/com/mogo/och/common/module/debug/DebugDataDispatch.kt
@@ -50,7 +50,7 @@ object DebugDataDispatch {
        when (type) {
            globalPathMock -> {
                sourceFilePath?.let {
-                    loadRawPoints(ROOT_PATH+it)
+                    loadRawPoints(ROOT_PATH+it,it)
                }
            }
            locationMock -> {
@@ -112,7 +112,7 @@ object DebugDataDispatch {
        }
    }

-    fun loadRawPoints(path:String) {
+    fun loadRawPoints(path:String,name:String) {
        val inputStream = FileInputStream(path)
        val reader = BufferedReader(InputStreamReader(inputStream))
        val jsonStr = StringBuilder()
@@ -136,6 +136,11 @@ object DebugDataDispatch {
            locationBuilder.longitude = latLng.longitude
            newBuilder.addWayPoints(locationBuilder)
        }
+        try {
+            newBuilder.lineId = name.toLong()
+        } catch (e: Exception) {
+            e.printStackTrace()
+        }

        val mogoLocation = MogoLocation()

--- 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
@@ -22,6 +22,9 @@ import io.reactivex.schedulers.Schedulers
 import mogo.telematics.pad.MessagePad
 import java.util.concurrent.ConcurrentHashMap

+/**
+ * 计算当前位置距离站点距离和走过的和未走过的轨迹点
+ */
 object  TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{

    private val distanceListeners: ConcurrentHashMap<String, IDistanceListener> = ConcurrentHashMap()
@@ -166,10 +169,12 @@ object  TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
                isSameStation(this.endStationInfo.stationPoint,endStationInfo)){
                if(this.lineId!=lineId){
                    resetStation()
+                    cleanRoutePoints()
                    stationDistance.clear()
                }
            }else{
                resetStation()
+                cleanRoutePoints()
                stationDistance.clear()
            }
            this.endStationInfo.stationPoint = endStationInfo
@@ -544,86 +549,115 @@ object  TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
    }


-    fun canStartAutopilot(lineId: Long):String{
-
-        fun distanceWithStartStation():String{
-            if (startStationInfo.stationPoint==null) {
-                return "请选择起始结束站点"
-            }
-            val currentPoint = CallerChassisLocationGCJ02ListenerManager.getChassisLocationGCJ02()
-            val distance = CoordinateUtils.calculateLineDistance(
-                currentPoint.longitude,
-                currentPoint.latitude,
-                startStationInfo.stationPoint!!.longitude,
-                startStationInfo.stationPoint!!.latitude
-            )
-            if(distance <= OchCommonConst.AUTOMATIC_PLANNING_MAX_DISTANCE){
-                return ""
-            }else{
-                return "距离起始站点过远：${distance}米"
-            }
+    /**
+     * 距离站点的距离
+     */
+    private fun distanceWithStartStation():String{
+        if (startStationInfo.stationPoint==null) {
+            return "请选择起始结束站点"
        }
-
-        if(mRoutePoints.isNullOrEmpty()){
-            // 判断距离起始站的距离
-            return distanceWithStartStation()
+        val currentPoint = CallerChassisLocationGCJ02ListenerManager.getChassisLocationGCJ02()
+        val distance = CoordinateUtils.calculateLineDistance(
+            currentPoint.longitude,
+            currentPoint.latitude,
+            startStationInfo.stationPoint!!.longitude,
+            startStationInfo.stationPoint!!.latitude
+        )
+        return if(distance <= OchCommonConst.AUTOMATIC_PLANNING_MAX_DISTANCE){
+            ""
        }else{
-            if(lineId!=this.lineId){
-                // 判断距离起始站的距离
-                return distanceWithStartStation()
-            }else{
-                val currentPoint = CallerChassisLocationGCJ02ListenerManager.getChassisLocationGCJ02()
-                // 判断距离轨迹的距离
-                val carLocationInfo: Triple<Int, Boolean?, Float> =
-                    CoordinateCalculateRouteUtil.getNearestPointInfo(
-                        preCarLocationIndexInTrajectory,
-                        mRoutePoints!!.size - 1,
-                        mRoutePoints!!,
-                        currentPoint,
-                        2
-                    )
-                if(carLocationInfo.first<=OchCommonConst.AUTOMATIC_PLANNING_MAX_DISTANCE){
-                    return ""// 可以启动自驾
-                }else{
-                    mRoutePoints?.let {
-                        // 判断距离线段的距离  垂足的距离
-                        val nextPoint:MogoLocation
-                        val prePoint:MogoLocation
-                        if(carLocationInfo.second==true){
-                            if(carLocationInfo.first>0){
-                                nextPoint = it[carLocationInfo.first]
-                                prePoint = it[carLocationInfo.first-1]
-                            } else {
-                                // 距离第一个点大于15m 过远
-                                return "距离第一个轨迹点超过15m：${carLocationInfo.first}米"
-                            }
-                        }else{
-                            if (carLocationInfo.first + 1 < it.size) {
-                                nextPoint = it[carLocationInfo.first + 1]
-                                prePoint = it[carLocationInfo.first]
-                            }else{
-                                nextPoint = it[carLocationInfo.first ]
-                                prePoint = it[carLocationInfo.first-1]
-                            }
-                        }
-                        val pointToLine = LocationUtils.pointToLine(
-                            prePoint.longitude,
-                            prePoint.latitude,
-                            nextPoint.longitude,
-                            nextPoint.latitude,
-                            currentPoint.longitude,
-                            currentPoint.latitude
-                        )
-                        if(pointToLine<=OchCommonConst.AUTOMATIC_PLANNING_MAX_DISTANCE){
-                            return ""
-                        }else{
-                            return "距离轨迹线的距离大于15m,无法启动自驾"
-                        }
+            "距离起始站点过远：${distance}米"
+        }
+    }
+
+    /**
+     * 距离轨迹的距离
+     */
+    private fun distanceWithTrajectory():String{
+        val currentPoint =
+            CallerChassisLocationGCJ02ListenerManager.getChassisLocationGCJ02()
+        // 判断距离轨迹的距离
+        val carLocationInfo: Triple<Int, Boolean?, Float> =
+            CoordinateCalculateRouteUtil.getNearestPointInfo(
+                0,
+                mRoutePoints!!.size - 1,
+                mRoutePoints!!,
+                currentPoint,
+                2,
+                useHeading = false
+            )
+        if (carLocationInfo.third <= OchCommonConst.AUTOMATIC_PLANNING_MAX_DISTANCE) {
+            return ""// 可以启动自驾
+        } else {
+            mRoutePoints?.let {
+                // 判断距离线段的距离  垂足的距离
+                val nextPoint: MogoLocation
+                val prePoint: MogoLocation
+                if (carLocationInfo.second == true) {
+                    if (carLocationInfo.first > 0) {
+                        nextPoint = it[carLocationInfo.first]
+                        prePoint = it[carLocationInfo.first - 1]
+                    } else {
+                        // 距离第一个点大于15m 过远
+                        return "距离第一个轨迹点超过15m：${carLocationInfo.first}米"
                    }
+                } else {
+                    if (carLocationInfo.first + 1 < it.size) {
+                        nextPoint = it[carLocationInfo.first + 1]
+                        prePoint = it[carLocationInfo.first]
+                    } else {
+                        nextPoint = it[carLocationInfo.first]
+                        prePoint = it[carLocationInfo.first - 1]
+                    }
+                }
+                val pointToLine = LocationUtils.pointToLine(
+                    prePoint.longitude,
+                    prePoint.latitude,
+                    nextPoint.longitude,
+                    nextPoint.latitude,
+                    currentPoint.longitude,
+                    currentPoint.latitude
+                )
+                if (pointToLine <= OchCommonConst.AUTOMATIC_PLANNING_MAX_DISTANCE) {
+                    return ""
+                } else {
+                    return "距离轨迹线的距离大于15m,无法启动自驾"
                }
            }
        }
        return "未能计算出车辆距离轨迹或站点的距离"
    }

+
+    /**
+     * 返回空为可启动自驾
+     * 返回其他不可启动自驾 返回为原因
+     */
+    fun canStartAutopilot(lineId: Long?):String{
+        if(lineId==null){
+            return "请确认线路ID"
+        }
+
+        try {
+            if(mRoutePoints.isNullOrEmpty()){
+                // 判断距离起始站的距离
+                return distanceWithStartStation()
+            }else {
+                return if (this.lineId == 0L || this.lineId == null) {
+                    distanceWithTrajectory()
+                } else {
+                    if (lineId != this.lineId) {
+                        // 判断距离起始站的距离
+                        distanceWithStartStation()
+                    } else {
+                        distanceWithTrajectory()
+                    }
+                }
+            }
+        }catch (e:Exception){
+            e.printStackTrace()
+        }
+        return ""
+    }
+
 }
--- 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
@@ -599,7 +599,8 @@ object CoordinateCalculateRouteUtil {
        mRoutePoints: List<MogoLocation>,
        location: MogoLocation,
        type:Int,
-        size:Int = 4
+        size:Int = 4,
+        useHeading:Boolean = true,
    ): Triple<Int,Boolean?,Float> {
        val startTime = System.currentTimeMillis()
        Logger.d(SceneConstant.M_OCHCOMMON + "calculateRouteSumLength",
@@ -711,7 +712,7 @@ object CoordinateCalculateRouteUtil {
                key.recycle()
                iterator.remove()
            } else {
-                if (location.heading != 0.0) {
+                if (location.heading != 0.0 && useHeading) {
                    key.degree?.let {
                        val dexAngle = DrivingDirectionUtils.getAngleDiff(location.heading, it)
                        if (dexAngle > 90) {