update trigger LowSpdTruckAhead: obj stability check;

add trigger UnknownBigTargetAhead;

test/goroslib_test.go → test/ey_common_test.go

@@ -11,65 +11,64 @@ import (
 )
 
 type Object struct {
-	ID            uint32
-	StableAge     int
-	LastExistTime float64
+	ID               uint32
+	StableFrame      int
+	LastExistingTime float64
 }
 
 var objectsStability = make(map[uint32]Object)
 
-// speedCheck 目标物速度检测
-func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	var targetMinSpeed float32 = 2 / 3.6  // m/s
-	var targetMaxSpeed float32 = 20 / 3.6 // m/s
-
-	if targetMinSpeed < obj.Speed && obj.Speed < targetMaxSpeed {
-		return true
-	}
-	return false
-}
+const (
+	Cgcs2000X           = 456256.260152
+	Cgcs2000Y           = 4397809.886833
+	MinStableFrameCount = 5
+	MaxTimeBetweenFrame = 0.5
+)
 
 // 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 {
-	var targetType uint8 = 3
+	const targetType uint8 = 0
 
-	if targetType == obj.Type {
-		return true
-	}
-	return false
+	return targetType == obj.Type
 }
 
 // sizeCheck 目标物大小检测
 func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	// 多功能车
-	var targetMinLength float32 = 3.6   // m
-	var targetMinWidth float32 = 1.605  // m
-	var targetMinHeight float32 = 1.995 // m
+	const targetMinLength = 3.6   // m
+	const targetMinWidth = 1.605  // m
+	const targetMinHeight = 1.995 // m
 
 	// 金龙车
 	//targetMinLength := 5.99
 	//targetMinWidth := 2.065
 	//targetMinHeight := 2.82
 
-	if obj.Length > targetMinLength || obj.Width > targetMinWidth || obj.Height > targetMinHeight {
-		return true
+	// 至少满足两个条件
+	counter := 0
+	if obj.Length >= targetMinLength {
+		counter++
+	}
+	if obj.Width >= targetMinWidth {
+		counter++
 	}
-	return false
+	if obj.Height >= targetMinHeight {
+		counter++
+	}
+
+	return counter > 1
 }
 
 // posCheck 判断目标物位置关系
 func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	laneWidth := 3.5 // m
+	const laneWidth = 3.5 // m
 
-	if obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5 {
-		return true
-	}
-	return false
+	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 {
@@ -77,68 +76,49 @@ 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), 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))
 
 		// 判断目标物是否满足筛选条件
-		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj) || sizeCheck(&obj)) {
+		if typeCheck(&obj) && posCheck(&obj) && sizeCheck(&obj) {
+			fmt.Println(time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 
 			// 目标物初见存档
 			if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
 				objectsStability[obj.Id] = Object{
-					ID:            obj.Id,
-					StableAge:     1,
-					LastExistTime: msg.Header.TimeStamp,
+					ID:               obj.Id,
+					StableFrame:      1,
+					LastExistingTime: msg.Header.TimeStamp,
 				}
 				fmt.Println("create: ", objectsStability[obj.Id])
 			} else {
 				currObjRecord := Object{
-					ID:            obj.Id,
-					StableAge:     1,
-					LastExistTime: msg.Header.TimeStamp,
+					ID:               obj.Id,
+					StableFrame:      1,
+					LastExistingTime: msg.Header.TimeStamp,
 				}
 
 				// 目标物确认/更新/重置
-				if currObjRecord.LastExistTime-prevObjRecord.LastExistTime <= existingTimeThreshold { // 目标物消失不超过0.5s
-					currObjRecord.StableAge = prevObjRecord.StableAge + 1
-					if existingFrameThreshold == currObjRecord.StableAge {
+				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-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
+							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"
+						//continue // todo
+						//return "UnknownBigTargetAhead"
 					} else {
-
+						fmt.Println("update: ", currObjRecord)
+						objectsStability[obj.Id] = currObjRecord
 					}
 				} else {
 					fmt.Println("reset: ", currObjRecord)
+					objectsStability[obj.Id] = currObjRecord
 				}
-				objectsStability[obj.Id] = currObjRecord
-				fmt.Println("update: ", currObjRecord)
 			}
 		}
 	}

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

@@ -4,6 +4,22 @@ import (
 	"cicv-data-closedloop/pjisuv_msgs"
 	"fmt"
 	"math"
+	"time"
+)
+
+type Object struct {
+	ID            uint32
+	StableAge     int
+	LastExistTime float64
+}
+
+var objectsStability = make(map[uint32]Object)
+
+const (
+	Cgcs2000X           = 456256.260152
+	Cgcs2000Y           = 4397809.886833
+	MinStableFrameCount = 5
+	MaxTimeBetweenFrame = 0.5
 )
 
 func Topic() string {
@@ -17,56 +33,29 @@ func Label() string {
 
 // speedCheck 目标物速度检测
 func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	var targetMinSpeed float32 = 2 / 3.6  // m/s
-	var targetMaxSpeed float32 = 20 / 3.6 // m/s
+	const targetMinSpeed = 2 / 3.6  // m/s
+	const targetMaxSpeed = 20 / 3.6 // m/s
 
-	if targetMinSpeed < obj.Speed && obj.Speed < targetMaxSpeed {
-		return true
-	}
-	return false
+	return targetMinSpeed < obj.Speed && obj.Speed < targetMaxSpeed
 }
 
 // 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 {
-	var targetType uint8 = 3
+	const targetType uint8 = 3
 
-	if targetType == obj.Type {
-		return true
-	}
-	return false
-}
-
-// sizeCheck 目标物大小检测
-func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	// 多功能车
-	var targetMinLength float32 = 3.6   // m
-	var targetMinWidth float32 = 1.605  // m
-	var targetMinHeight float32 = 1.995 // m
-
-	// 金龙车
-	//targetMinLength := 5.99
-	//targetMinWidth := 2.065
-	//targetMinHeight := 2.82
-
-	if obj.Length > targetMinLength || obj.Width > targetMinWidth || obj.Height > targetMinHeight {
-		return true
-	}
-	return false
+	return targetType == obj.Type
 }
 
 // posCheck 判断目标物位置关系
 func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	laneWidth := 3.5 // m
+	const laneWidth = 3.5 // m
 
-	if obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5 {
-		return true
-	}
-	return false
+	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 {
@@ -74,9 +63,50 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 		}
 	}()
 
+	fmt.Println()
+	fmt.Println(time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 	for _, obj := range msg.Objs {
-		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj)) {
-			return "LowSpdTruckAhead"
+		//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) {
+
+			// 目标物初见存档
+			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 <= 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
+					}
+				} else {
+					//fmt.Println("reset: ", currObjRecord)
+					objectsStability[obj.Id] = currObjRecord
+				}
+			}
 		}
 	}
 	return ""

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

@@ -0,0 +1,121 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"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"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "UnknownBigTargetAhead"
+}
+
+// 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
+
+	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
+
+	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)
+		}
+	}()
+
+	for _, obj := range msg.Objs {
+
+		// 判断目标物是否满足筛选条件
+		if typeCheck(&obj) && posCheck(&obj) && sizeCheck(&obj) {
+
+			// 目标物初见存档
+			if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
+				objectsStability[obj.Id] = Object{
+					ID:               obj.Id,
+					StableFrame:      1,
+					LastExistingTime: msg.Header.TimeStamp,
+				}
+			} else {
+				currObjRecord := Object{
+					ID:               obj.Id,
+					StableFrame:      1,
+					LastExistingTime: msg.Header.TimeStamp,
+				}
+
+				// 目标物确认/更新/重置
+				if currObjRecord.LastExistingTime-prevObjRecord.LastExistingTime <= MaxTimeBetweenFrame { // 目标物消失不超过0.5s
+					currObjRecord.StableFrame = prevObjRecord.StableFrame + 1
+					if currObjRecord.StableFrame == MinStableFrameCount {
+						delete(objectsStability, obj.Id)
+
+						return "UnknownBigTargetAhead"
+					} else {
+						objectsStability[obj.Id] = currObjRecord
+					}
+				} else {
+					objectsStability[obj.Id] = currObjRecord
+				}
+			}
+		}
+	}
+
+	return ""
+}