Merge remote-tracking branch 'gogs/master'

aarch64/pjisuv/master/service/produce_window.go

@@ -1500,13 +1500,15 @@ func ProduceWindow() {
 								continue
 							}
 							if _, ok := ObjDicOfTpperception[obj.Id]; !ok {
-								ObjDicOfTpperception[obj.Id] = [][]float32{{}, {}, {}, {}}
+								ObjDicOfTpperception[obj.Id] = [][]float32{{}, {}, {}, {}, {}}
 							}
 							ObjDicOfTpperception[obj.Id][0] = append(ObjDicOfTpperception[obj.Id][0], obj.X)
 							ObjDicOfTpperception[obj.Id][1] = append(ObjDicOfTpperception[obj.Id][1], obj.Y)
 							ObjDicOfTpperception[obj.Id][2] = append(ObjDicOfTpperception[obj.Id][2], obj.Vxrel)
 							absspeed := math.Sqrt(math.Pow(float64(obj.Vxabs), 2) + math.Pow(float64(obj.Vyabs), 2))
 							ObjDicOfTpperception[obj.Id][3] = append(ObjDicOfTpperception[obj.Id][3], float32(absspeed))
+							ObjDicOfTpperception[obj.Id][4] = append(ObjDicOfTpperception[obj.Id][4], obj.Heading)
+
 							objTypeDicOfTpperception[obj.Id] = obj.Type
 							objSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
 						}

trigger/pjisuv/cicv_location/EgoReversing/main/EgoReversing.go

@@ -0,0 +1,63 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+var (
+	count1 int = 0
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "EgoReversing"
+}
+func calculateDirectionAngle(speedX, speedY float64) float64 {
+	// 使用反正切函数计算方向角
+	angle := math.Atan2(speedY, speedX)
+
+	// 将角度转换为度数
+	angleDegree := angle * (180 / math.Pi)
+
+	// 将角度限定在 0~360 范围内
+	if angleDegree < 0 {
+		angleDegree += 360
+	} else if angleDegree > 360 {
+		angleDegree -= 360
+	}
+
+	return angleDegree
+}
+
+// 主进程的逻辑是先判断触发再缓存全局变量
+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%200 == 0 {
+		OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
+		if ok3 {
+			directionAngle := calculateDirectionAngle(data.VelocityX, data.VelocityY)
+			AbsSpeed := math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))
+			diffAngle := math.Abs(float64(directionAngle - data.Yaw))
+			//fmt.Println(diffAngle)
+			if AbsSpeed >= 1 && diffAngle >= 160 && diffAngle <= 200 && OutsideWorkshopFlag.(bool) {
+				eventLabel := "EgoReversing"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+	}
+	count1++
+	return ""
+}

trigger/pjisuv/cicv_location/FindTrafficLightP1/main/FindTrafficLightP1.go

@@ -42,8 +42,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 	if count1%10 == 0 {
 
 		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
-		VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation")
-		if enterflag && VelocityXOfCicvLocation.(float64) > 1 {
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
 			//eventLabel := "FindTrafficLight"
 			//fmt.Println(eventLabel)
 			return "FindTrafficLightP1"

trigger/pjisuv/cicv_location/FindTrafficLightP2/main/FindTrafficLightP2.go

@@ -42,8 +42,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 	if count1%10 == 0 {
 
 		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
-		VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation")
-		if enterflag && VelocityXOfCicvLocation.(float64) > 1 {
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
 			//eventLabel := "FindTrafficLight"
 			//fmt.Println(eventLabel)
 			return "FindTrafficLightP1"

trigger/pjisuv/cicv_location/FindTrafficLightP3/main/FindTrafficLightP3.go

@@ -42,8 +42,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 	if count1%10 == 0 {
 
 		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
-		VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation")
-		if enterflag && VelocityXOfCicvLocation.(float64) > 1 {
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
 			//eventLabel := "FindTrafficLight"
 			//fmt.Println(eventLabel)
 			return "FindTrafficLightP1"

trigger/pjisuv/cicv_location/FindTrafficLightP4/main/FindTrafficLightP4.go

@@ -42,8 +42,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 	if count1%10 == 0 {
 
 		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
-		VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation")
-		if enterflag && VelocityXOfCicvLocation.(float64) > 1 {
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
 			//eventLabel := "FindTrafficLight"
 			//fmt.Println(eventLabel)
 			return "FindTrafficLightP1"

trigger/pjisuv/cicv_location/OverSpeed/main/OverSpeed.go

@@ -24,8 +24,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 	//threshold := 65.0
 	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
 	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
-	threshold := 9999.0
-	if math.Pow(math.Pow(data.VelocityX, 2)+math.Pow(data.VelocityY, 2), 0.5)*3.6 >= threshold && OutsideWorkshopFlag == true {
+	threshold := 10.0
+	if math.Pow(math.Pow(data.VelocityX, 2)+math.Pow(data.VelocityY, 2), 0.5) >= threshold && OutsideWorkshopFlag == true {
 		return "OverSpeed"
 	} else {
 		return ""

trigger/pjisuv/cicv_ticker/FrontCarDrivingWrongDirection/main/FrontCarDrivingWrongDirection.go

@@ -0,0 +1,96 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	Maxlenobj int32 = 0
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontCarDrivingWrongDirection"
+}
+
+func Rule(shareVars *sync.Map) {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	// 1 使用goroutine
+	go func(shareVars *sync.Map) {
+		// 2 定义触发器的间隔时间
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+
+			}
+		}
+	}(shareVars)
+}
+
+func isWrongDirection(ObjectList [][]float32, AngularVelocityZ float64, YawOfCicvLocation float32) bool {
+
+	for i, heading := range ObjectList[4] {
+		objXi := ObjectList[0][i]
+		objYi := ObjectList[1][i]
+		Anglei := math.Abs(float64(heading - YawOfCicvLocation))
+
+		if math.Abs(float64(objYi)) <= 1.8 && math.Abs(AngularVelocityZ) <= 0.6 && objXi > 15 && Anglei <= 200 && Anglei >= 160 {
+			//fmt.Println(objY)
+			for j := 0; j < len(ObjectList[1])-i-1; j++ {
+				objXij := ObjectList[0][1+i+j]
+				objYij := ObjectList[1][1+i+j]
+				if math.Abs(float64(objYij)) <= 1.8 && math.Abs(AngularVelocityZ) <= 0.6 && objXij <= 10 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	YawOfCicvLocation, ok2 := shareVars.Load("YawOfCicvLocation")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	AngularVelocityZOfCicvLocation, _ := shareVars.Load("AngularVelocityZOfCicvLocation")
+	AngularVelocityZ := AngularVelocityZOfCicvLocation.(float64)
+
+	if ok && ok1 && ok2 && OutsideWorkshopFlag.(bool) == true {
+		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+			if len(ObjDic[0]) <= 10 || !isWrongDirection(objValue, AngularVelocityZ, YawOfCicvLocation.(float32)) {
+				continue
+			}
+			event_lable := "FrontCarDrivingWrongDirection"
+			fmt.Println(event_lable)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
+	}
+
+}

trigger/pjisuv/cicv_ticker/FrontVehicleBrake/main/FrontVehicleBrake.go

@@ -72,6 +72,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleBrake"
 			fmt.Println(event_lable)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 		if Maxlenobj >= 100 {

trigger/pjisuv/cicv_ticker/FrontVehicleBrakeInCurve/main/FrontVehicleBrakeInCurve.go

@@ -113,10 +113,11 @@ func FinalCallback(shareVars *sync.Map) {
 				}
 				event_lable := "FrontVehicleBrakeInCurve"
 				fmt.Println(event_lable)
+				ObjDicOfTpperception = make(map[uint32][][]float32)
 				pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 			}
 		}
-		if Maxlenobj >= 60 {
+		if Maxlenobj >= 100 {
 			ObjDicOfTpperception = make(map[uint32][][]float32)
 			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
 			Maxlenobj = 0

trigger/pjisuv/cicv_ticker/FrontVehicleBrakeInJunction/main/FrontVehicleBrakeInJunction.go

@@ -120,10 +120,11 @@ func FinalCallback(shareVars *sync.Map) {
 				}
 				event_lable := "FrontVehicleBrakeInJunction"
 				fmt.Println(event_lable)
+				ObjDicOfTpperception = make(map[uint32][][]float32)
 				pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 			}
 		}
-		if Maxlenobj >= 60 {
+		if Maxlenobj >= 100 {
 			ObjDicOfTpperception = make(map[uint32][][]float32)
 			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
 			Maxlenobj = 0

trigger/pjisuv/cicv_ticker/FrontVehicleCutInFar/main/FrontVehicleCutInFar.go

@@ -77,6 +77,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutInFar"
 			fmt.Println(event_lable)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/FrontVehicleCutInNear/main/FrontVehicleCutInNear.go

@@ -78,6 +78,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutInNear"
 			fmt.Println(event_lable)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/FrontVehicleCutOutFar/main/FrontVehicleCutOutFar.go

@@ -77,7 +77,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutOutFar"
 			fmt.Println(event_lable)
-			//ObjDicOfTpperception = make(map[uint32][][]float32)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/FrontVehicleCutOutNear/main/FrontVehicleCutOutNear.go

@@ -77,6 +77,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutOutNear"
 			fmt.Println(event_lable)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/GearJump/main/GearJump.go

@@ -64,7 +64,7 @@ func FinalCallback(shareVars *sync.Map) {
 		}
 
 		GearPosSlice = []int16{}
-		shareVars.Store("AccelXSlice", GearPosSlice)
+		shareVars.Store("GearPosSlice", GearPosSlice)
 
 	}
 

trigger/pjisuv/cicv_ticker/OutOperationZone/main/OutOperationZone.go

@@ -107,9 +107,10 @@ func Rule(shareVars *sync.Map) {
 func FinalCallback(shareVars *sync.Map) {
 	PositionX, ok1 := shareVars.Load("PositionXOfCicvLocation")
 	PositionY, ok2 := shareVars.Load("PositionYOfCicvLocation")
-	if ok1 && ok2 {
+	AbsSpeed, ok3 := shareVars.Load("AbsSpeed")
+	if ok1 && ok2 && ok3 {
 		p := Point{x: PositionX.(float64), y: PositionY.(float64)}
-		if !isPointInPolygon(p, vertices) {
+		if !isPointInPolygon(p, vertices) && AbsSpeed.(float64) >= 1 {
 			eventLabel := "OutOperationZone"
 			fmt.Println(eventLabel)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}

trigger/pjisuv/cicv_ticker/RearVehicleApproach/main/RearVehicleApproach.go

@@ -71,6 +71,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "RearVehicleApproach"
 			fmt.Println(event_lable)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}