add CurveOverspeed & AbnormalStopOnCurve trigger

trigger/pjisuv/cicv_location/AbnormalStopOnCurve/main/AbnormalStopOnCurve.go

@@ -0,0 +1,108 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	StartTime  int64
+	threshold  int64 = 3
+	IsStopped  bool
+	IsEndPoint bool
+	IsCurve    bool
+	count1     int64
+	pointcurve = Point{39.73004426154644, 116.49248639463602}
+	EndPoint   = Point{0, 0}
+	pointlist1 = []Point{pointcurve}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "AbnormalStopOnCurve"
+}
+
+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 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 {
+		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}
+		IsCurve = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
+		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
+		if Automode == 1 && IsCurve && !IsEndPoint {
+			if data.VelocityX < 0.5 {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > threshold {
+					if !IsStopped {
+						IsStopped = true
+						return Label()
+					}
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
+			}
+		} else {
+			StartTime = 0
+			IsStopped = false
+		}
+	}
+	count1++
+	return ""
+}

trigger/pjisuv/cicv_location/CurveOverspeed/main/CurveOverspeed.go

@@ -0,0 +1,85 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	threshold  float64 = 3
+	IsCurve    bool
+	count1     int64
+	pointcurve = Point{39.73004426154644, 116.49248639463602}
+	pointlist1 = []Point{pointcurve}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CurveOverspeed"
+}
+
+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 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 {
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Automode = Automode.(int16)
+		IsCurve = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
+
+		if Automode == 1 && IsCurve {
+			absspeed := math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))
+			if absspeed >= threshold {
+				eventLabel := "CurveOverspeed"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+	}
+	count1++
+	return ""
+}