cicv_data_closed_loop/test/ey_common_test.go

package test

import (
	"cicv-data-closedloop/pjisuv_msgs"
	"fmt"
	"github.com/bluenviron/goroslib/v2"
	"log"
	"math"
	"sync"
	"testing"
	"time"
)

var shareVars = new(sync.Map)

func UpdateShareVariable(msg *pjisuv_msgs.PerceptionLocalization) {
	shareVars.Store("OutsideWorkshopFlag", true)

	shareVars.Store("VelocityXOfCicvLocation", msg.VelocityX)
	shareVars.Store("VelocityYOfCicvLocation", msg.VelocityY)
}

func TestGoRosLib(t *testing.T) {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println(fmt.Sprintf("Recovered from panic: [%s], [%#v]", Label(), r))
		}
	}()

	rosNode, err := goroslib.NewNode(goroslib.NodeConf{
		Name:          "eyTest",
		MasterAddress: "localhost:11311",
	})
	if err != nil {
		log.Panicln(err, fmt.Sprintf("failed to create rosNode: [%#v]", err), false)
	}

	_, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  rosNode,
		Topic: "/tpperception",
		Callback: func(msg *pjisuv_msgs.PerceptionObjects) {
			Rule(shareVars, msg)
		},
	})
	if err != nil {
		log.Panicln(err, fmt.Sprintf("failed to create subscriber: [%#v]", err), false)
	}

	_, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  rosNode,
		Topic: "/cicv_location",
		Callback: func(msg *pjisuv_msgs.PerceptionLocalization) {
			UpdateShareVariable(msg)
		},
	})
	if err != nil {
		log.Panicln(err, fmt.Sprintf("failed to create subscriber: [%#v]", err), false)
	}

	select {}
}

// ---------------------------------------------------------------------------------------------------------------------

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

// Label todo 禁止存在下划线_
// MultiPedestrianAhead
// MultiCarAhead
// MultiTruckAhead
// MultiBicycleAhead
// MultiTricycleAhead
// MultiTrafficConeAhead
func Label() string {
	return "MultiTrafficConeAhead"
}

type Record struct {
	StableFrame      int
	LastExistingTime float64
}

var record *Record = nil

// objTypeCheck 目标物类型检测
// × 金龙车：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 objTypeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
	const targetType uint8 = 4

	return targetType == obj.Type
}

// 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() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()

	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
	const minTargetNum = 3

	if len(msg.Objs) >= minTargetNum {

		currTargetNum := 0
		fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
		for _, obj := range msg.Objs {
			// 判断目标物是否满足筛选条件
			if objTypeCheck(&obj) && objPosCheck(&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))
				currTargetNum++
			}
		}

		if currTargetNum >= minTargetNum {
			if nil == record {
				record = &Record{
					StableFrame:      1,
					LastExistingTime: msg.Header.TimeStamp,
				}
				fmt.Println("---------- create ----------")
			} else {
				newRecord := Record{
					StableFrame:      1,
					LastExistingTime: msg.Header.TimeStamp,
				}

				if newRecord.LastExistingTime-record.LastExistingTime <= MaxTimeBetweenFrame {
					newRecord.StableFrame = record.StableFrame + 1
					if newRecord.StableFrame == MinStableFrameCount {
						record = nil
						fmt.Println("!!!!!!!!!! found !!!!!!!!!!")
						return Label()
					} else {
						fmt.Println("---------- update ----------")
					}
				} else {
					fmt.Println("---------- reset ----------")
				}
				record = &newRecord
			}
		}

	}

	return ""
}