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

[3.3.0]

Browse Source 
[feature]
[边界处理、设置站点和清理站点]
This commit is contained in:
yangyakun
2023-06-20 13:56:12 +08:00
parent bf57f3053a
commit 6ee8fdab68
5 changed files with 219 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,19 +1,21 @@
package com.mogo.och.common.module.manager.trajectorymamager
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.CallerChassisLocationGCJ02ListenerManager
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.och.common.module.manager.loopmanager.BusPassengerModelLoopManager
import com.mogo.och.common.module.manager.loopmanager.LoopInfo
import com.mogo.och.common.module.utils.CoordinateCalculateRouteUtil
import com.zhjt.service.chain.ChainLog
import mogo.telematics.pad.MessagePad
import java.util.ArrayList
import java.util.concurrent.ConcurrentHashMap
object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
@@ -54,6 +56,9 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
@Volatile
private var mRoutePoints: MutableList<MogoLocation>? = ArrayList()
@Volatile
private var lineId:Long? = null
@Volatile
private var endStationInfo: StationAndIndex = StationAndIndex(null, null, null, null)
@@ -72,7 +77,13 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
globalPathResp?.wayPointsList?.let {
if (it.size > 0) {
d(M_OCHCOMMON + TAG, "收到轨迹:${it.size}第一个点${it[0]}最后一个点:${it.last()}")
if(globalPathResp.lineId == lineId){
d(M_OCHCOMMON + TAG, "重复轨迹")
startCalculateDistanceLoop()
return
}
endCalculateDistanceLoop()
this.lineId = globalPathResp.lineId
updateRoutePoints(it)
preCarLocationIndexInTrajectory = 0
startCalculateDistanceLoop()
@@ -128,7 +139,6 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
endCalculateDistanceLoop()
return
}
d(M_OCHCOMMON + TAG, "站点坐标：${endStationInfo.stationPoint}")
calculateRouteSumLength(it)
}
}
@@ -151,7 +161,7 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
@Synchronized
private fun calculateRouteSumLength(
fun calculateRouteSumLength(
location: MogoLocation,
) {
if (mRoutePoints.isNullOrEmpty()) return
@@ -163,25 +173,15 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
) {
//要前往的站在轨迹中对应的点的信息
val startStationInfoInTra = CoordinateCalculateRouteUtil.getNearestPointInfo(
preCarLocationIndexInTrajectory, mRoutePoints!!, startStationInfo.stationPoint!!
preCarLocationIndexInTrajectory, mRoutePoints!!.size,mRoutePoints!!, startStationInfo.stationPoint!!,1
)
startStationInfo.isNext = startStationInfoInTra.second
startStationInfo.index = startStationInfoInTra.first
startStationInfo.distance = startStationInfoInTra.third
preCarLocationIndexInTrajectory = startStationInfoInTra.first
writeLog(startStationInfoInTra,location)
}
// 计算车的位置在轨迹中的信息 这个是一个变量可以缓存
val carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo(
preCarLocationIndexInTrajectory, mRoutePoints!!, location
)
if(carLocationInfo.second==null||carLocationInfo.third>1_000){
preCarLocationIndexInTrajectory = 0
return
}
d(M_OCHCOMMON+ TAG,"距离车最近的点:${carLocationInfo.first} 点下后面的轨迹：${carLocationInfo.second} 距离点的距离：${carLocationInfo.third}")
preCarLocationIndexInTrajectory = carLocationInfo.first
// 计算结束站点在轨迹中的信息 这个是一个常量可以缓存
if (endStationInfo.stationPoint != null
&& endStationInfo.isNext == null
@@ -190,24 +190,58 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
) {
//要前往的站在轨迹中对应的点
val stationPointInRouteIndex = CoordinateCalculateRouteUtil.getNearestPointInfo(
preCarLocationIndexInTrajectory, mRoutePoints!!, endStationInfo.stationPoint!!
preCarLocationIndexInTrajectory,mRoutePoints!!.size, mRoutePoints!!, endStationInfo.stationPoint!!,3
)
endStationInfo.isNext = stationPointInRouteIndex.second
endStationInfo.index = stationPointInRouteIndex.first
endStationInfo.distance = stationPointInRouteIndex.third
writeLog(stationPointInRouteIndex, location)
}
val carLocationInfo:Triple<Int,Boolean?,Float>
if(endStationInfo.isNext==true){
// 计算车的位置在轨迹中的信息 这个是一个变量可以缓存
carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo(
preCarLocationIndexInTrajectory,endStationInfo.index!!, mRoutePoints!!, location,2
)
}else{
carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo(
preCarLocationIndexInTrajectory,endStationInfo.index!!+1, mRoutePoints!!, location,2
)
}
writeLog(carLocationInfo, location)
if(carLocationInfo.second==null||carLocationInfo.third>1_000){
preCarLocationIndexInTrajectory = 0
return
}
preCarLocationIndexInTrajectory = carLocationInfo.first
// 距离回调
if(distanceListeners.size>0) {
invokeDistance(carLocationInfo, location)
try {
if(distanceListeners.size>0) {
invokeDistance(carLocationInfo, location)
}
}catch (e:Exception){
e(M_OCHCOMMON+ TAG,"距离计算错误")
}
// 不带站点轨迹回调
if(trajectoryListeners.size>0) {
invokeTrajectory(carLocationInfo, location)
try {
if(trajectoryListeners.size>0) {
invokeTrajectory(carLocationInfo, location)
}
}catch (e:Exception){
e(M_OCHCOMMON+ TAG,"轨迹线(轨迹两头)计算错误")
}
// 只展示站点之间轨迹
if(trajectoryWithStationListeners.size>0) {
invokeTrajectoryWithStation(carLocationInfo, location)
try {
if(trajectoryWithStationListeners.size>0) {
invokeTrajectoryWithStation(carLocationInfo, location)
}
}catch (e:Exception){
e(M_OCHCOMMON+ TAG,"轨迹线(站点两头)计算错误")
}
}
@@ -303,6 +337,7 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
endCut = endStationInfo.index!!+1
}
}
d(M_OCHCOMMON+ TAG,"根据站点切个：第一个点：$fromCut 最后一个点$endCut")
if(carLocationInfo.second==true){// isNext = true
routeArrivied.addAll(mRoutePoints!!.subList(fromCut,carLocationInfo.first))
routeArriving.addAll(mRoutePoints!!.subList(carLocationInfo.first,endCut))
@@ -323,4 +358,15 @@ object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
}
}
@ChainLog(
linkChainLog = ChainConstant.CHAIN_TYPE_SOCKET_AUTOPILOT,
linkCode = ChainConstant.CHAIN_SOURCE_ADAS,
nodeAliasCode = ChainConstant.CHAIN_CODE_OCH_COMMON_DISTANCE,
paramIndexes = [0,1]
)
fun writeLog(carLocationInfo: Triple<Int, Boolean?, Float>, location: MogoLocation) {
d(M_OCHCOMMON+ TAG,"距离车最近的点:${carLocationInfo.first} 当前位置在点的前后：${carLocationInfo.second} 距离点的距离：${carLocationInfo.third}")
d(M_OCHCOMMON+ TAG,"定位信息:${location}")
}
}

162
OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/utils/CoordinateCalculateRouteUtil.kt
View File

@@ -12,6 +12,7 @@ import com.mogo.och.common.module.manager.trajectorymamager.DistanceDegree
import mogo.telematics.pad.MessagePad
import java.util.TreeMap
import kotlin.math.acos
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.pow
import kotlin.math.sin
@@ -547,23 +548,44 @@ object CoordinateCalculateRouteUtil {
}
fun getHeadingAngle(location: MogoLocation, nextPoint: MogoLocation): Double {
val y = Math.sin(nextPoint.longitude - location.longitude) * Math.cos(nextPoint.latitude)
val x = Math.cos(location.latitude) * Math.sin(nextPoint.latitude) - Math.sin(
location.latitude
) *
Math.cos(nextPoint.latitude) * Math.cos(nextPoint.longitude - location.longitude)
var bearing = Math.atan2(y, x)
return getHeadingAngle(
location.longitude,
location.latitude,
nextPoint.longitude,
nextPoint.latitude
)
}
fun getHeadingAngle(location: LatLng, nextPoint: LatLng): Double {
return getHeadingAngle(
location.longitude,
location.latitude,
nextPoint.longitude,
nextPoint.latitude
)
}
fun getHeadingAngle(locationLongitude: Double, locationLatitude: Double,
nextPointLongitude: Double, nextPointLatitude: Double): Double {
val y = sin(nextPointLongitude - locationLongitude) * cos(nextPointLatitude)
val x = cos(locationLatitude) * sin(nextPointLatitude) - sin(locationLatitude) *
cos(nextPointLatitude) * cos(nextPointLongitude - locationLongitude)
var bearing = atan2(y, x)
bearing = Math.toDegrees(bearing)
if (bearing < 0) {
bearing += 360.0
}
return bearing
return 360-bearing
}
/**
* 支持带航行角的和不带航向角的
* 带航向角的会删除 航向角差大于90°的点
* @param preIndex 上次计算缓存
* @param mRoutePoints 轨迹点
* @param location 目标坐标
* @param type 1 开始站点 2 定位点 3 结束站点
* @return Triple<Int,Boolean?,Float>
* 距离目标坐标最近的轨迹点下标、
* 最近点坐标是目标坐标的上一个点还是下一个点
@@ -572,13 +594,16 @@ object CoordinateCalculateRouteUtil {
@JvmStatic
fun getNearestPointInfo(
preIndex: Int,
endIndex: Int,
mRoutePoints: List<MogoLocation>,
location: MogoLocation,
type:Int, //
size:Int = 8
): Triple<Int,Boolean?,Float> {
var currentIndex:Int = preIndex //记录疑似点 //基础点
// 轨迹中的点和定位点的距离集合
val distanceMap: TreeMap<DistanceDegree, Int> = TreeMap()
for (index in preIndex until mRoutePoints.size) {
for (index in preIndex until endIndex) {
val latLngIndex = mRoutePoints[index]
val distance = CoordinateUtils.calculateLineDistance(
location.longitude,
@@ -587,7 +612,7 @@ object CoordinateCalculateRouteUtil {
latLngIndex.latitude
)
distanceMap[DistanceDegree(distance, null,null)] = index
if (distanceMap.size > 4) {
if (distanceMap.size > size) {
distanceMap.pollLastEntry()
}
}
@@ -600,49 +625,81 @@ object CoordinateCalculateRouteUtil {
if (pointIndex + 1 < mRoutePoints.size) {
val nextPoint = mRoutePoints[pointIndex + 1]
nextDegree = CoordinateCalculateRouteUtil.getDegree(location, currentPoint, nextPoint)
nextDegree = getDegree(location, currentPoint, nextPoint)
}else{
nextDegree = 135.0
}
// 需要减距离 和上一个轨迹点成钝角
if (pointIndex - 1 >= 0) {
val prePoint = mRoutePoints[pointIndex - 1]
preDegree = CoordinateCalculateRouteUtil.getDegree(
preDegree = getDegree(
location,
currentPoint,
prePoint
)
}else{// 第一个个点处理
preDegree = 135.0
}
if(nextDegree>90){
fun getDegreeNext(){
if (pointIndex + 1 < mRoutePoints.size) {
val nextPoint = mRoutePoints[pointIndex + 1]
val headingAngle = CoordinateCalculateRouteUtil.getHeadingAngle(currentPoint, nextPoint)
val headingAngle = getHeadingAngle(currentPoint, nextPoint)
distanceDegree.degree = headingAngle
distanceDegree.isNext = false
}else{
val prePoint = mRoutePoints[pointIndex - 1]
val headingAngle = getHeadingAngle(prePoint,currentPoint)
distanceDegree.degree = headingAngle
distanceDegree.isNext = true
}
}
fun getDegreePre(){
if (pointIndex - 1 >= 0) {
val prePoint = mRoutePoints[pointIndex - 1]
val headingAngle = getHeadingAngle(prePoint, currentPoint)
distanceDegree.degree = headingAngle;
distanceDegree.isNext = true
}else{
val nextPoint = mRoutePoints[pointIndex + 1]
val headingAngle = getHeadingAngle(currentPoint,nextPoint)
distanceDegree.degree = headingAngle
distanceDegree.isNext = false
}
}
if(preDegree>90){
if (pointIndex - 1 >= 0) {
val prePoint = mRoutePoints[pointIndex - 1]
val headingAngle = CoordinateCalculateRouteUtil.getHeadingAngle(prePoint, currentPoint)
distanceDegree.degree = headingAngle;
distanceDegree.isNext = true
}
if(nextDegree>90){
getDegreeNext()
}
// 距离过近直接返回
if(distanceDegree.distance<1){
currentIndex = pointIndex
return Triple(currentIndex,distanceDegree.isNext,distanceDegree.distance)
if(preDegree>90){
getDegreePre()
}
if(preDegree<90&&nextDegree<90&&preDegree+nextDegree>90){
if (pointIndex + 1 < mRoutePoints.size&&pointIndex - 1 >= 0) {
val nextPoint = mRoutePoints[pointIndex + 1]
val prePoint = mRoutePoints[pointIndex - 1]
val degree = getDegree(currentPoint, prePoint, nextPoint)
if(degree>90) {
getDegreePre()
// isNext 值无所谓了 就 ture和false都行
// 通过航向角过滤一遍
if(distanceDegree.degree!=null&&DrivingDirectionUtils.getAngleDiff(location.heading, distanceDegree.degree!!)<90){
currentIndex = pointIndex
return Triple(currentIndex,distanceDegree.isNext,distanceDegree.distance)
}
}
}
}
}
// 根据角度来排除一些点
val iterator = distanceMap.iterator()
// 有航向角比较航向角
if(location.heading!=0.0) {
while (iterator.hasNext()) {
val next = iterator.next()
val key = next.key
if (key.degree == null) {
iterator.remove()
} else {
while (iterator.hasNext()) {
val next = iterator.next()
val key = next.key
if (key.degree == null) {
iterator.remove()
} else {
if (location.heading != 0.0) {
key.degree?.let {
val dexAngle = DrivingDirectionUtils.getAngleDiff(location.heading, it)
if (dexAngle > 90) {
@@ -653,7 +710,7 @@ object CoordinateCalculateRouteUtil {
}
}
// 上一次查询此次最近点包含上个点和下个点
// 最近点中包含上次计算的点和上次计算的最近的一个点
if(distanceMap.containsValue(preIndex)&&distanceMap.containsValue(preIndex+1)){
var preIndexDistance:DistanceDegree?=null
var preIndexNextDistance:DistanceDegree?=null
@@ -677,7 +734,45 @@ object CoordinateCalculateRouteUtil {
}
}
// 最近的点 只有一个前面的点
var maxIndex = 0
var minIndex = Int.MAX_VALUE
distanceMap.iterator().forEach { en ->
val value = en.value
if(value<minIndex){
minIndex = value
}
if(value>maxIndex){
maxIndex = value
}
}
val middleVale =minIndex + (maxIndex-minIndex)/2
when (type) {
1 -> {// 求开始站点
val iteratorRemove = distanceMap.iterator()
while (iteratorRemove.hasNext()) {
val next = iteratorRemove.next()
val key = next.key
if(next.value>middleVale){
iteratorRemove.remove()
}
}
}
3 -> {// 求结束站点
val iteratorRemove = distanceMap.iterator()
while (iteratorRemove.hasNext()) {
val next = iteratorRemove.next()
val key = next.key
if(next.value<middleVale){
iteratorRemove.remove()
}
}
}
else -> {}
}
// 根据距离来计算 最近的点 只有一个前面的点
var tempDistance = Float.MAX_VALUE
var isNext:Boolean? = null
distanceMap.iterator().forEach { en ->
@@ -696,6 +791,7 @@ object CoordinateCalculateRouteUtil {
}
}
}
return Triple(currentIndex,isNext,tempDistance)
}
}

18
OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java
View File

@@ -49,6 +49,7 @@ import com.mogo.och.common.module.biz.provider.LoginService;
import com.mogo.och.common.module.callback.OchAdasStartFailureCallback;
import com.mogo.och.common.module.manager.AbnormalFactorsLoopManager;
import com.mogo.och.common.module.manager.OCHAdasAbilityManager;
import com.mogo.och.common.module.manager.trajectorymamager.TrajectoryAndDistanceManager;
import com.mogo.och.common.module.map.AmapNaviToDestinationModel;
import com.mogo.och.common.module.utils.CoordinateCalculateRouteUtil;
import com.mogo.och.common.module.utils.PinYinUtil;
@@ -1634,4 +1635,21 @@ public class TaxiModel {
}
}
public void setStation(){
OrderQueryRespBean.Result currentOCHOrder = getCurrentOCHOrder();
if(currentOCHOrder!=null){
MogoLocation startStation = new MogoLocation();
startStation.setLongitude(currentOCHOrder.startSiteGcjPoint.get(0));
startStation.setLatitude(currentOCHOrder.startSiteGcjPoint.get(1));
MogoLocation endStation = new MogoLocation();
endStation.setLongitude(currentOCHOrder.endSiteGcjPoint.get(0));
endStation.setLatitude(currentOCHOrder.endSiteGcjPoint.get(1));
TrajectoryAndDistanceManager.INSTANCE.setStationPoint(startStation,endStation);
}
}
public void cleanStation(){
TrajectoryAndDistanceManager.INSTANCE.setStationPoint(null,null);
}
}

3
OCH/mogo-och-taxi/src/main/java/com/mogo/och/taxi/presenter/TaxiPresenter.java
View File

@@ -240,10 +240,12 @@ public class TaxiPresenter extends Presenter<TaxiFragment> implements ITaxiADASS
CallerLogger.INSTANCE.d(M_TAXI + TAG,"order = "+order.toString());
if (TaxiOrderStatusEnum.UserArriveAtStart.getCode() == order.orderStatus){
TaxiModel.getInstance().queryAutopilotStatus(true);
TaxiModel.getInstance().setStation();
}
if (TaxiOrderStatusEnum.OnTheWayToEnd.getCode() == order.orderStatus){
TaxiModel.getInstance().startDynamicCalculateRouteInfo();
TaxiModel.getInstance().setStation();
runOnUIThread(() -> {
mView.updateCtvAutopilotStatusTag(true);
CallerOrderListenerManager.INSTANCE.invokeOrderStatus(true);
@@ -256,6 +258,7 @@ public class TaxiPresenter extends Presenter<TaxiFragment> implements ITaxiADASS
TaxiOrderStatusEnum.JourneyCompleted.getCode() == order.orderStatus){
TaxiModel.getInstance().startOrStopCalculateRouteInfo(false);
TaxiModel.getInstance().setRouteLineMarker(null);
TaxiModel.getInstance().cleanStation();
runOnUIThread(() -> {
if(TaxiOrderStatusEnum.ArriveAtEnd.getCode() == order.orderStatus){
CallerOrderListenerManager.INSTANCE.invokeOrderStatus(false);

1
core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/deva/chain/ChainConstant.kt
View File

@@ -100,6 +100,7 @@ class ChainConstant {
//operation by user
const val CHAIN_CODE_OCH_TAXI_START_AUTOPILOT = "CHAIN_CODE_OCH_TAXI_START_AUTOPILOT"
const val CHAIN_CODE_OCH_COMMON_DISTANCE = "CHAIN_CODE_OCH_COMMON_DISTANCE"
const val CHAIN_CODE_EAGLE_START_AUTOPILOT = "CHAIN_CODE_EAGLE_START_AUTOPILOT"
const val CHAIN_CODE_EAGLE_START_AUTOPILOT_RESULT = "CHAIN_CODE_EAGLE_START_AUTOPILOT_RESULT"