[6.0.0]

[autopilo]
[根据距离计算是否可用启动自驾]

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/biz/constant/OchCommonConst.kt

@@ -25,5 +25,8 @@ class OchCommonConst {
         const val LOGINSERVICE = "/ochbiz/common/login"
 
         const val BUSINESS_STRING = 100
+
+        // 自动驾驶自动规划的最大距离
+        const val AUTOMATIC_PLANNING_MAX_DISTANCE = 15
     }
 }

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

@@ -12,6 +12,8 @@ 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
+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
@@ -541,4 +543,87 @@ object  TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
         d(M_OCHCOMMON+ TAG,location)
     }
 
+
+    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}米"
+            }
+        }
+
+        if(mRoutePoints.isNullOrEmpty()){
+            // 判断距离起始站的距离
+            return distanceWithStartStation()
+        }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,无法启动自驾"
+                        }
+                    }
+                }
+            }
+        }
+        return "未能计算出车辆距离轨迹或站点的距离"
+    }
+
 }