cicv_data_closed_loop/trigger/pjisuv/cicv_location/TargetCarBehindWhenReversing/main/TargetCarBehindWhenReversing.go

package main

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

type Point struct {
	Latitude  float64
	Longitude float64
}

var (
	count1 int64
)

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

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

func IfObstaclesNearby(shareVars *sync.Map) bool {
	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
	if ok1 {
		for _, obj := range ObjDic {
			if obj[0][len(obj[0])-1] >= -15 && obj[0][len(obj[0])-1] <= -1 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 6 {
				return true

			}
		}

	}
	return false
}
func calculateDirectionAngle(speedX, speedY float64) float64 {
	// 使用反正切函数计算方向角
	angle := math.Atan2(speedY, speedX)

	// 将角度转换为度数
	angleDegree := angle * (180 / math.Pi)

	// 将角度限定在 0~360 范围内
	if angleDegree < 0 {
		angleDegree += 360
	} else if angleDegree > 360 {
		angleDegree -= 360
	}

	return angleDegree
}
func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	if count1%100 == 0 {
		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
		directionAngle := calculateDirectionAngle(data.VelocityX, data.VelocityY)
		diffAngle := math.Abs(float64(directionAngle - data.Yaw))
		if OutsideWorkshopFlag == true {
			AbsSpeed, _ := shareVars.Load("AbsSpeed")
			flag := IfObstaclesNearby(shareVars)
			if flag && AbsSpeed.(float64) >= 1 && diffAngle >= 160 && diffAngle <= 200 {
				eventLabel := "TargetCarBehindWhenReversing"
				fmt.Println(eventLabel)
				count1 = 1
				return Label()
			}

		}
	}
	count1++
	return ""
}