update trigger LowSpdTruckAhead: remove fuzzy detection (size)

test/goroslib_test.go

@@ -10,6 +10,14 @@ import (
 	"time"
 )
 
+type Object struct {
+	ID            uint32
+	StableAge     int
+	LastExistTime float64
+}
+
+var objectsStability = make(map[uint32]Object)
+
 // speedCheck 目标物速度检测
 func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	var targetMinSpeed float32 = 2 / 3.6  // m/s
@@ -71,17 +79,67 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 
 	CGCS2000_X := 456256.260152
 	CGCS2000_Y := 4397809.886833
+	existingFrameThreshold := 5  // frame
+	existingTimeThreshold := 0.5 // s
+
+	//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))
+	//
+	//	// todo: 对各判断条件单独进行稳定性校验？
+	//	if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj) || sizeCheck(&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-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
+	//		//fmt.Println("!!!")
+	//		return "LowSpdTruckAhead"
+	//	}
+	//}
+	//return ""
 
 	fmt.Println()
-	fmt.Println(time.Unix(int64(msg.Header.TimeStamp), 0).Format(time.DateTime))
+	fmt.Println(time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 	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))
+		//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))
 
-		// todo: 对各判断条件单独进行稳定性校验？
+		// 判断目标物是否满足筛选条件
 		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj) || sizeCheck(&obj)) {
-			fmt.Println("!!!")
-			//return "LowSpdTruckAhead"
+
+			// 目标物初见存档
+			if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
+				objectsStability[obj.Id] = Object{
+					ID:            obj.Id,
+					StableAge:     1,
+					LastExistTime: msg.Header.TimeStamp,
+				}
+				fmt.Println("create: ", objectsStability[obj.Id])
+			} else {
+				currObjRecord := Object{
+					ID:            obj.Id,
+					StableAge:     1,
+					LastExistTime: msg.Header.TimeStamp,
+				}
+
+				// 目标物确认/更新/重置
+				if currObjRecord.LastExistTime-prevObjRecord.LastExistTime <= existingTimeThreshold { // 目标物消失不超过0.5s
+					currObjRecord.StableAge = prevObjRecord.StableAge + 1
+					if existingFrameThreshold == currObjRecord.StableAge {
+						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-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
+						delete(objectsStability, obj.Id)
+
+						continue // todo
+						//return "LowSpdTruckAhead"
+					} else {
+
+					}
+				} else {
+					fmt.Println("reset: ", currObjRecord)
+				}
+				objectsStability[obj.Id] = currObjRecord
+				fmt.Println("update: ", currObjRecord)
+			}
 		}
 	}
 	return ""

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

@@ -75,9 +75,7 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 	}()
 
 	for _, obj := range msg.Objs {
-
-		// todo: 对各判断条件单独进行稳定性校验？
-		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj) || sizeCheck(&obj)) {
+		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj)) {
 			return "LowSpdTruckAhead"
 		}
 	}