cicv_data_closed_loop/test/goroslib_test.go

package test

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

type Object struct {
	ID            uint32
	StableAge     int
	LastExistTime float64
}

var objectsStability = make(map[uint32]Object)

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

	CGCS2000_X := 456256.260152
	CGCS2000_Y := 4397809.886833
	existingFrameThreshold := 5  // frame
	existingTimeThreshold := 0.5 // s

	//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))
	//
	//	// todo: 对各判断条件单独进行稳定性校验？
	//	if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&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-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
	//		//fmt.Println("!!!")
	//		return "LowSpdTruckAhead"
	//	}
	//}
	//return ""

	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) || sizeCheck(&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 <= existingTimeThreshold { // 目标物消失不超过0.5s
					currObjRecord.StableAge = prevObjRecord.StableAge + 1
					if existingFrameThreshold == currObjRecord.StableAge {
						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-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
						delete(objectsStability, obj.Id)

						continue // todo
						//return "LowSpdTruckAhead"
					} else {

					}
				} else {
					fmt.Println("reset: ", currObjRecord)
				}
				objectsStability[obj.Id] = currObjRecord
				fmt.Println("update: ", currObjRecord)
			}
		}
	}
	return ""
}

func TestGoRosLib(t *testing.T) {
	defer func() {
		if err := recover(); err != nil {
			log.Println(err, fmt.Sprintf("recover: [%#v]", err), false)
		}
	}()

	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(msg)
		},
	})
	if err != nil {
		log.Panicln(err, fmt.Sprintf("failed to create subscriber: [%#v]", err), false)
	}

	select {}
}