cicv_data_closed_loop/trigger/pjisuv/cicv_ticker/CutinWithSightBblock/main/CutinWithSightBblock.go

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 "CutinWithSightBblock"
}

func Rule(shareVars *sync.Map) {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	// 1 使用goroutine
	go func(shareVars *sync.Map) {
		// 2 定义触发器的间隔时间
		ticker := time.NewTicker(time.Duration(2) * time.Second)
		defer ticker.Stop()
		// 3 运行一个无限循环
		for {
			select {
			// 定时器触发时执行的代码
			case <-ticker.C:
				FinalCallback(shareVars)

			}
		}
	}(shareVars)
}
func isCuttingIn(ObjectList [][]float32, AngularVelocityZ float64) (bool, float32, float32, float64) {

	for i, objY := range ObjectList[1] {

		if math.Abs(float64(objY)) >= 1.8 && math.Abs(AngularVelocityZ) <= 0.6 && ObjectList[0][i] >= 2 {
			//fmt.Println(objY)
			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])) <= 0.7 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 1 {
					//fmt.Println(objX)
					return true, ObjectList[5][1+i+j], ObjectList[0][1+i+j], math.Abs(float64(ObjectList[1][1+i+j]))
				}
			}
		}
	}
	return false, 0.0, 0.0, 0.0
}
func SightBlock(cutobjx float32, cutobjy float64, objId uint32, cutinframe float32, ObjectSlice map[uint32][][]float32) bool {

	for Id, objValue := range ObjectSlice {
		if Id != objId {
			for i := 0; i < len(objValue[1]); i++ {
				if objValue[2][i] == cutinframe {
					//fmt.Println("yes")
					objx := objValue[0][i]
					objy := math.Abs(float64(objValue[1][i]))
					//fmt.Println(objx)
					//fmt.Println(objy)
					diffx := cutobjx - objx
					diffy := math.Abs(cutobjy - objy)
					if diffx >= 0 && diffx <= 6 && diffy <= 4.0 {
						return true
					}
				}

			}

		}
	}
	return false
}

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 {
		for objId, objValue := range ObjDic {
			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
			cutinflag, cutinframe, cutobjx, cutobjy := isCuttingIn(objValue, AngularVelocityZ)
			if cutinflag {
				if SightBlock(cutobjx, cutobjy, objId, cutinframe, ObjDic) {
					event_lable := "CutinWithSightBblock"
					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
			}
		}

	}
}