update trigger LowSpdTruckAhead & UnknownBigTargetAhead;

add trigger Multi[Target]Ahead;

test/ey_common_test.go

@@ -10,57 +10,27 @@ import (
 	"time"
 )
 
-type Object struct {
-	ID               uint32
+type Record struct {
 	StableFrame      int
 	LastExistingTime float64
 }
 
-var objectsStability = make(map[uint32]Object)
+var record *Record = nil
 
-const (
-	Cgcs2000X           = 456256.260152
-	Cgcs2000Y           = 4397809.886833
-	MinStableFrameCount = 5
-	MaxTimeBetweenFrame = 0.5
-)
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "test"
+}
 
 // typeCheck 目标物类型检测
 // × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
 // √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
 func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	const targetType uint8 = 0
+	const targetType uint8 = 4
 
 	return targetType == obj.Type
 }
 
-// sizeCheck 目标物大小检测
-func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	// 多功能车
-	const targetMinLength = 3.6   // m
-	const targetMinWidth = 1.605  // m
-	const targetMinHeight = 1.995 // m
-
-	// 金龙车
-	//targetMinLength := 5.99
-	//targetMinWidth := 2.065
-	//targetMinHeight := 2.82
-
-	// 至少满足两个条件
-	counter := 0
-	if obj.Length >= targetMinLength {
-		counter++
-	}
-	if obj.Width >= targetMinWidth {
-		counter++
-	}
-	if obj.Height >= targetMinHeight {
-		counter++
-	}
-
-	return counter > 1
-}
-
 // posCheck 判断目标物位置关系
 func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const laneWidth = 3.5 // m
@@ -68,7 +38,7 @@ func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
 }
 
-// Rule 检测前方未知大目标物
+// Rule 检测前方指定目标物数量
 func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
 		if r := recover(); r != nil {
@@ -76,52 +46,57 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 		}
 	}()
 
-	fmt.Println()
-	for _, obj := range msg.Objs {
-		//fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
-		//	obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
+	const Cgcs2000X = 456256.260152
+	const Cgcs2000Y = 4397809.886833
+	const MinStableFrameCount = 5   // frame
+	const MaxTimeBetweenFrame = 0.5 // second
+	const minTargetNum = 4
+
+	fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
+	if len(msg.Objs) >= minTargetNum {
 
-		// 判断目标物是否满足筛选条件
-		if typeCheck(&obj) && posCheck(&obj) && sizeCheck(&obj) {
-			fmt.Println(time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
+		currTargetNum := 0
+		for _, obj := range msg.Objs {
 
-			// 目标物初见存档
-			if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
-				objectsStability[obj.Id] = Object{
-					ID:               obj.Id,
+			// 判断目标物是否满足筛选条件
+			if typeCheck(&obj) && posCheck(&obj) {
+				fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
+					obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
+				currTargetNum++
+			}
+		}
+
+		if currTargetNum >= minTargetNum {
+			if nil == record {
+				record = &Record{
 					StableFrame:      1,
 					LastExistingTime: msg.Header.TimeStamp,
 				}
-				fmt.Println("create: ", objectsStability[obj.Id])
+				fmt.Println("---------- create ----------")
 			} else {
-				currObjRecord := Object{
-					ID:               obj.Id,
+				newRecord := Record{
 					StableFrame:      1,
 					LastExistingTime: msg.Header.TimeStamp,
 				}
 
-				// 目标物确认/更新/重置
-				if currObjRecord.LastExistingTime-prevObjRecord.LastExistingTime <= MaxTimeBetweenFrame { // 目标物消失不超过0.5s
-					currObjRecord.StableFrame = prevObjRecord.StableFrame + 1
-					if currObjRecord.StableFrame == MinStableFrameCount {
-						fmt.Println("found: ", currObjRecord)
-						fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
-							obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
-						delete(objectsStability, obj.Id)
-
-						//continue // todo
-						//return "UnknownBigTargetAhead"
+				if newRecord.LastExistingTime-record.LastExistingTime <= MaxTimeBetweenFrame {
+					newRecord.StableFrame = record.StableFrame + 1
+					if newRecord.StableFrame == MinStableFrameCount {
+						record = nil
+						fmt.Println("!!!!!!!!!! found !!!!!!!!!!") // todo
+						return Label()
 					} else {
-						fmt.Println("update: ", currObjRecord)
-						objectsStability[obj.Id] = currObjRecord
+						fmt.Println("---------- update ----------")
 					}
 				} else {
-					fmt.Println("reset: ", currObjRecord)
-					objectsStability[obj.Id] = currObjRecord
+					fmt.Println("---------- reset ----------")
 				}
+				record = &newRecord
 			}
 		}
+
 	}
+
 	return ""
 }
 

trigger/pjisuv/tpperception/lowSpdTruckAhead/main/lowSpdTruckAhead.go

@@ -4,7 +4,6 @@ import (
 	"cicv-data-closedloop/pjisuv_msgs"
 	"fmt"
 	"math"
-	"time"
 )
 
 type Object struct {
@@ -15,13 +14,6 @@ type Object struct {
 
 var objectsStability = make(map[uint32]Object)
 
-const (
-	Cgcs2000X           = 456256.260152
-	Cgcs2000Y           = 4397809.886833
-	MinStableFrameCount = 5
-	MaxTimeBetweenFrame = 0.5
-)
-
 func Topic() string {
 	return "/tpperception"
 }
@@ -59,15 +51,14 @@ func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
 		if r := recover(); r != nil {
-			fmt.Println("Recovered from panic:", r)
+			fmt.Println(fmt.Sprintf("Recovered from panic: [%s], [%#v]", Label(), r))
 		}
 	}()
 
-	fmt.Println()
-	fmt.Println(time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
+	const MinStableFrameCount = 5
+	const MaxTimeBetweenFrame = 0.5
+
 	for _, obj := range msg.Objs {
-		//fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
-		//	obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
 
 		// 判断目标物是否满足筛选条件
 		if speedCheck(&obj) && posCheck(&obj) && typeCheck(&obj) {
@@ -79,7 +70,6 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 					StableAge:     1,
 					LastExistTime: msg.Header.TimeStamp,
 				}
-				fmt.Println("create: ", objectsStability[obj.Id])
 			} else {
 				currObjRecord := Object{
 					ID:            obj.Id,
@@ -91,21 +81,11 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 				if currObjRecord.LastExistTime-prevObjRecord.LastExistTime <= MaxTimeBetweenFrame { // 目标物消失不超过0.5s
 					currObjRecord.StableAge = prevObjRecord.StableAge + 1
 					if currObjRecord.StableAge == MinStableFrameCount {
-						fmt.Println("found: ", currObjRecord)
-						fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
-							obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
 						delete(objectsStability, obj.Id)
-
-						//continue // todo
-						return "LowSpdTruckAhead"
-					} else {
-						//fmt.Println("update: ", currObjRecord)
-						objectsStability[obj.Id] = currObjRecord
+						return Label()
 					}
-				} else {
-					//fmt.Println("reset: ", currObjRecord)
-					objectsStability[obj.Id] = currObjRecord
 				}
+				objectsStability[obj.Id] = currObjRecord
 			}
 		}
 	}

trigger/pjisuv/tpperception/multiTargetAhead/main/multiTargetAhead.go

@@ -0,0 +1,100 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+// Label todo 禁止存在下划线_
+// MultiPedestrianAhead
+// MultiCarAhead
+// MultiTruckAhead
+// MultiBicycleAhead
+// MultiTricycleAhead
+// MultiTrafficConeAhead
+func Label() string {
+	return "MultiPedestrianAhead"
+}
+
+type Record struct {
+	StableFrame      int
+	LastExistingTime float64
+}
+
+var record *Record = nil
+
+// typeCheck 目标物类型检测
+// × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
+// √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
+func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	const targetType uint8 = 1
+
+	return targetType == obj.Type
+}
+
+// posCheck 判断目标物位置关系
+func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	const laneWidth = 3.5 // m
+
+	return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
+}
+
+// Rule 检测前方指定目标物数量
+func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+
+	const Cgcs2000X = 456256.260152
+	const Cgcs2000Y = 4397809.886833
+	const MinStableFrameCount = 5   // frame
+	const MaxTimeBetweenFrame = 0.5 // second
+	const minTargetNum = 4
+
+	if len(msg.Objs) >= minTargetNum {
+
+		currTargetNum := 0
+		for _, obj := range msg.Objs {
+
+			// 判断目标物是否满足筛选条件
+			if typeCheck(&obj) && posCheck(&obj) {
+				currTargetNum++
+			}
+		}
+
+		if currTargetNum >= minTargetNum {
+			if nil == record {
+				record = &Record{
+					StableFrame:      1,
+					LastExistingTime: msg.Header.TimeStamp,
+				}
+			} else {
+				newRecord := Record{
+					StableFrame:      1,
+					LastExistingTime: msg.Header.TimeStamp,
+				}
+
+				if newRecord.LastExistingTime-record.LastExistingTime <= MaxTimeBetweenFrame {
+					newRecord.StableFrame = record.StableFrame + 1
+					if newRecord.StableFrame == MinStableFrameCount {
+						record = nil
+						return Label()
+					} else {
+					}
+				} else {
+				}
+				record = &newRecord
+			}
+		}
+
+	}
+
+	return ""
+}

trigger/pjisuv/tpperception/unknownBigTargetAhead/main/unknownBigTargetAhead.go

@@ -6,21 +6,6 @@ import (
 	"math"
 )
 
-type Object struct {
-	ID               uint32
-	StableFrame      int
-	LastExistingTime float64
-}
-
-var objectsStability = make(map[uint32]Object)
-
-const (
-	Cgcs2000X           = 456256.260152
-	Cgcs2000Y           = 4397809.886833
-	MinStableFrameCount = 5
-	MaxTimeBetweenFrame = 0.5
-)
-
 func Topic() string {
 	return "/tpperception"
 }
@@ -30,6 +15,14 @@ func Label() string {
 	return "UnknownBigTargetAhead"
 }
 
+type Object struct {
+	ID               uint32
+	StableFrame      int
+	LastExistingTime float64
+}
+
+var objectsStability = make(map[uint32]Object)
+
 // typeCheck 目标物类型检测
 // × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
 // √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
@@ -77,10 +70,13 @@ func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
 		if r := recover(); r != nil {
-			fmt.Println("Recovered from panic:", r)
+			fmt.Println(fmt.Sprintf("Recovered from panic: [%s], [%#v]", Label(), r))
 		}
 	}()
 
+	const MinStableFrameCount = 5   // frame
+	const MaxTimeBetweenFrame = 0.5 // second
+
 	for _, obj := range msg.Objs {
 
 		// 判断目标物是否满足筛选条件
@@ -105,14 +101,10 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 					currObjRecord.StableFrame = prevObjRecord.StableFrame + 1
 					if currObjRecord.StableFrame == MinStableFrameCount {
 						delete(objectsStability, obj.Id)
-
-						return "UnknownBigTargetAhead"
-					} else {
-						objectsStability[obj.Id] = currObjRecord
+						return Label()
 					}
-				} else {
-					objectsStability[obj.Id] = currObjRecord
 				}
+				objectsStability[obj.Id] = currObjRecord
 			}
 		}
 	}