|
@@ -2,9 +2,9 @@ package main
|
|
|
|
|
|
import (
|
|
import (
|
|
"cicv-data-closedloop/pjisuv_msgs"
|
|
"cicv-data-closedloop/pjisuv_msgs"
|
|
- "cicv-data-closedloop/pjisuv_param"
|
|
|
|
"fmt"
|
|
"fmt"
|
|
"math"
|
|
"math"
|
|
|
|
+ "sync"
|
|
)
|
|
)
|
|
|
|
|
|
/*
|
|
/*
|
|
@@ -26,27 +26,32 @@ func Topic() string {
|
|
return "/tpperception"
|
|
return "/tpperception"
|
|
}
|
|
}
|
|
|
|
|
|
-// Label todo 禁止存在下划线_
|
|
|
|
|
|
+// 禁止存在下划线_
|
|
func Label() string {
|
|
func Label() string {
|
|
return "PerceptionSpeedDiffer"
|
|
return "PerceptionSpeedDiffer"
|
|
}
|
|
}
|
|
|
|
|
|
-func Rule(data *pjisuv_msgs.PerceptionObjects, param *pjisuv_param.PjisuvParam) string {
|
|
|
|
|
|
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string {
|
|
defer func() {
|
|
defer func() {
|
|
if r := recover(); r != nil {
|
|
if r := recover(); r != nil {
|
|
fmt.Println("Recovered from panic:", r)
|
|
fmt.Println("Recovered from panic:", r)
|
|
}
|
|
}
|
|
}()
|
|
}()
|
|
- for _, obj := range data.Objs {
|
|
|
|
- if len(param.ObjSpeedDicOfTpperception) != 0 {
|
|
|
|
- if _, exist := param.ObjSpeedDicOfTpperception[obj.Id]; exist {
|
|
|
|
- objSpeed := math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
|
|
|
|
- diffRate := math.Abs(objSpeed-param.ObjSpeedDicOfTpperception[obj.Id]) / (param.ObjSpeedDicOfTpperception[obj.Id] + 0.0001)
|
|
|
|
- if diffRate > 0.1 && objSpeed > 0.5 && param.ObjSpeedDicOfTpperception[obj.Id] > 0.5 {
|
|
|
|
- return "PerceptionSpeedDiffer"
|
|
|
|
|
|
+ value, ok := shareVars.Load("ObjSpeedDicOfTpperception")
|
|
|
|
+ if ok {
|
|
|
|
+ objSpeedDicOfTpperception := value.(map[uint32]float64)
|
|
|
|
+ for _, obj := range data.Objs {
|
|
|
|
+ if len(objSpeedDicOfTpperception) != 0 {
|
|
|
|
+ if _, exist := objSpeedDicOfTpperception[obj.Id]; exist {
|
|
|
|
+ objSpeed := math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
|
|
|
|
+ diffRate := math.Abs(objSpeed-objSpeedDicOfTpperception[obj.Id]) / (objSpeedDicOfTpperception[obj.Id] + 0.0001)
|
|
|
|
+ if diffRate > 0.1 && objSpeed > 0.5 && objSpeedDicOfTpperception[obj.Id] > 0.5 {
|
|
|
|
+ return Label()
|
|
|
|
+ }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
+
|
|
return ""
|
|
return ""
|
|
}
|
|
}
|