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

package main

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

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

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

// 主进程的逻辑是先判断触发再缓存全局变量
func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()

	positionXOfCicvLocation, ok1 := shareVars.Load("PositionXOfCicvLocation")
	positionYOfCicvLocation, ok2 := shareVars.Load("PositionYOfCicvLocation")
	if ok1 && ok2 {
		v1 := positionXOfCicvLocation.(float64)
		v2 := positionYOfCicvLocation.(float64)
		if v1 != 0 && v2 != 0 {
			d := math.Sqrt((v1-data.PositionX)*(v1-data.PositionX) + (v2-data.PositionY)*(v2-data.PositionY))
			if d >= 2 {
				return Label()
			}
		}
	}
	return ""
}