package main 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 { return "/tpperception" } // Label todo 禁止存在下划线_ func Label() string { return "LowSpdTruckAhead" } // speedCheck 目标物速度检测 func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool { const targetMinSpeed = 2 / 3.6 // m/s const targetMaxSpeed = 20 / 3.6 // m/s 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 { const targetType uint8 = 3 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) } }() 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) { // 目标物初见存档 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 "" }