diff --git 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 index 0a8713c2d0..27989afe50 100644 --- 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() 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 872447d708..ed0a0b4c39 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 @@ -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 = 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 = - 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 = + 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 "" + } + } \ No newline at end of file diff --git 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 index 164681283a..10bbe0ded4 100644 --- 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, location: MogoLocation, type:Int, - size:Int = 4 + size:Int = 4, + useHeading:Boolean = true, ): Triple { 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) {