trigger/main/pji_DGNC/CCCscp_f/main/CCCscp_f.go

package main

import (
	"awesomeProject/entity"
	"awesomeProject/pjisuv_msgs"
	"fmt"
	"github.com/bluenviron/goroslib/v2"
	"math"
	"os"
	"os/signal"
	"time"
)

var (
	Framenum    int64 = 0
	param       entity.PjisuvParam
	ObjectSlice = make(map[uint32][][]float32)
)

func main() {
	ticker := time.NewTicker(5 * time.Second)
	defer ticker.Stop()
	go listener()
	for {
		select {
		case <-ticker.C:
			FinalCallback()
		}
	}
}

func isCross(ObjectList [][]float32) bool {
	for i := 0; i < len(ObjectList[0]); i++ {
		Type := ObjectList[4][0]
		xi := ObjectList[0][i]
		yi := ObjectList[1][i]
		diff_hi := ObjectList[2][i]
		if xi >= 0 && yi >= 2 && diff_hi > 0 && Type != 100.0 { //简化条件
			//if xi >= 0 && yi >= 2 && diff_hi <= 120 && diff_hi >= 60 && Type != 1.0 {//理论条件
			for j := 0; j < len(ObjectList[0])-i-1; j++ {
				xj := ObjectList[0][j]
				yj := ObjectList[1][j]
				diff_hj := ObjectList[2][j]
				if xj >= 0 && yj <= -2 && diff_hj > 0 { //简化条件
					//if xj >= 0 && yj <= -2 && diff_hj <= 120 && diff_hj >= 60 {//理论条件
					return true
				}
			}
		}
	}
	return false
}

func FinalCallback() {
	for _, objValue := range ObjectSlice {
		//fmt.Println(objValue)
		//fmt.Println("------------------------------------------------")

		if len(objValue[0]) <= 10 || !isCross(objValue) {
			continue
		}
		event_lable := "CCCscp_f"
		fmt.Println(event_lable)
		ObjectSlice = make(map[uint32][][]float32)
	}
}

func CallbackCicvLocation(data *pjisuv_msgs.PerceptionLocalization) {
	param.YawOfCicvLocation = data.Yaw
}

func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
	Framenum += 1
	fmt.Println("obj length", len(data.Objs))
	fmt.Println("Framenum", Framenum)
	for _, obj := range data.Objs {
		fmt.Println("ID", obj.Id, "Type", obj.Type)

		if math.Abs(float64(obj.X)) >= 80 || math.Abs(float64(obj.Y)) >= 30 {
			continue
		}
		if _, ok := ObjectSlice[obj.Id]; !ok {
			ObjectSlice[obj.Id] = [][]float32{{}, {}, {}, {}, {}}
		}
		//absspeed := math.Sqrt(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2)) * 3.6
		ObjectSlice[obj.Id][0] = append(ObjectSlice[obj.Id][0], obj.X)
		ObjectSlice[obj.Id][1] = append(ObjectSlice[obj.Id][1], obj.Y)
		// 计算自车与目标的最小角度差（范围归到0到180度）
		diffh := (float64(obj.Heading - float32(param.YawOfCicvLocation)))
		if diffh < -180.0 {
			diffh = 360.0 + diffh
		} else if diffh > 180.0 {
			diffh = 360.0 - diffh
		} else {
			diffh = math.Abs(diffh)
		}
		//fmt.Println(diffh)
		ObjectSlice[obj.Id][2] = append(ObjectSlice[obj.Id][2], float32(diffh))
		ObjectSlice[obj.Id][3] = append(ObjectSlice[obj.Id][3], float32(Framenum))
		ObjectSlice[obj.Id][4] = append(ObjectSlice[obj.Id][4], float32(obj.Type))

		//ObjectSlice[obj.Id][3] = append(ObjectSlice[obj.Id][3], float32(absspeed))
		// 滑动窗口（保持切片大小为200，大于等于200则持续丢弃第一个，整体向前移动一位）
		if len(ObjectSlice[obj.Id][0]) >= 200 {
			ObjectSlice[obj.Id][0] = ObjectSlice[obj.Id][0][1:]
			ObjectSlice[obj.Id][1] = ObjectSlice[obj.Id][1][1:]
			ObjectSlice[obj.Id][2] = ObjectSlice[obj.Id][2][1:]
			ObjectSlice[obj.Id][3] = ObjectSlice[obj.Id][3][1:]
			ObjectSlice[obj.Id][4] = ObjectSlice[obj.Id][4][1:]
		}
	}
}

func listener() {
	// create a node and connect to the master
	n, err := goroslib.NewNode(goroslib.NodeConf{
		Name:          "goroslib_sub",
		MasterAddress: "127.0.0.1:11311",
	})
	if err != nil {
		panic(err)
	}
	defer n.Close()

	// create a subscriber
	subCicvLocation, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:     n,
		Topic:    "/cicv_location",
		Callback: CallbackCicvLocation,
	})
	if err != nil {
		panic(err)
	}
	defer subCicvLocation.Close()

	// create a subscriber
	subTpperception, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:     n,
		Topic:    "tpperception",
		Callback: CallbackTpperception,
	})
	if err != nil {
		panic(err)
	}
	defer subTpperception.Close()

	// wait for CTRL-C
	c := make(chan os.Signal, 1)
	signal.Notify(c, os.Interrupt)
	<-c
}