红绿灯—感知融合

增加工控机感知红绿灯
感知红绿灯和AI云红绿灯融合

core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt

@@ -25,6 +25,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListener
 import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager.invokeAutopilotSNRequest
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotCarConfigListenerManager.invokeAutopilotCarConfigData
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotCarStatusListenerManager.invokeAutopilotCarStateData
+import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager.invokeAutopilotIdentifyDataUpdate
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotRecordListenerManager.invokeAutopilotRecordResult
 import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager.invokeAutopilotWarnMessage
@@ -224,7 +225,11 @@ class MoGoAdasListenerImpl : OnAdasListener {
         header: MessagePad.Header?,
         trafficLights: TrafficLightOuterClass.TrafficLights?
     ) {
-
+        if(trafficLights != null){
+            CallerAutopilotIdentifyListenerManager.invokeAutopilotPerceptionTrafficLight(
+                trafficLights
+            )
+        }
     }
 
     override fun onPredictionObstacleTrajectory(

core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/V2XProvider.kt

@@ -9,6 +9,7 @@ import com.mogo.eagle.core.function.call.trafficlight.CallTrafficLightManager
 import com.mogo.eagle.core.function.v2x.events.V2XEventManager
 import com.mogo.eagle.core.function.v2x.redlightwarning.RedLightWarningManager
 import com.mogo.eagle.core.function.v2x.speedlimit.SpeedLimitDataManager
+import com.mogo.eagle.core.function.v2x.trafficlight.core.TrafficLightDispatcher
 import com.mogo.eagle.core.function.v2x.vip.VipCarManager
 import com.mogo.eagle.core.utilcode.mogo.AppIdentityModeUtils
 
@@ -22,6 +23,7 @@ class V2XProvider : IMoGoFunctionServerProvider {
         CallTrafficLightManager.getTrafficLightProvider().initTrafficLightServer(context)
         VipCarManager.INSTANCE.initServer(context)
         SpeedLimitDataManager.getInstance().start()
+        TrafficLightDispatcher.INSTANCE.initServer(context)
 
         if (AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode) && AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)) {
             //不处理
@@ -34,7 +36,7 @@ class V2XProvider : IMoGoFunctionServerProvider {
 
     override fun onDestroy() {
         VipCarManager.INSTANCE.destroy()
-
+        TrafficLightDispatcher.INSTANCE.destroy()
         if (AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode) && AppIdentityModeUtils.isPassenger(FunctionBuildConfig.appIdentityMode)) {
             //不处理
         } else {

core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/trafficlight/core/TrafficLightDispatcher.kt

@@ -0,0 +1,112 @@
+package com.mogo.eagle.core.function.v2x.trafficlight.core
+
+import android.content.Context
+import com.mogo.eagle.core.data.trafficlight.TrafficLightResult
+import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotIdentifyListener
+import com.mogo.eagle.core.function.api.trafficlight.IMoGoTrafficLightListener
+import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotIdentifyListenerManager
+import com.mogo.eagle.core.function.call.hmi.CallerHmiManager
+import com.mogo.eagle.core.function.call.trafficlight.CallTrafficLightListenerManager
+import com.mogo.eagle.core.function.v2x.trafficlight.TrafficLightHMIManager
+import perception.TrafficLightOuterClass
+import perception.TrafficLightOuterClass.TrafficLight
+
+/**
+ * @author XuXinChao
+ * @description 对多个红绿灯信号来源进行统一调度（AI云、工控机）
+ * @since: 2022/4/28
+ */
+class TrafficLightDispatcher : IMoGoAutopilotIdentifyListener , IMoGoTrafficLightListener {
+
+    companion object{
+        const val TAG = "TrafficLightDispatcher"
+        val INSTANCE: TrafficLightDispatcher by lazy(mode = LazyThreadSafetyMode.SYNCHRONIZED){
+            TrafficLightDispatcher()
+        }
+
+    }
+
+    private var mContext: Context? = null
+    //是否有AI获取红绿灯灯态
+    private var hasAiLightStatus: Boolean = false
+
+    fun initServer(context: Context){
+        mContext = context
+        //注册监听AI云获取红绿灯状态
+        CallTrafficLightListenerManager.registerTrafficLightListener(TAG, this)
+        //注册监听AI云进入路口
+        CallTrafficLightListenerManager.registerEnterCrossRoadListener(TAG, this)
+        //注册监听工控机感知红绿灯
+        CallerAutopilotIdentifyListenerManager.addListener(TAG, this)
+    }
+
+    /**
+     * 工控机感知红绿灯
+     * @param trafficLights 感知红绿灯
+     */
+    override fun onAutopilotPerceptionTrafficLight(trafficLights: TrafficLightOuterClass.TrafficLights?) {
+        if(!hasAiLightStatus){
+            trafficLights?.let { it ->
+                var light :TrafficLight?= null
+                if(it.hasStraight()){
+                    light = it.straight
+                }else if(it.hasLeft()){
+                    light = it.left
+                }else if(it.hasRight()){
+                    light = it.right
+                }else if(it.hasUTurn()){
+                    light = it.uTurn
+                }
+                if(light == null){
+                    //隐藏红绿灯显示
+                    CallerHmiManager.showWarningTrafficLight(0)
+                }else{
+                    when (light.state) {
+                        TrafficLightOuterClass.LightState.STATE_RED -> {
+                            //红灯
+                            CallerHmiManager.showWarningTrafficLight(1)
+                            CallerHmiManager.changeCountdownGreen(0)
+                        }
+                        TrafficLightOuterClass.LightState.STATE_YELLOW -> {
+                            //黄灯
+                            CallerHmiManager.showWarningTrafficLight(2)
+                            CallerHmiManager.changeCountdownGreen(0)
+                        }
+                        TrafficLightOuterClass.LightState.STATE_GREEN -> {
+                            //绿灯
+                            CallerHmiManager.showWarningTrafficLight(3)
+                            CallerHmiManager.changeCountdownGreen(0)
+                        }
+                        else -> {}
+                    }
+                }
+
+            }
+        }
+    }
+
+    /**
+     * AI云获取红绿灯状态
+     */
+    override fun onTrafficLightStatus(trafficLightResult: TrafficLightResult) {
+        TrafficLightHMIManager.INSTANCE.updateTrafficLight(trafficLightResult)
+        hasAiLightStatus = true
+    }
+
+    /**
+     * 车辆进入路口
+     */
+    override fun onEnterCrossRoad(enter: Boolean) {
+        hasAiLightStatus = enter
+    }
+
+    fun destroy(){
+        //取消注册监听AI云获取红绿灯状态
+        CallTrafficLightListenerManager.unRegisterTrafficLightListener(TAG)
+        //取消注册监听工控机感知红绿灯
+        CallerAutopilotIdentifyListenerManager.removeListener(TAG)
+        //取消注册监听AI云进入路口
+        CallTrafficLightListenerManager.unRegisterEnterCrossRoadListener(TAG)
+    }
+
+}

core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotIdentifyListener.kt

@@ -1,6 +1,7 @@
 package com.mogo.eagle.core.function.api.autopilot
 
 import mogo.telematics.pad.MessagePad
+import perception.TrafficLightOuterClass
 
 /**
  * @author xiaoyuzhou
@@ -23,4 +24,10 @@ interface IMoGoAutopilotIdentifyListener {
      */
     fun onAutopilotWarnMessage(warn: MessagePad.Warn?) {}
 
+    /**
+     * 感知红绿灯
+     * @param trafficLights 感知红绿灯
+     */
+    fun onAutopilotPerceptionTrafficLight(trafficLights: TrafficLightOuterClass.TrafficLights?){}
+
 }

core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotIdentifyListenerManager.kt

@@ -4,6 +4,7 @@ import androidx.annotation.Nullable
 import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotIdentifyListener
 import com.mogo.eagle.core.function.call.base.CallerBase
 import mogo.telematics.pad.MessagePad
+import perception.TrafficLightOuterClass
 import java.util.concurrent.ConcurrentHashMap
 
 /**
@@ -79,5 +80,15 @@ object CallerAutopilotIdentifyListenerManager : CallerBase() {
         }
     }
 
+    /**
+     * 感知红绿灯
+     */
+    fun invokeAutopilotPerceptionTrafficLight(trafficLights: TrafficLightOuterClass.TrafficLights?){
+        M_AUTOPILOT_IDENTIFY_LISTENERS.forEach{
+            val tag = it.key
+            val listener = it.value
+            listener.onAutopilotPerceptionTrafficLight(trafficLights)
+        }
+    }
 
 }