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

package main

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

var (
	threshold float64 = 15 / 3.6
	count1    int     = 0
)

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

// 禁止存在下划线_
func Label() string {
	return "ReverseAndOverspeed"
}
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%200 == 0 {
		OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
		if ok3 {
			directionAngle := calculateDirectionAngle(data.VelocityX, data.VelocityY)
			AbsSpeed, _ := shareVars.Load("AbsSpeed")
			diffAngle := math.Abs(float64(directionAngle - data.Yaw))
			//fmt.Println(diffAngle)
			if AbsSpeed.(float64) >= threshold && diffAngle >= 160 && diffAngle <= 200 && OutsideWorkshopFlag.(bool) {

				eventLabel := "ReverseAndOverspeed"
				fmt.Println(eventLabel)
				count1 = 1
				return Label()
			}
		}
	}
	count1++
	return ""
}