update trigger UnknownBigTargetAhead: add ego vehicle min speed check;

trigger/pjisuv/tpperception/UnknownBigTargetAhead/main/UnknownBigTargetAhead.go

@@ -60,13 +60,24 @@ func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	return counter > 1
 }
 
-// posCheck 判断目标物位置关系
-func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+// objPosCheck 判断目标物位置关系
+func objPosCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const laneWidth = 3.5 // m
 
 	return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
 }
 
+// isEgoStationary 判断主车是否“静止”（速度低于阈值）
+func isEgoStationary(shareVars *sync.Map) bool {
+	const minSpeed = 1 // m/s
+
+	VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation") // float64
+	VelocityYOfCicvLocation, _ := shareVars.Load("VelocityYOfCicvLocation")
+	speed := math.Sqrt(math.Pow(VelocityXOfCicvLocation.(float64), 2) + math.Pow(VelocityYOfCicvLocation.(float64), 2))
+
+	return speed > minSpeed
+}
+
 // Rule 检测前方未知大目标物
 func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
@@ -75,14 +86,24 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 		}
 	}()
 
+	// 判断主车所在位置 & 行驶速度
+	outsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	if !outsideWorkshopFlag.(bool) || !isEgoStationary(shareVars) {
+		return ""
+	}
+
+	//const Cgcs2000X = 456256.260152
+	//const Cgcs2000Y = 4397809.886833
 	const MinStableFrameCount = 5   // frame
 	const MaxTimeBetweenFrame = 0.5 // second
-	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
-	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+
+	//fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 	for _, obj := range msg.Objs {
 
 		// 判断目标物是否满足筛选条件
-		if typeCheck(&obj) && posCheck(&obj) && sizeCheck(&obj) {
+		if typeCheck(&obj) && objPosCheck(&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-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
 
 			// 目标物初见存档
 			if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
@@ -91,6 +112,7 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 					StableFrame:      1,
 					LastExistingTime: msg.Header.TimeStamp,
 				}
+				//fmt.Println("---------- create ----------")
 			} else {
 				currObjRecord := Object{
 					ID:               obj.Id,
@@ -101,10 +123,15 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 				// 目标物确认/更新/重置
 				if currObjRecord.LastExistingTime-prevObjRecord.LastExistingTime <= MaxTimeBetweenFrame { // 目标物消失不超过0.5s
 					currObjRecord.StableFrame = prevObjRecord.StableFrame + 1
-					if currObjRecord.StableFrame == MinStableFrameCount && OutsideWorkshopFlag == true {
+					if currObjRecord.StableFrame == MinStableFrameCount {
 						delete(objectsStability, obj.Id)
+						//fmt.Println("!!!!!!!!!! found !!!!!!!!!!")
 						return Label()
+					} else {
+						//fmt.Println("---------- update ----------")
 					}
+				} else {
+					//fmt.Println("---------- reset ----------")
 				}
 				objectsStability[obj.Id] = currObjRecord
 			}