|
|
@@ -8,7 +8,6 @@ import (
|
|
|
commonEntity "cicv-data-closedloop/common/entity"
|
|
|
"cicv-data-closedloop/common/util"
|
|
|
"cicv-data-closedloop/pjisuv_msgs"
|
|
|
- "cicv-data-closedloop/pjisuv_param"
|
|
|
"cicv-data-closedloop/pjisuv_ticker"
|
|
|
"github.com/bluenviron/goroslib/v2"
|
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
|
|
|
@@ -21,34 +20,25 @@ import (
|
|
|
"time"
|
|
|
)
|
|
|
|
|
|
-// 所有共享变量
|
|
|
var (
|
|
|
- tickChan = make(chan string)
|
|
|
-
|
|
|
- pjisuvParam = pjisuv_param.PjisuvParam{
|
|
|
- ObjDicOfTpperception: make(map[uint32][]float32),
|
|
|
- ObjTypeDicOfTpperception: make(map[uint32]uint8),
|
|
|
- ObjSpeedDicOfTpperception: make(map[uint32]float64),
|
|
|
- }
|
|
|
-
|
|
|
- shareVars = new(sync.Map)
|
|
|
-
|
|
|
- // 保存时间窗口需要锁，防止数据竟态
|
|
|
- saveTimeWindowMutex sync.Mutex
|
|
|
- // /cicv_location
|
|
|
- mutexOfCicvLocation sync.RWMutex
|
|
|
+ // -----------------------------共享变量
|
|
|
// /tpperception
|
|
|
- mutexOfTpperception sync.RWMutex
|
|
|
- // /pj_control_pub
|
|
|
- mutexOfPjControlPub sync.RWMutex
|
|
|
+ objDicOfTpperception = make(map[uint32][]float32)
|
|
|
+ objTypeDicOfTpperception = make(map[uint32]uint8)
|
|
|
+ objSpeedDicOfTpperception = make(map[uint32]float64)
|
|
|
+ // /pji_control_pub
|
|
|
+ numCountPjiControlCommandOfPjControlPub int
|
|
|
+ egoSteeringCmdOfPjControlPub []float64
|
|
|
+ egoThrottleCmdOfPjControlPub []float64
|
|
|
// /data_read
|
|
|
- mutexOfDataRead sync.RWMutex
|
|
|
- // /pj_vehicle_fdb_pub
|
|
|
- mutexOfCicvAmrTrajectory sync.RWMutex
|
|
|
-
|
|
|
+ numCountDataReadOfDataRead int
|
|
|
+ egoSteeringRealOfDataRead []float64
|
|
|
+ egoThrottleRealOfDataRead []float64
|
|
|
+ // --------------------------------------------------
|
|
|
+ shareVars = new(sync.Map)
|
|
|
+ saveTimeWindowMutex sync.Mutex // 保存时间窗口需要锁，防止数据竟态
|
|
|
latestTimeWindowEnd = util.GetTimeCustom(time.Now())
|
|
|
- // 每个触发器5秒触发一次
|
|
|
- triggerInterval = 3.0
|
|
|
+ triggerInterval = 3.0 // 每个触发器3秒触发一次
|
|
|
)
|
|
|
|
|
|
// 负责监听所有主题并修改时间窗口
|
|
|
@@ -109,10 +99,9 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfAmrPose2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfAmrPose2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
+ if len(masterConfig.RuleOfAmrPose3) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfAmrPose3 {
|
|
|
+ faultLabel = f(shareVars, data)
|
|
|
if faultLabel != "" {
|
|
|
saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
subscribersTimes[i] = time.Now()
|
|
|
@@ -120,7 +109,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
TriggerSuccess:
|
|
|
subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
@@ -154,17 +142,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfBoundingBoxesFast2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfBoundingBoxesFast2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfBoundingBoxesFast3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfBoundingBoxesFast3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -175,7 +152,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
TriggerSuccess:
|
|
|
subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
@@ -209,17 +185,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCameraFault2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCameraFault2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCameraFault3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCameraFault3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -264,17 +229,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCanData2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCanData2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCanData3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCanData3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -319,17 +273,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCh128x1LslidarPointCloud2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCh128x1LslidarPointCloud3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -374,17 +317,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCh64wLLslidarPointCloud2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCh64wLLslidarPointCloud3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -429,17 +361,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCh64wLScan2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wLScan2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCh64wLScan3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCh64wLScan3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -484,17 +405,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCh64wRLslidarPointCloud2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCh64wRLslidarPointCloud3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -539,17 +449,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCh64wRScan2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wRScan2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCh64wRScan3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCh64wRScan3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -593,17 +492,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCicvLidarclusterMovingObjects2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCicvLidarclusterMovingObjects3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -645,17 +533,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCicvAmrTrajectory2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCicvAmrTrajectory2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCicvAmrTrajectory3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCicvAmrTrajectory3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -670,18 +547,13 @@ func ProduceWindow() {
|
|
|
subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
subscribersTimeMutexes[i].Unlock()
|
|
|
-
|
|
|
- // 触发后更新共享变量
|
|
|
- mutexOfCicvAmrTrajectory.RLock()
|
|
|
- {
|
|
|
- var currentCurvateres []float64
|
|
|
- for _, point := range data.Trajectoryinfo.Trajectorypoints {
|
|
|
- currentCurvateres = append(currentCurvateres, math.Abs(float64(point.Curvature)))
|
|
|
- }
|
|
|
- pjisuvParam.LastCurvaturesOfCicvAmrTrajectory = currentCurvateres
|
|
|
- pjisuvParam.DecisionType = data.Trajectoryinfo.DecisionType
|
|
|
+ // 更新共享变量
|
|
|
+ currentCurvateres := make([]float64, 0)
|
|
|
+ for _, point := range data.Trajectoryinfo.Trajectorypoints {
|
|
|
+ currentCurvateres = append(currentCurvateres, math.Abs(float64(point.Curvature)))
|
|
|
}
|
|
|
- mutexOfCicvAmrTrajectory.RUnlock()
|
|
|
+ shareVars.Store("LastCurvaturesOfCicvAmrTrajectory", currentCurvateres)
|
|
|
+ shareVars.Store("DecisionType", data.Trajectoryinfo.DecisionType)
|
|
|
},
|
|
|
})
|
|
|
}
|
|
|
@@ -709,17 +581,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCicvLocation2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCicvLocation2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCicvLocation3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCicvLocation3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -773,18 +634,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCloudClusters2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCloudClusters2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCloudClusters3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCloudClusters3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -829,17 +678,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfHeartbeatInfo2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfHeartbeatInfo2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfHeartbeatInfo3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfHeartbeatInfo3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -885,18 +723,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfLidarPretreatmentCost2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarPretreatmentCost2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfLidarPretreatmentCost3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfLidarPretreatmentCost3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -942,18 +768,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfLidarPretreatmentOdometry3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -999,18 +813,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfLidarRoi2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarRoi2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfLidarRoi3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfLidarRoi3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1055,18 +857,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfLine12) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLine12 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfLine13) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfLine13 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1111,18 +901,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfLine22) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLine22 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfLine23) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfLine23 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1168,18 +946,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfMapPolygon2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfMapPolygon2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfMapPolygon3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfMapPolygon3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1224,18 +990,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfObstacleDisplay2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfObstacleDisplay2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfObstacleDisplay3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfObstacleDisplay3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1278,18 +1032,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfPjControlPub2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjControlPub2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfPjControlPub3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfPjControlPub3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1305,16 +1047,15 @@ func ProduceWindow() {
|
|
|
}
|
|
|
subscribersTimeMutexes[i].Unlock()
|
|
|
// 更新共享变量
|
|
|
- mutexOfPjControlPub.RLock()
|
|
|
- {
|
|
|
- pjisuvParam.NumCountPjiControlCommandOfPjControlPub++
|
|
|
- if pjisuvParam.NumCountPjiControlCommandOfPjControlPub == 10 {
|
|
|
- pjisuvParam.EgoSteeringCmdOfPjControlPub = append(pjisuvParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
|
|
|
- pjisuvParam.EgoThrottleCmdOfPjControlPub = append(pjisuvParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
|
|
|
- pjisuvParam.NumCountPjiControlCommandOfPjControlPub = 0
|
|
|
- }
|
|
|
+ numCountPjiControlCommandOfPjControlPub++
|
|
|
+ if numCountPjiControlCommandOfPjControlPub == 10 {
|
|
|
+ egoSteeringCmdOfPjControlPub = append(egoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
|
|
|
+ egoThrottleCmdOfPjControlPub = append(egoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
|
|
|
+ numCountPjiControlCommandOfPjControlPub = 0
|
|
|
}
|
|
|
- mutexOfPjControlPub.RUnlock()
|
|
|
+ shareVars.Store("NumCountPjiControlCommandOfPjControlPub", numCountPjiControlCommandOfPjControlPub)
|
|
|
+ shareVars.Store("EgoSteeringCmdOfPjControlPub", egoSteeringCmdOfPjControlPub)
|
|
|
+ shareVars.Store("EgoThrottleCmdOfPjControlPub", egoThrottleCmdOfPjControlPub)
|
|
|
},
|
|
|
})
|
|
|
}
|
|
|
@@ -1345,18 +1086,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfPointsCluster2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPointsCluster2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfPointsCluster3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfPointsCluster3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1401,18 +1130,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfPointsConcat2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPointsConcat2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfPointsConcat3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfPointsConcat3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1457,18 +1174,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfReferenceDisplay2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfReferenceDisplay2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfReferenceDisplay3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfReferenceDisplay3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1513,18 +1218,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfReferenceTrajectory2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfReferenceTrajectory2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfReferenceTrajectory3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfReferenceTrajectory3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1569,18 +1262,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfRoiPoints2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfRoiPoints2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfRoiPoints3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfRoiPoints3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1625,18 +1306,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfRoiPolygon2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfRoiPolygon2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfRoiPolygon3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfRoiPolygon3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1681,18 +1350,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfTf2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTf2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfTf3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfTf3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1735,18 +1392,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfTpperception2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTpperception2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfTpperception3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfTpperception3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1761,26 +1406,22 @@ func ProduceWindow() {
|
|
|
subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
subscribersTimeMutexes[i].Unlock()
|
|
|
+
|
|
|
// 更新共享变量
|
|
|
- mutexOfTpperception.RLock()
|
|
|
- {
|
|
|
- for _, obj := range data.Objs {
|
|
|
- if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
|
|
|
- continue
|
|
|
- }
|
|
|
- // 检查 ObjDicOfTpperception 是否为 nil，如果是，则初始化它
|
|
|
- if pjisuvParam.ObjDicOfTpperception == nil {
|
|
|
- pjisuvParam.ObjDicOfTpperception = make(map[uint32][]float32)
|
|
|
- }
|
|
|
- if _, ok := pjisuvParam.ObjDicOfTpperception[obj.Id]; !ok {
|
|
|
- pjisuvParam.ObjDicOfTpperception[obj.Id] = []float32{}
|
|
|
- }
|
|
|
- pjisuvParam.ObjDicOfTpperception[obj.Id] = append(pjisuvParam.ObjDicOfTpperception[obj.Id], obj.Y)
|
|
|
- pjisuvParam.ObjTypeDicOfTpperception[obj.Id] = obj.Type
|
|
|
- pjisuvParam.ObjSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
|
|
|
+ for _, obj := range data.Objs {
|
|
|
+ if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
|
|
|
+ continue
|
|
|
+ }
|
|
|
+ if _, ok := objDicOfTpperception[obj.Id]; !ok {
|
|
|
+ objDicOfTpperception[obj.Id] = []float32{}
|
|
|
}
|
|
|
+ objDicOfTpperception[obj.Id] = append(objDicOfTpperception[obj.Id], obj.Y)
|
|
|
+ objTypeDicOfTpperception[obj.Id] = obj.Type
|
|
|
+ objSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
|
|
|
}
|
|
|
- mutexOfTpperception.RUnlock()
|
|
|
+ shareVars.Store("ObjDicOfTpperception", objDicOfTpperception)
|
|
|
+ shareVars.Store("ObjTypeDicOfTpperception", objTypeDicOfTpperception)
|
|
|
+ shareVars.Store("ObjSpeedDicOfTpperception", objSpeedDicOfTpperception)
|
|
|
},
|
|
|
})
|
|
|
}
|
|
|
@@ -1811,18 +1452,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfTpperceptionVis2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTpperceptionVis2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfTpperceptionVis3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfTpperceptionVis3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1867,18 +1496,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfTprouteplan2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTprouteplan2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfTprouteplan3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfTprouteplan3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1923,18 +1540,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfTrajectoryDisplay2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTrajectoryDisplay2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfTrajectoryDisplay3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfTrajectoryDisplay3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -1979,18 +1584,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfUngroundCloudpoints2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfUngroundCloudpoints2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfUngroundCloudpoints3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfUngroundCloudpoints3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -2036,18 +1629,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfCameraImage2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCameraImage2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfCameraImage3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfCameraImage3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -2072,18 +1653,6 @@ func ProduceWindow() {
|
|
|
Node: commonConfig.RosNode,
|
|
|
Topic: topic,
|
|
|
Callback: func(data *pjisuv_msgs.Retrieval) {
|
|
|
- // 更新共享变量
|
|
|
- mutexOfDataRead.RLock()
|
|
|
- {
|
|
|
- pjisuvParam.NumCountDataReadOfDataRead++
|
|
|
- if pjisuvParam.NumCountDataReadOfDataRead == 10 {
|
|
|
- pjisuvParam.EgoSteeringRealOfDataRead = append(pjisuvParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
|
|
|
- pjisuvParam.EgoThrottleRealOfDataRead = append(pjisuvParam.EgoThrottleRealOfDataRead, data.AccPed2)
|
|
|
- pjisuvParam.NumCountDataReadOfDataRead = 0
|
|
|
- }
|
|
|
- pjisuvParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
|
|
|
- }
|
|
|
- mutexOfDataRead.RUnlock()
|
|
|
subscribersTimeMutexes[i].Lock()
|
|
|
if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
subscribersMutexes[i].Lock()
|
|
|
@@ -2101,18 +1670,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfDataRead2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfDataRead2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfDataRead3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfDataRead3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -2127,6 +1684,17 @@ func ProduceWindow() {
|
|
|
subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
subscribersTimeMutexes[i].Unlock()
|
|
|
+ // 更新共享变量
|
|
|
+ numCountDataReadOfDataRead++
|
|
|
+ if numCountDataReadOfDataRead == 10 {
|
|
|
+ egoSteeringRealOfDataRead = append(egoSteeringRealOfDataRead, data.ActStrWhAng)
|
|
|
+ egoThrottleRealOfDataRead = append(egoThrottleRealOfDataRead, data.AccPed2)
|
|
|
+ numCountDataReadOfDataRead = 0
|
|
|
+ }
|
|
|
+ shareVars.Store("NumCountDataReadOfDataRead", numCountDataReadOfDataRead)
|
|
|
+ shareVars.Store("EgoSteeringRealOfDataRead", egoSteeringRealOfDataRead)
|
|
|
+ shareVars.Store("EgoThrottleRealOfDataRead", egoThrottleRealOfDataRead)
|
|
|
+ shareVars.Store("ActStrWhAngOfDataRead", data.ActStrWhAng)
|
|
|
},
|
|
|
})
|
|
|
}
|
|
|
@@ -2156,18 +1724,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfPjiGps2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjiGps2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfPjiGps3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfPjiGps3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -2209,18 +1765,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfPjVehicleFdbPub2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjVehicleFdbPub2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfPjVehicleFdbPub3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfPjVehicleFdbPub3 {
|
|
|
faultLabel = f(shareVars, data)
|
|
|
@@ -2264,18 +1808,6 @@ func ProduceWindow() {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfEndPointMessage2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfEndPointMessage2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
if len(masterConfig.RuleOfEndPointMessage3) > 0 {
|
|
|
for _, f := range masterConfig.RuleOfEndPointMessage3 {
|
|
|
faultLabel = f(shareVars, data)
|
