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

package main

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

type Point struct {
	Latitude  float64
	Longitude float64
}

var (
	StartTime      int64
	IsStopped      bool
	IsEndPoint     bool
	IsTrafficLight bool
	count1         int64
	//定义园区４个信号灯的坐标
	point2     = Point{39.72975930689718, 116.48861102824081}
	point3     = Point{39.7288805296616, 116.48812315228867}
	point4     = Point{39.73061430369551, 116.49225103553502}
	point5     = Point{39.73077491578002, 116.49060085035634}
	EndPoint   = Point{0, 0}
	pointlist1 = []Point{point2, point3, point4, point5}
)

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

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

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
}
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] <= 13 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 6 {
				return true

			}
		}
	}
	return false
}
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%10 == 0 {
		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
		Longitude, _ := shareVars.Load("EndPointX")
		Latitude, _ := shareVars.Load("EndPointY")

		Automode = Automode.(int16)
		EndPoint.Longitude = Longitude.(float64)
		EndPoint.Latitude = Latitude.(float64)
		pointlist2 := []Point{EndPoint}
		IsTrafficLight = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
		if Automode == 1 && !IsTrafficLight && !IsEndPoint && OutsideWorkshopFlag == true {
			AbsSpeed, _ := shareVars.Load("AbsSpeed")
			flag := IfObstaclesNearby(shareVars)
			if AbsSpeed.(float64) < 0.5 && flag {
				// 如果之前没有记录开始时间，记录当前时间
				if StartTime == 0 {
					StartTime = time.Now().Unix()
				}
				// 判断是否持续超过 50s
				if time.Now().Unix()-StartTime > 5 {
					if !IsStopped {
						IsStopped = true
						return Label()
					}
				}
			} else {
				// 如果速度大于 0.1，重置开始时间和停止标志
				StartTime = 0
				IsStopped = false
			}
		} else {
			StartTime = 0
			IsStopped = false
		}
	}
	count1++
	return ""
}