cicv_data_closed_loop/trigger/jili/tpperception/CBFA/main/CBFA.go

package main

import (
	"cicv-data-closedloop/pjisuv_msgs"
	"fmt"
	"sync"
)

var (
	Framenum             int64 = 0
	YawofCicvLocation    any
	ObjDicOfTpperception any
	objDic               = make(map[uint32][][]float32)
	yaw                  float64
	objId                uint32
	obj                  [][]float32
	ok1                  bool
	ok2                  bool
	ObjectSlice          = make(map[uint32][][]float32)
)

func findIndex(lst []float32, target float32) int {
	for i, v := range lst {
		if v == target {
			return i
		}
	}
	return -1
}

func isCrossAndOcclusion(id uint32, ObjectList [][]float32, ObjectSlice map[uint32][][]float32) bool {
	for i := 0; i < len(ObjectList[0]); i++ {
		xi := ObjectList[0][i]
		yi := ObjectList[1][i]
		diff_hi := ObjectList[7][i]
		Type := ObjectList[6][0]

		if xi >= 0 && yi >= 3 && diff_hi >= 0 && Type != 100.0 {
			startFrame1 := ObjectList[5][i]

			for j := 0; j < len(ObjectList[0])-i-1; j++ {
				xj := ObjectList[0][j]
				yj := ObjectList[1][j]
				diff_hj := ObjectList[7][j]
				if xj >= 0 && xj >= 20 && xj <= 50 && yj <= -1 && diff_hj >= 0 {
					startFrame2 := ObjectList[5][j]
					for this_id, objValue := range ObjectSlice {
						if this_id != id {
							this_startFrame_index1 := findIndex(objValue[5], startFrame1)
							this_startFrame_index2 := findIndex(objValue[5], startFrame2)
							this_type := objValue[6][0]
							//fmt.Println(objValue[0][this_startFrame_index2], xj)
							if this_startFrame_index1 != -1 && this_startFrame_index2 != -1 {
								if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] > 0 && objValue[1][this_startFrame_index1] < yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] > 0 && (this_type != 100.0) { //简化条件
									return true
								}
							}
						} else {
							continue
						}
					}
				}
			}
		}
	}
	return false
}

func Topic() string {
	return "/tpperception"
}

// 禁止存在下划线_
func Label() string {
	return "CBFA"
}

func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	YawofCicvLocation, ok1 = shareVars.Load("YawOfCicvLocation")
	if !ok1 {
		return ""
	}
	yaw = YawofCicvLocation.(float64)
	Framenum += 1
	ObjDicOfTpperception, ok2 = shareVars.Load("ObjDicOfTpperception")
	if !ok2 {
		return ""
	}
	objDic = ObjDicOfTpperception.(map[uint32][][]float32)
	for objId, objValue := range objDic {
		if len(objValue[0]) <= 10 || !isCrossAndOcclusion(objId, objValue, ObjectSlice) {
			continue
		}
		return Label()
	}
	return ""
}