diff --git a/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/events/scenario/scene/airoad/AiRoadMarker.kt b/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/events/scenario/scene/airoad/AiRoadMarker.kt index fbd569d4c4..257b3103a7 100644 --- a/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/events/scenario/scene/airoad/AiRoadMarker.kt +++ b/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/events/scenario/scene/airoad/AiRoadMarker.kt @@ -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() } -// -// private val blueLineColors = arrayListOf(Color.parseColor("#002ABAD9"), Color.parseColor("#30A3FF")) - private val markerCache = ConcurrentSet() 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>, 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() 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 = 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()