[6.0.0]

[distance]
[当前位置是否可启动自驾]

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()
 

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

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) {