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

package main

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

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

// Label todo 禁止存在下划线_
func Label() string {
	return "LowSpdTruckAhead"
}

// 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
}

// 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

	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
}

// posCheck 判断目标物位置关系
func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
	laneWidth := 3.5 // m

	if obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5 {
		return true
	}
	return false
}

// 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 {

		// todo: 对各判断条件单独进行稳定性校验？
		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj) || sizeCheck(&obj)) {
			return "LowSpdTruckAhead"
		}
	}
	return ""
}