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

package main

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

var (
	Framenum             int64 = 0
	PosxofCicvLocation   any
	PosyofCicvLocation   any
	posx                 float64
	posy                 float64
	ObjDicOfTpperception any
	objDic               = make(map[uint32][][]float32)
	yaw                  float64
	Trajcetorypoint      any
	trajcetory           []pjisuv_msgs.TrajectoryPoint
	objId                uint32
	ok1                  bool
	ok2                  bool
	ok3                  bool
	ok4                  bool
	LeftCurveFlag        = false
	RightCurveFlag       = false
)

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

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

func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	PosxofCicvLocation, ok1 = shareVars.Load("PositionXOfCicvLocation")
	PosyofCicvLocation, ok2 = shareVars.Load("PositionYOfCicvLocation")
	if !ok1 && !ok2 {
		return ""
	}
	posx = PosxofCicvLocation.(float64)
	posy = PosyofCicvLocation.(float64)
	Framenum += 1
	ObjDicOfTpperception, ok3 = shareVars.Load("ObjDicOfTpperception")
	if !ok3 {
		return ""
	}
	objDic = ObjDicOfTpperception.(map[uint32][][]float32)
	// Trajcetorypoint的类型？
	Trajcetorypoint, ok4 = shareVars.Load("Trajcetorypoint")
	if !ok4 {
		return ""
	}
	trajcetory = Trajcetorypoint.([]pjisuv_msgs.TrajectoryPoint)
	if len(trajcetory) > 2 {
		StartHeading := trajcetory[0].Heading
		EndHeading := trajcetory[len(trajcetory)-1].Heading
		diffHeading := StartHeading - EndHeading

		if diffHeading < -1.0 && diffHeading > -3.0 {
			LeftCurveFlag = true
		} else {
			LeftCurveFlag = false
		}
		if diffHeading > 1.0 && diffHeading < 3.0 {
			RightCurveFlag = true
			//fmt.Println(diffHeading)
		} else {
			RightCurveFlag = false
		}

	} else {
		LeftCurveFlag = false
		RightCurveFlag = false
	}
	if LeftCurveFlag == true {
		for objid, objVal := range objDic {
			if objVal[objid][6] != 100 { //简化条件
				//if obj.Type == 2 || obj.Type == 3 {//理论条件
				absspeed := math.Sqrt(math.Pow(float64(objVal[objid][2]), 2) + math.Pow(float64(objVal[objid][3]), 2))
				Distance := math.Sqrt(math.Pow(posy-float64(objVal[objid][0]), 2) + math.Pow(posy-float64(objVal[objid][1]), 2))
				//fmt.Println("Distance:", Distance)
				if objVal[objid][0] >= 2.0 && objVal[objid][0] <= 15.0 && (math.Abs(float64(objVal[objid][1])) <= 4.0 || Distance <= 15.0) && absspeed > 1.5 {
					return Label()
					break
				}
			}
		}
	}
	return ""
}