|
|
@@ -6,60 +6,137 @@ import (
|
|
|
"github.com/bluenviron/goroslib/v2"
|
|
|
"log"
|
|
|
"math"
|
|
|
+ "sync"
|
|
|
"testing"
|
|
|
"time"
|
|
|
)
|
|
|
|
|
|
-type Record struct {
|
|
|
- StableFrame int
|
|
|
- LastExistingTime float64
|
|
|
+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)
|
|
|
}
|
|
|
|
|
|
-var record *Record = nil
|
|
|
+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 "test"
|
|
|
+ return "MultiTrafficConeAhead"
|
|
|
+}
|
|
|
+
|
|
|
+type Record struct {
|
|
|
+ StableFrame int
|
|
|
+ LastExistingTime float64
|
|
|
}
|
|
|
|
|
|
-// typeCheck 目标物类型检测
|
|
|
+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 typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
|
|
|
+func objTypeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
|
|
|
const targetType uint8 = 4
|
|
|
|
|
|
return targetType == obj.Type
|
|
|
}
|
|
|
|
|
|
-// posCheck 判断目标物位置关系
|
|
|
-func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
|
|
|
+// 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(msg *pjisuv_msgs.PerceptionObjects) string {
|
|
|
+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 = 4
|
|
|
+ const minTargetNum = 3
|
|
|
|
|
|
- fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
|
|
|
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 typeCheck(&obj) && posCheck(&obj) {
|
|
|
+ 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++
|
|
|
@@ -83,7 +160,7 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
|
|
|
newRecord.StableFrame = record.StableFrame + 1
|
|
|
if newRecord.StableFrame == MinStableFrameCount {
|
|
|
record = nil
|
|
|
- fmt.Println("!!!!!!!!!! found !!!!!!!!!!") // todo
|
|
|
+ fmt.Println("!!!!!!!!!! found !!!!!!!!!!")
|
|
|
return Label()
|
|
|
} else {
|
|
|
fmt.Println("---------- update ----------")
|
|
|
@@ -99,32 +176,3 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
|
|
|
|
|
|
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 {}
|
|
|
-}
|
Yishi Wang