add TargetCarBehindWhenReversing trigger

trigger/pjibot_delivery/wheel_odom/AuLongStop/main/AuLongStop.go

@@ -0,0 +1,49 @@
+package main
+
+import (
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
+	"math"
+	"time"
+)
+
+func Topic() string {
+	return "/wheel_odom"
+}
+
+func Label() string {
+	return "AuLongStop"
+}
+
+var (
+	StartTime int64
+	IsStopped bool
+)
+
+func Rule(data *nav_msgs.Odometry) string {
+	velocityX := data.Twist.Twist.Linear.X
+	velocityY := data.Twist.Twist.Linear.Y
+	absV := math.Pow(math.Pow(velocityX, 2)+math.Pow(velocityY, 2), 0.5)
+	if absV < 0.3 {
+		// 如果之前没有记录开始时间，记录当前时间
+		if StartTime == 0 {
+			StartTime = time.Now().Unix()
+		}
+
+		// 判断是否持续超过一分钟
+		if time.Now().Unix()-StartTime > 10 {
+			if !IsStopped {
+				event_label := "AuLongStop"
+				fmt.Println(event_label)
+				IsStopped = true
+				return Label()
+			}
+		}
+	} else {
+		// 如果速度大于 0.1，重置开始时间和停止标志
+		StartTime = 0
+		//is_stopped = false
+	}
+	return ""
+
+}

trigger/pjisuv/cicv_location/BrakeWithHighSpeed/main/BrakeWithHighSpeed.go

@@ -0,0 +1,35 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+)
+
+var (
+	threshold float64 = 5.4
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "BrakeWithHighSpeed"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	AbsSpeed, _ := shareVars.Load("AbsSpeed")
+	if data.AccelX*9.8 < -12.0 && OutsideWorkshopFlag == true && AbsSpeed.(float64) > threshold {
+		return Label()
+	}
+	return ""
+}

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

@@ -47,10 +47,11 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 		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))
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
 			diffAngle := math.Abs(float64(directionAngle - data.Yaw))
 			//fmt.Println(diffAngle)
-			if AbsSpeed >= 1 && diffAngle >= 160 && diffAngle <= 200 && OutsideWorkshopFlag.(bool) {
+			if AbsSpeed.(float64) >= 1 && diffAngle >= 160 && diffAngle <= 200 && OutsideWorkshopFlag.(bool) {
+
 				eventLabel := "EgoReversing"
 				fmt.Println(eventLabel)
 				count1 = 1

trigger/pjisuv/cicv_location/TargetCarBehindWhenReversing/main/TargetCarBehindWhenReversing.go

@@ -0,0 +1,83 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "TargetCarBehindWhenReversing"
+}
+
+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] >= -15 && obj[0][len(obj[0])-1] <= -1 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 6 {
+				return true
+
+			}
+		}
+
+	}
+	return false
+}
+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%100 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		directionAngle := calculateDirectionAngle(data.VelocityX, data.VelocityY)
+		diffAngle := math.Abs(float64(directionAngle - data.Yaw))
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if flag && AbsSpeed.(float64) >= 1 && diffAngle >= 160 && diffAngle <= 200 {
+				eventLabel := "TargetCarBehindWhenReversing"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+
+		}
+	}
+	count1++
+	return ""
+}

trigger/pjisuv/cicv_ticker/BeOvertakenInCorner/main/BeOvertakenInCorner.go

@@ -118,6 +118,7 @@ func FinalCallback(shareVars *sync.Map) {
 				}
 				event_lable := "BeOvertakenInCorner"
 				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/BeOvertakenWithHighSpeed/main/BeOvertakenWithHighSpeed.go

@@ -0,0 +1,92 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	Maxlenobj int32   = 0
+	threshold float64 = 5.0
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "BeOvertakenWithHighSpeed"
+}
+
+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 IsOvertaken(ObjectList [][]float32) bool {
+	//fmt.Println("yes")
+	//fmt.Println(ObjectList)
+	for i, objX := range ObjectList[0] {
+
+		if math.Abs(float64(ObjectList[1][i])) <= 4 && objX <= -2 {
+			for j := 0; j < len(ObjectList[0])-i-1; j++ {
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 4 && ObjectList[0][1+i+j] >= 2.5 {
+					return true
+				}
+			}
+		}
+	}
+
+	return false
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	AbsSpeed, ok3 := shareVars.Load("AbsSpeed")
+
+	if ok && ok3 && AbsSpeed.(float64) >= threshold {
+		ObjDicOfTpperception, okn := shareVars.Load("objDicOfTpperception")
+		ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+
+		if okn && OutsideWorkshopFlag.(bool) == true {
+			for _, objValue := range ObjDic {
+				Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+				if len(ObjDic[0]) <= 10 || !IsOvertaken(objValue) {
+					continue
+				}
+				event_lable := "BeOvertakenWithHighSpeed"
+				fmt.Println(event_lable)
+				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/HuaLong/main/HuaLong.go

@@ -76,7 +76,7 @@ func FinalCallback(shareVars *sync.Map) {
 			fmt.Println(event_lable)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
-		if len(egoSteeringRealOfDataRead.([]float64)) >= 600 {
+		if len(egoSteeringRealOfDataRead.([]float64)) >= 800 {
 			egoSteeringRealOfDataRead = []float64{}
 			shareVars.Store("egoSteeringRealOfDataRead", egoSteeringRealOfDataRead)
 		}

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

@@ -50,7 +50,7 @@ func isApproach(ObjectList [][]float32) bool {
 
 		if math.Abs(float64(ObjectList[1][i])) <= 1.3 && objX <= -10 {
 			for j := 0; j < len(ObjectList[0])-i-1; j++ {
-				if math.Abs(float64(ObjectList[1][1+i+j])) <= 1.3 && ObjectList[0][1+i+j] >= -8 && ObjectList[2][1+i+j] >= 1 {
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 1.3 && ObjectList[0][1+i+j] >= -6 && ObjectList[2][1+i+j] >= 1 {
 					return true
 				}
 			}