modify AbnormalParking trigger

add LocationJump trigger

aarch64/pjisuv/master/config/trigger_var.go

@@ -250,4 +250,10 @@ var (
 	RuleOfPjVehicleFdbPub1 []func(data *pjisuv_msgs.VehicleFdb) string
 	RuleOfPjVehicleFdbPub2 []func(data *pjisuv_msgs.VehicleFdb, param *pjisuv_param.PjisuvParam) string
 	RuleOfPjVehicleFdbPub3 []func(shareVars *sync.Map, data *pjisuv_msgs.VehicleFdb) string
+
+	//40
+	TopicOfEndPointMessage = "end_point_message"
+	RuleOfEndPointMessage1 []func(data *geometry_msgs.Point) string
+	RuleOfEndPointMessage2 []func(data *geometry_msgs.Point, param *pjisuv_param.PjisuvParam) string
+	RuleOfEndPointMessage3 []func(shareVars *sync.Map, data *geometry_msgs.Point) string
 )

aarch64/pjisuv/master/service/produce_window.go

@@ -684,18 +684,6 @@ func ProduceWindow() {
 					Node:  commonConfig.RosNode,
 					Topic: topic,
 					Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
-						// 更新共享变量
-						mutexOfCicvLocation.RLock()
-						{
-							pjisuvParam.VelocityXOfCicvLocation = data.VelocityX
-							pjisuvParam.VelocityYOfCicvLocation = data.VelocityY
-							pjisuvParam.VelocityZOfCicvLocation = data.VelocityZ
-							pjisuvParam.YawOfCicvLocation = data.Yaw
-							pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
-							pjisuvParam.PositionXOfCicvLocation = data.PositionX
-							pjisuvParam.PositionYOfCicvLocation = data.PositionY
-						}
-						mutexOfCicvLocation.RUnlock()
 
 						subscribersTimeMutexes[i].Lock()
 						if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
@@ -739,6 +727,22 @@ func ProduceWindow() {
 							subscribersMutexes[i].Unlock()
 						}
 						subscribersTimeMutexes[i].Unlock()
+						// 更新共享变量
+						mutexOfCicvLocation.RLock()
+						{
+							//shareVars.Store("VelocityXOfCicvLocation",data.VelocityX) pjisuvParam以后不要新加全局变量了，采用这种方式添加
+							//value, _ := shareVars.Load("VelocityXOfCicvLocation") 在Rule函数数中通过Load方法获取全局变量，获取之后通过value.(float64)方法转换数据类型
+							//s := value.(float64)
+
+							pjisuvParam.VelocityXOfCicvLocation = data.VelocityX
+							pjisuvParam.VelocityYOfCicvLocation = data.VelocityY
+							pjisuvParam.VelocityZOfCicvLocation = data.VelocityZ
+							pjisuvParam.YawOfCicvLocation = data.Yaw
+							pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
+							pjisuvParam.PositionXOfCicvLocation = data.PositionX
+							pjisuvParam.PositionYOfCicvLocation = data.PositionY
+						}
+						mutexOfCicvLocation.RUnlock()
 					},
 				})
 			}
@@ -2196,6 +2200,7 @@ func ProduceWindow() {
 					Node:  commonConfig.RosNode,
 					Topic: topic,
 					Callback: func(data *pjisuv_msgs.VehicleFdb) {
+						shareVars.Store("Automode", data.Automode)
 						subscribersTimeMutexes[i].Lock()
 						if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
 							subscribersMutexes[i].Lock()
@@ -2242,6 +2247,63 @@ func ProduceWindow() {
 					},
 				})
 			}
+			// 40 有共享变量的订阅者必须被创建
+			if topic == masterConfig.TopicOfEndPointMessage {
+				subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+					Node:  commonConfig.RosNode,
+					Topic: topic,
+					Callback: func(data *geometry_msgs.Point) {
+						// 更新共享变量
+
+						shareVars.Store("EndPointX", data.X)
+						shareVars.Store("EndPointY", data.Y)
+
+						subscribersTimeMutexes[i].Lock()
+						if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
+							subscribersMutexes[i].Lock()
+							faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+							lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
+							faultLabel := ""
+							if len(masterConfig.RuleOfEndPointMessage1) > 0 {
+								for _, f := range masterConfig.RuleOfEndPointMessage1 {
+									faultLabel = f(data)
+									if faultLabel != "" {
+										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+
+										subscribersTimes[i] = time.Now()
+										goto TriggerSuccess
+									}
+								}
+							}
+
+							if len(masterConfig.RuleOfEndPointMessage2) > 0 {
+								for _, f := range masterConfig.RuleOfEndPointMessage2 {
+									faultLabel = f(data, &pjisuvParam)
+									if faultLabel != "" {
+										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+
+										subscribersTimes[i] = time.Now()
+										goto TriggerSuccess
+									}
+								}
+							}
+							if len(masterConfig.RuleOfEndPointMessage3) > 0 {
+								for _, f := range masterConfig.RuleOfEndPointMessage3 {
+									faultLabel = f(shareVars, data)
+									if faultLabel != "" {
+										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+										subscribersTimes[i] = time.Now()
+										goto TriggerSuccess
+									}
+								}
+							}
+						TriggerSuccess:
+							subscribersMutexes[i].Unlock()
+						}
+						subscribersTimeMutexes[i].Unlock()
+					},
+				})
+			}
 			if err != nil {
 				c_log.GlobalLogger.Info("创建订阅者报错，可能由于节点未启动，再次尝试：", err)
 				time.Sleep(time.Duration(2) * time.Second)

trigger/pjisuv/cicv_location/AbnormalParking/main/AbnormalParking.go

@@ -2,14 +2,30 @@ package main
 
 import (
 	"cicv-data-closedloop/pjisuv_msgs"
-	"cicv-data-closedloop/pjisuv_param"
 	"fmt"
+	"math"
+	"sync"
 	"time"
 )
 
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
 var (
-	StartTime int64
-	IsStopped bool
+	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 {
@@ -21,35 +37,77 @@ func Label() string {
 	return "AbnormalParking"
 }
 
-func Rule(data *pjisuv_msgs.PerceptionLocalization, param *pjisuv_param.PjisuvParam) string {
+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 param.AutomodeOfPjVehicleFdbPub == 1 && param.DecisionType != 4 {
-		if data.VelocityX < 0.5 {
-			// 如果之前没有记录开始时间，记录当前时间
-			if StartTime == 0 {
-				StartTime = time.Now().Unix()
-			}
-			// 判断是否持续超过 50s
-			if time.Now().Unix()-StartTime > 5 {
-				if !IsStopped {
-					eventLabel := "AbnormalParking"
-					fmt.Println(eventLabel)
-					IsStopped = true
-					return "AbnormalParking"
+	if count1%10 == 0 {
+		Automode, _ := shareVars.Load("Automode")
+		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 {
+			if data.VelocityX < 0.5 {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 5 {
+					if !IsStopped {
+						eventLabel := "AbnormalParking"
+						fmt.Println(eventLabel)
+						IsStopped = true
+						return "AbnormalParking"
+					}
 				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
 			}
 		} else {
-			// 如果速度大于 0.1，重置开始时间和停止标志
 			StartTime = 0
 			IsStopped = false
 		}
-	} else {
-		StartTime = 0
-		IsStopped = false
 	}
+	count1++
 	return ""
 }

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

@@ -0,0 +1,36 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"cicv-data-closedloop/pjisuv_param"
+	"fmt"
+	"math"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "LocationJump"
+}
+
+// 主进程的逻辑是先判断触发再缓存全局变量
+func Rule(data *pjisuv_msgs.PerceptionLocalization, param *pjisuv_param.PjisuvParam) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if param.PositionXOfCicvLocation != 0 && param.PositionYOfCicvLocation != 0 {
+		d := math.Sqrt((param.PositionXOfCicvLocation-data.PositionX)*(param.PositionXOfCicvLocation-data.PositionX) + (param.PositionYOfCicvLocation-data.PositionY)*(param.PositionYOfCicvLocation-data.PositionY))
+		if d >= 2 {
+			eventLabel := "LocationJump"
+			fmt.Println(eventLabel)
+			return "LocationJump"
+		}
+	}
+
+	return ""
+}

trigger/pjisuv/map_polygon/OutOfLane/main/OutOfLane.go

@@ -70,7 +70,7 @@ func Rule(data *pjisuv_msgs.PolygonStamped, param *pjisuv_param.PjisuvParam) str
 		}
 	}()
 	Points := data.Polygon.Points
-	D1, D2, D3 := 3.813, 0.952, 1.086
+	D1, D2, D3 := 1.0, 0.4, 0.4 //这是实车缩小后的尺寸
 	corners := getVehicleCorners(param.PositionXOfCicvLocation, param.PositionYOfCicvLocation, D1, D2, D3, param.YawOfCicvLocation)
 
 	for i := 0; i < len(Points)-1; i++ {