|
@@ -0,0 +1,92 @@
|
|
|
+package main
|
|
|
+
|
|
|
+import (
|
|
|
+ "cicv-data-closedloop/pjisuv_ticker"
|
|
|
+ "fmt"
|
|
|
+ "math"
|
|
|
+ "sync"
|
|
|
+ "time"
|
|
|
+)
|
|
|
+
|
|
|
+var (
|
|
|
+ Maxlenobj int32 = 0
|
|
|
+)
|
|
|
+
|
|
|
+
|
|
|
+func Topic() string {
|
|
|
+ return pjisuv_ticker.TickerTopic
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+func Label() string {
|
|
|
+ return "VehiclesCutOutTogether"
|
|
|
+}
|
|
|
+
|
|
|
+func Rule(shareVars *sync.Map) {
|
|
|
+ defer func() {
|
|
|
+ if r := recover(); r != nil {
|
|
|
+ fmt.Println("Recovered from panic:", r)
|
|
|
+ }
|
|
|
+ }()
|
|
|
+
|
|
|
+ go func(shareVars *sync.Map) {
|
|
|
+
|
|
|
+ ticker := time.NewTicker(time.Duration(3) * time.Second)
|
|
|
+ defer ticker.Stop()
|
|
|
+
|
|
|
+ for {
|
|
|
+ select {
|
|
|
+
|
|
|
+ case <-ticker.C:
|
|
|
+ FinalCallback(shareVars)
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }(shareVars)
|
|
|
+}
|
|
|
+func isCuttingIn(ObjectList [][]float32, AngularVelocityZ float64) int {
|
|
|
+
|
|
|
+ for i, objY := range ObjectList[1] {
|
|
|
+
|
|
|
+ if math.Abs(float64(objY)) <= 0.9 && math.Abs(AngularVelocityZ) <= 0.6 && ObjectList[0][i] >= 2 {
|
|
|
+
|
|
|
+ for j := 0; j < len(ObjectList[1])-i-1; j++ {
|
|
|
+ objX := ObjectList[0][1+i+j]
|
|
|
+ if math.Abs(float64(ObjectList[1][1+i+j])) >= 1.7 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 2 {
|
|
|
+
|
|
|
+ return 1
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return 0
|
|
|
+}
|
|
|
+func FinalCallback(shareVars *sync.Map) {
|
|
|
+ OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
|
|
|
+ ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
|
|
|
+ ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
|
|
|
+ AngularVelocityZOfCicvLocation, _ := shareVars.Load("AngularVelocityZOfCicvLocation")
|
|
|
+ AngularVelocityZ := AngularVelocityZOfCicvLocation.(float64)
|
|
|
+
|
|
|
+ if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
|
|
|
+ numobjcutin := 0
|
|
|
+ for _, objValue := range ObjDic {
|
|
|
+ Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
|
|
|
+ numcoutin := isCuttingIn(objValue, AngularVelocityZ)
|
|
|
+ numobjcutin = numobjcutin + numcoutin
|
|
|
+ }
|
|
|
+ if numobjcutin >= 2 {
|
|
|
+ event_lable := "VehiclesCutOutTogether"
|
|
|
+ fmt.Println(event_lable)
|
|
|
+ pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
|
|
|
+ }
|
|
|
+
|
|
|
+ if Maxlenobj >= 100 {
|
|
|
+ ObjDicOfTpperception = make(map[uint32][][]float32)
|
|
|
+ shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
|
|
|
+ Maxlenobj = 0
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+}
|