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

package main

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

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

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

type Point struct {
	Latitude  float64
	Longitude float64
}

var (
	count2 int = 0

	//定义园区部门Ｔ字路口的经纬度坐标值
	point3 = Point{39.73040966605621, 116.48995329696209}
	point4 = Point{39.73083727413453, 116.49079780188244}
	point5 = Point{39.72976753711939, 116.49043130389033}
	point6 = Point{39.73012466515933, 116.49128381717591}
	point7 = Point{39.729251498328246, 116.49077484625299}
	point8 = Point{39.72964529630643, 116.49164592200161}

	pointlist = []Point{point3, point4, point5, point6, point7, point8}
)

func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	if count2%10 == 0 {
		enterflag := IfEnter(pointlist, 12.0, data.Latitude, data.Longitude)
		velocityXOfCicvLocation, ok := shareVars.Load("VelocityXOfCicvLocation")
		if ok {
			if enterflag && velocityXOfCicvLocation.(float64) >= 1 {
				return Label()
			}
		}
	}
	count2++
	return ""
}

func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
	// 判断是否进入点列表中的区域
	point1 := Point{Latitude: lat, Longitude: lon}
	for _, point := range pointlist {
		d := distance(point1, point)
		if d <= radius {
			return true
		}
	}
	return false
}

// 计算两点之间的距离（米）
func distance(point1, point2 Point) float64 {
	// 经纬度转弧度
	lat1 := point1.Latitude * math.Pi / 180
	lon1 := point1.Longitude * math.Pi / 180
	lat2 := point2.Latitude * math.Pi / 180
	lon2 := point2.Longitude * math.Pi / 180

	// 计算距离
	dlon := lon2 - lon1
	dlat := lat2 - lat1
	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
	d := 6371000 * c
	return d
}