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

[V2X][Road]优化代码逻辑

Browse Source
This commit is contained in:
renwj
2022-09-27 11:32:09 +08:00
parent 03f21e6dd8
commit 0a34234f8b
1 changed files with 26 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/events/scenario/scene/airoad/AiRoadMarker.kt
View File

@@ -7,10 +7,10 @@ import androidx.lifecycle.*
import androidx.lifecycle.Lifecycle.Event
import androidx.lifecycle.Lifecycle.Event.ON_CREATE
import androidx.lifecycle.Lifecycle.Event.ON_DESTROY
import com.mogo.cloud.commons.utils.*
import com.mogo.eagle.core.data.map.*
import com.mogo.eagle.core.data.map.MapRoadInfo.StopLine
import com.mogo.eagle.core.function.api.map.listener.*
import com.mogo.eagle.core.function.call.autopilot.*
import com.mogo.eagle.core.function.call.map.*
import com.mogo.eagle.core.function.call.map.CallerMapRoadListenerManager.OnRoadListener
import com.mogo.eagle.core.utilcode.kotlin.*
@@ -44,10 +44,6 @@ object AiRoadMarker {
private val redLineColors = arrayListOf(Color.parseColor("#002ABAD9"), Color.parseColor("#FF7A30"))
// private val blueLine by lazy { AtomicReference<IMogoPolyline>() }
//
// private val blueLineColors = arrayListOf(Color.parseColor("#002ABAD9"), Color.parseColor("#30A3FF"))
private val markerCache = ConcurrentSet<Marker>()
private val redLineOptions by lazy {
@@ -60,16 +56,6 @@ object AiRoadMarker {
}
}
// private val blueLineOptions by lazy {
// MogoPolylineOptions().apply {
// zIndex(30000f)
// setGps(true)
// width(40f)
// useGradient(true)
// colorValues(blueLineColors)
// }
// }
fun init(ctx: Context) {
ctx.lifeCycleOwner.lifecycle.addObserver(object : LifecycleEventObserver {
override fun onStateChanged(source: LifecycleOwner, event: Event) {
@@ -84,16 +70,16 @@ object AiRoadMarker {
pool.execute {
var interrupted = false
Logger.d(TAG, "--- consumer --- 0 ---")
Log.d(TAG, "--- consumer --- 0 ---")
while (!interrupted) {
try {
Logger.d(TAG, "--- consumer --- 1 ---")
Log.d(TAG, "--- consumer --- 1 ---")
val top = queue.take() ?: continue
Logger.d(TAG, "--- consumer --- 2 ---")
Log.d(TAG, "--- consumer --- 2 ---")
val location = carLocation.get() ?: continue
val isFrontOfCar = isFrontOfCar(top.poi_lon, top.poi_lat, location.first, location.second, location.third)
val isSameRoad = isSameRoad(top.poi_lon, top.poi_lat, location.first, location.second, location.third)
Logger.d(TAG, "--- consumer --- 3 ---isFrontOfCar: $isFrontOfCar, isSameRoad: $isSameRoad")
Log.d(TAG, "--- consumer --- 3 ---isFrontOfCar: $isFrontOfCar, isSameRoad: $isSameRoad")
if (isFrontOfCar && isSameRoad) {
marker.set(top)
marker(top)
@@ -110,9 +96,9 @@ object AiRoadMarker {
}
private fun isSameRoad(x1: Double, y1: Double, x2: Double, y2: Double, angle: Double): Boolean {
val c1 = MogoMap.getInstance().mogoMap.getCenterLineRangeInfo(x1, y1, angle.toFloat(), 10f)
val c2 = MogoMap.getInstance().mogoMap.getCenterLineRangeInfo(x2, y2, angle.toFloat(), 10f)
Logger.d(TAG, "isSameRoad--: $c1 ==> $c2")
val c1 = MogoMap.getInstance().mogoMap.getCenterLineRangeInfo(x1, y1, angle.toFloat(), 500f)
val c2 = MogoMap.getInstance().mogoMap.getCenterLineRangeInfo(x2, y2, angle.toFloat(), 500f)
Log.d(TAG, "isSameRoad--: [$x1, $y1, ${c1.road_id}] ==> [$x2, $y2, ${c2.road_id}]")
return c1 != null && c2 != null && c1.road_id.isNotEmpty() && c1.road_id == c2.road_id
}
@@ -121,7 +107,7 @@ object AiRoadMarker {
private var hasCrossRoad = false
override fun onRoadIdInfo(roadId: String) {
Logger.d(TAG, "--- onRoadInfo --- roadId: $roadId")
Log.d(TAG, "--- onRoadInfo --- roadId: $roadId")
val oldRoadId = this.roadId
if (oldRoadId != roadId) {
if (hasCrossRoad) {
@@ -131,7 +117,7 @@ object AiRoadMarker {
this.roadId = roadId
}
override fun onStopLineInfo(info: StopLine) {
Logger.d(TAG, "--- onStopLineInfo --- info: $info")
Log.d(TAG, "--- onStopLineInfo --- info: $info")
if (!hasCrossRoad) {
hasCrossRoad = true
}
@@ -146,13 +132,12 @@ object AiRoadMarker {
return
}
try {
val loc = CoordinateUtils.transformGcj02toWgs84(location.latitude, location.longitude)
Logger.d(TAG, "x:${loc[0]}, y:${loc[1]}, angle: ${location.bearing}")
val loc = arrayOf(CallerAutoPilotStatusListenerManager.getCurWgs84Lon(), CallerAutoPilotStatusListenerManager.getCurWgs84Lat())
carLocation.set(Triple(loc[0], loc[1], location.bearing.toDouble()))
val marker = marker.get() ?: return
val isOutOfRange = isOutOfRange(marker.poi_lon, marker.poi_lat, carLocation.get().first, carLocation.get().second, carLocation.get().third)
Logger.d(TAG, "--- onLocationChanged: isOutOfRange: $isOutOfRange")
if (isOutOfRange) {
Log.d(TAG, "--- onLocationChanged: isOutOfRange --- ")
unMarker(marker)
}
} catch (t: Throwable) {
@@ -160,20 +145,6 @@ object AiRoadMarker {
}
}
}
//
// private fun drawBlueLine(points: List<Pair<Double, Double>>, location: MogoLocation) {
// val line = blueLine.get()
// blueLineOptions.points(points.filter {
// DrivingDirectionUtils.getDegreeOfCar2Poi2(location.longitude, location.latitude, it.first, it.second, location.bearing.toDouble()) < 90L
// }.map {
// MogoLatLng(it.second, it.first)
// })
// if (line == null || line.isDestroyed) {
// overlayManager.addPolyline(blueLineOptions)
// } else {
// line.setOption(blueLineOptions)
// }
// }
private fun onCreate() {
CallerMapRoadListenerManager.registerRoadListener(TAG, onRoadListener)
@@ -189,39 +160,39 @@ object AiRoadMarker {
fun enqueue(marker: Marker) {
Logger.d(TAG, "--- enqueue --- 1 ---")
Log.d(TAG, "--- enqueue --- 1 ---")
if (markerCache.contains(marker)) {
Logger.d(TAG, "--- enqueue --- cache hit ---")
Log.d(TAG, "--- enqueue --- cache hit ---")
return
}
Logger.d(TAG, "--- enqueue --- 2 ---")
Log.d(TAG, "--- enqueue --- 2 ---")
markerCache += marker
pool.execute {
Logger.d(TAG, "--- enqueue --- 3 ---${Thread.currentThread().name}::${Thread.currentThread().id}")
Log.d(TAG, "--- enqueue --- 3 ---${Thread.currentThread().name}::${Thread.currentThread().id}")
queue.put(marker)
}
}
private fun marker(marker: Marker) {
Logger.d(TAG, "--- marker --- 1 ---")
Log.d(TAG, "--- marker --- 1 ---")
val location = carLocation.get() ?: return
Logger.d(TAG, "--- marker --- 2 ---")
Log.d(TAG, "--- marker --- 2 ---")
removeRedLine()
//施工中心点前方的自车行驶方向上300米距离
val l1 = MogoMap.getInstance().mogoMap.getCenterLineRangeInfo(marker.poi_lon, marker.poi_lat, location.third.toFloat(), 300f)
//施工中心点后方的自车行驶方向上300米距离
Logger.d(TAG, "--- marker --- 3 --- l1: $l1")
Log.d(TAG, "--- marker --- 3 --- l1: $l1")
val l2 = MogoMap.getInstance().mogoMap.getCenterLineRangeInfo(marker.poi_lon, marker.poi_lat, location.third.toFloat(), -300f)
Logger.d(TAG, "--- marker --- 4 --- l2: $l2")
Log.d(TAG, "--- marker --- 4 --- l2: $l2")
val points = LinkedList<MogoLatLng>()
if (l2 != null && l2.points.isNotEmpty()) {
points.addAll(l2.points.map {
points.addAll(l2.points.reversed().map {
MogoLatLng(it.second, it.first)
})
}
val centerX= marker.poi_lon
val centerY = marker.poi_lat
Logger.d(TAG, "--- marker --- 5 --- marker: $marker")
Log.d(TAG, "--- marker --- 5 --- marker: $marker")
val farthestPoint = marker.polygon?.let {
var find: Pair<Double, Double> = Pair(centerX, centerY)
var min = Long.MAX_VALUE
@@ -235,7 +206,7 @@ object AiRoadMarker {
MogoLatLng(find.second, find.second)
} ?: MogoLatLng(centerY, centerX)
marker.farthestPoint = Pair(farthestPoint.lon, farthestPoint.lat)
Logger.d(TAG, "--- marker --- 6 --- marker: $marker")
Log.d(TAG, "--- marker --- 6 --- marker: $marker")
if (l1 != null && l1.points.isNotEmpty()) {
for (l in l1.points) {
if (DrivingDirectionUtils.getDegreeOfCar2Poi2(farthestPoint.lon, farthestPoint.lat, l.first, l.second, location.third) < 90L) {
@@ -245,7 +216,7 @@ object AiRoadMarker {
}
var line = redLine.get()
redLineOptions.points(points)
Logger.d(TAG, "--- marker --- 7 --- points: ${points.size}")
Log.d(TAG, "--- marker --- 7 --- points: ${points.size}")
if (line == null || line.isDestroyed) {
val l = overlayManager.addPolyline(redLineOptions)
this.redLine.set(l)
@@ -260,26 +231,15 @@ object AiRoadMarker {
private fun removeRedLine() {
val old = redLine.get()
Logger.d(TAG, "--- removeRedLine --- 1 ---")
Log.d(TAG, "--- removeRedLine --- 1 ---")
if (old != null) {
Logger.d(TAG, "--- removeRedLine --- 2 ---")
Log.d(TAG, "--- removeRedLine --- 2 ---")
redLine.set(null)
old.isVisible = false
old.remove()
}
}
// private fun removeBlueLine() {
// val old = blueLine.get()
// Logger.d(TAG, "--- removeBlueLine --- 1 ---")
// if (old != null) {
// Logger.d(TAG, "--- removeBlueLine --- 2 ---")
// blueLine.set(null)
// old.isVisible = false
// old.remove()
// }
// }
private fun unMarker(marker: Marker) {
markerCache -= marker
removeRedLine()