commit

aarch64/pjisuv/master/service/produce_window.go

@@ -2237,6 +2237,7 @@ func ProduceWindow() {
 						subscribersTimeMutexes[i].Unlock()
 						// 更新共享变量
 						shareVars.Store("Automode", data.Automode)
+						shareVars.Store("AutomodeOfPjVehicleFdbPub", data.Automode)
 					},
 				})
 			}

pjisuv_param/pjisuv_param.go

@@ -1,15 +1,15 @@
 package pjisuv_param
 
 type PjisuvParam struct {
-	WhetherOnfRoad bool
-	// /cicv_location
-	VelocityXOfCicvLocation        float64
-	VelocityYOfCicvLocation        float64
-	VelocityZOfCicvLocation        float64
-	YawOfCicvLocation              float64
-	AngularVelocityZOfCicvLocation float64
-	PositionXOfCicvLocation        float64
-	PositionYOfCicvLocation        float64
+	//WhetherOnfRoad bool
+	//// /cicv_location
+	//VelocityXOfCicvLocation        float64
+	//VelocityYOfCicvLocation        float64
+	//VelocityZOfCicvLocation        float64
+	//YawOfCicvLocation              float64
+	//AngularVelocityZOfCicvLocation float64
+	//PositionXOfCicvLocation        float64
+	//PositionYOfCicvLocation        float64
 
 	// /tpperception
 	ObjDicOfTpperception      map[uint32][]float32
@@ -26,7 +26,7 @@ type PjisuvParam struct {
 	EgoThrottleRealOfDataRead    []float64
 	StrgAngleRealValueOfDataRead float64
 	// /pj_vehicle_fdb_pub
-	AutomodeOfPjVehicleFdbPub int16
+	//AutomodeOfPjVehicleFdbPub int16
 
 	// /cicv_amr_trajectory
 	LastCurvaturesOfCicvAmrTrajectory []float64

trigger/pjisuv/cicv_location/AuLongStop/main/AuLongStop.go

@@ -2,8 +2,8 @@ package main
 
 import (
 	"cicv-data-closedloop/pjisuv_msgs"
-	"cicv-data-closedloop/pjisuv_param"
 	"fmt"
+	"sync"
 	"time"
 )
 
@@ -16,40 +16,42 @@ func Topic() string {
 	return "/cicv_location"
 }
 
-// Label todo 禁止存在下划线_
+// 禁止存在下划线_
 func Label() string {
 	return "AuLongStop"
 }
 
-func Rule(data *pjisuv_msgs.PerceptionLocalization, param *pjisuv_param.PjisuvParam) string {
+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 {
-		if data.VelocityX < 0.5 {
-			// 如果之前没有记录开始时间，记录当前时间
-			if StartTime == 0 {
-				StartTime = time.Now().Unix()
-			}
-			// 判断是否持续超过 50s
-			if time.Now().Unix()-StartTime > 50 {
-				if !IsStopped {
-					eventLabel := "AuLongStop"
-					fmt.Println(eventLabel)
-					IsStopped = true
-					return "AuLongStop"
+	automodeOfPjVehicleFdbPub, ok := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+	if ok {
+		if automodeOfPjVehicleFdbPub.(int16) == 1 {
+			if data.VelocityX < 0.5 {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 50 {
+					if !IsStopped {
+						IsStopped = true
+						return Label()
+					}
 				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
 			}
 		} else {
-			// 如果速度大于 0.1，重置开始时间和停止标志
 			StartTime = 0
 			IsStopped = false
 		}
-	} else {
-		StartTime = 0
-		IsStopped = false
 	}
+
 	return ""
 }

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

@@ -2,9 +2,9 @@ package main
 
 import (
 	"cicv-data-closedloop/pjisuv_msgs"
-	"cicv-data-closedloop/pjisuv_param"
 	"fmt"
 	"math"
+	"sync"
 )
 
 func Topic() string {
@@ -17,20 +17,24 @@ func Label() string {
 }
 
 // 主进程的逻辑是先判断触发再缓存全局变量
-func Rule(data *pjisuv_msgs.PerceptionLocalization, param *pjisuv_param.PjisuvParam) string {
+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.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"
+
+	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 ""
 }

trigger/pjisuv/cicv_location/OverSwing/main/OverSwing.go

@@ -2,9 +2,9 @@ package main
 
 import (
 	"cicv-data-closedloop/pjisuv_msgs"
-	"cicv-data-closedloop/pjisuv_param"
 	"fmt"
 	"math"
+	"sync"
 )
 
 /*
@@ -35,15 +35,17 @@ func Label() string {
 	return "OverSwing"
 }
 
-func Rule(data *pjisuv_msgs.PerceptionLocalization, param *pjisuv_param.PjisuvParam) string {
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
 	defer func() {
 		if r := recover(); r != nil {
 			fmt.Println("Recovered from panic:", r)
 		}
 	}()
-	if math.Abs(data.AngularVelocityZ) >= 27.0 && param.AutomodeOfPjVehicleFdbPub == 1 {
-		return "OverSwing"
-	} else {
-		return ""
+	automodeOfPjVehicleFdbPub, ok := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+	if ok {
+		if math.Abs(data.AngularVelocityZ) >= 27.0 && automodeOfPjVehicleFdbPub.(int16) == 1 {
+			return Label()
+		}
 	}
+	return ""
 }

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

@@ -4,9 +4,9 @@ package main
 
 import (
 	"cicv-data-closedloop/pjisuv_msgs"
-	"cicv-data-closedloop/pjisuv_param"
 	"fmt"
 	"math"
+	"sync"
 )
 
 func Topic() string {
@@ -17,6 +17,31 @@ func Label() string {
 	return "OutOfLane"
 }
 
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PolygonStamped) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	positionXOfCicvLocation, ok1 := shareVars.Load("PositionXOfCicvLocation")
+	positionYOfCicvLocation, ok2 := shareVars.Load("PositionYOfCicvLocation")
+	yawOfCicvLocation, ok3 := shareVars.Load("YawOfCicvLocation")
+	automodeOfPjVehicleFdbPub, ok4 := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+	if ok1 && ok2 && ok3 && ok4 {
+		Points := data.Polygon.Points
+		D1, D2, D3 := 1.0, 0.4, 0.4 //这是实车缩小后的尺寸
+		corners := getVehicleCorners(positionXOfCicvLocation.(float64), positionYOfCicvLocation.(float64), D1, D2, D3, yawOfCicvLocation.(float64))
+		for i := 0; i < len(Points)-1; i++ {
+			A := Points[i]
+			B := Points[i+1]
+			if polygonLineIntersect(corners, A, B) && automodeOfPjVehicleFdbPub.(int16) == 1 {
+				return Label()
+			}
+		}
+	}
+	return ""
+}
+
 type Point struct {
 	X, Y float64
 }
@@ -62,24 +87,3 @@ func polygonLineIntersect(polygon []pjisuv_msgs.Point64, A, B pjisuv_msgs.Point6
 
 	return false
 }
-
-func Rule(data *pjisuv_msgs.PolygonStamped, param *pjisuv_param.PjisuvParam) string {
-	defer func() {
-		if r := recover(); r != nil {
-			fmt.Println("Recovered from panic:", r)
-		}
-	}()
-	Points := data.Polygon.Points
-	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++ {
-		A := Points[i]
-		B := Points[i+1]
-		if polygonLineIntersect(corners, A, B) && param.AutomodeOfPjVehicleFdbPub == 1 {
-			return "OutOfLane"
-		}
-	}
-
-	return ""
-}

trigger/pjisuv/pj_vehicle_fdb_pub/DriverTakeOver/main/DriverTakeOver.go

@@ -2,8 +2,8 @@ package main
 
 import (
 	"cicv-data-closedloop/pjisuv_msgs"
-	"cicv-data-closedloop/pjisuv_param"
 	"fmt"
+	"sync"
 )
 
 /*
@@ -25,17 +25,17 @@ func Label() string {
 	return "DriverTakeOver"
 }
 
-func Rule(data *pjisuv_msgs.VehicleFdb, param *pjisuv_param.PjisuvParam) string {
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.VehicleFdb) string {
 	defer func() {
 		if r := recover(); r != nil {
 			fmt.Println("Recovered from panic:", r)
 		}
 	}()
-	label := ""
-	if data.Automode == 0 && param.AutomodeOfPjVehicleFdbPub == 1 {
-		label = "DriverTakeOver"
+	automodeOfPjVehicleFdbPub, ok := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+	if ok {
+		if data.Automode == 0 && automodeOfPjVehicleFdbPub.(int16) == 1 {
+			return Label()
+		}
 	}
-	//fmt.Println("当前自动驾驶状态为：", data.Automode, "上一个自动驾驶状态为：", param.AutomodeOfPjVehicleFdbPub)
-	param.AutomodeOfPjVehicleFdbPub = data.Automode
-	return label
+	return ""
 }