cicv_data_closed_loop/trigger/kinglong/tpperception/TTC/main/TTC.go

package main

import (
	"cicv-data-closedloop/kinglong_msgs"
	"math"
)

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

func Label() string {
	return "TTC"
}

func Rule(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string {
	for _, obj := range data.Objs {
		if math.Abs(float64(obj.Y)) <= 2 && obj.X >= 8 && math.Abs(velocityX) > 0.5 {
			theta := DegreesToRadians(yaw)
			vxrel := (float64(obj.Vxabs)-velocityX)*math.Cos(theta) + (float64(obj.Vyabs)-velocityY)*math.Sin(theta)
			ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
			if ttc >= 0 && ttc <= 5 {
				return "TTC"
			}
		}
	}
	return ""
}

// DegreesToRadians 计算角度转弧度
func DegreesToRadians(degrees float64) float64 {
	return degrees * math.Pi / 180.0
}