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

package main

import (
	"cicv-data-closedloop/pjisuv_msgs"
	"fmt"
	"math"
)

func Topic() string {
	return "/tpperception"
}

// Label todo 禁止存在下划线_
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
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(fmt.Sprintf("Recovered from panic: [%s], [%#v]", Label(), r))
		}
	}()

	const MinStableFrameCount = 5   // frame
	const MaxTimeBetweenFrame = 0.5 // second

	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 Label()
					}
				}
				objectsStability[obj.Id] = currObjRecord
			}
		}
	}

	return ""
}