|
|
@@ -22,7 +22,7 @@ import (
|
|
|
|
|
|
// 所有共享变量
|
|
|
var (
|
|
|
- latestTimeWindowEnd = util.GetTimeString(time.Now())
|
|
|
+ latestTimeWindowEnd = util.GetTimeCustom(time.Now())
|
|
|
pjisuvParam = pjisuv_param.PjisuvParam{
|
|
|
ObjDicOfTpperception: make(map[uint32][]float32),
|
|
|
ObjTypeDicOfTpperception: make(map[uint32]uint8),
|
|
|
@@ -56,1748 +56,1752 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
subscribersTimeMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
|
|
|
subscribersMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
|
|
|
for i, topic := range commonConfig.SubscribeTopics {
|
|
|
- // 增加了可扩展性
|
|
|
- if topic == masterConfig.TopicOfCicvExtend {
|
|
|
- go func() {
|
|
|
- for {
|
|
|
- time.Sleep(time.Duration(3500) * time.Millisecond)
|
|
|
- for _, f := range masterConfig.RuleOfCicvExtend {
|
|
|
- label := f(pjisuvParam)
|
|
|
- if label != "" {
|
|
|
- saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow())
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- break
|
|
|
+ for {
|
|
|
+ // 增加了可扩展性
|
|
|
+ if topic == masterConfig.TopicOfCicvExtend {
|
|
|
+ go func() {
|
|
|
+ for {
|
|
|
+ time.Sleep(time.Duration(3500) * time.Millisecond)
|
|
|
+ for _, f := range masterConfig.RuleOfCicvExtend {
|
|
|
+ label := f(pjisuvParam)
|
|
|
+ if label != "" {
|
|
|
+ saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow())
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ break
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
- }()
|
|
|
- }
|
|
|
+ }()
|
|
|
+ }
|
|
|
|
|
|
- // 其他常规监听器
|
|
|
- c_log.GlobalLogger.Info("创建订阅者订阅话题：" + topic)
|
|
|
-
|
|
|
- // 1
|
|
|
- if topic == masterConfig.TopicOfAmrPose && (len(masterConfig.RuleOfAmrPose1) > 0 || len(masterConfig.RuleOfAmrPose2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *visualization_msgs.MarkerArray) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfAmrPose1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfAmrPose1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 其他常规监听器
|
|
|
+ c_log.GlobalLogger.Info("创建订阅者订阅话题：" + topic)
|
|
|
+
|
|
|
+ // 1
|
|
|
+ if topic == masterConfig.TopicOfAmrPose && (len(masterConfig.RuleOfAmrPose1) > 0 || len(masterConfig.RuleOfAmrPose2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *visualization_msgs.MarkerArray) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfAmrPose1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfAmrPose1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if len(masterConfig.RuleOfAmrPose2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfAmrPose2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfAmrPose2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfAmrPose2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 2
|
|
|
+ if topic == masterConfig.TopicOfBoundingBoxesFast && (len(masterConfig.RuleOfBoundingBoxesFast1) > 0 || len(masterConfig.RuleOfBoundingBoxesFast2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.BoundingBoxArray) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfBoundingBoxesFast1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfBoundingBoxesFast1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 3
|
|
|
+ if topic == masterConfig.TopicOfCameraFault && (len(masterConfig.RuleOfCameraFault1) > 0 || len(masterConfig.RuleOfCameraFault2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.FaultVec) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCameraFault1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCameraFault1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- // 2
|
|
|
- if topic == masterConfig.TopicOfBoundingBoxesFast && (len(masterConfig.RuleOfBoundingBoxesFast1) > 0 || len(masterConfig.RuleOfBoundingBoxesFast2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.BoundingBoxArray) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfBoundingBoxesFast1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfBoundingBoxesFast1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 4
|
|
|
+ if topic == masterConfig.TopicOfCanData && (len(masterConfig.RuleOfCanData1) > 0 || len(masterConfig.RuleOfCanData2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.Frame) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCanData1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCanData1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 5
|
|
|
+ if topic == masterConfig.TopicOfCh128x1LslidarPointCloud && (len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh128x1LslidarPointCloud2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 6
|
|
|
+ if topic == masterConfig.TopicOfCh64wLLslidarPointCloud && (len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wLLslidarPointCloud2) > 1) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- // 3
|
|
|
- if topic == masterConfig.TopicOfCameraFault && (len(masterConfig.RuleOfCameraFault1) > 0 || len(masterConfig.RuleOfCameraFault2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.FaultVec) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCameraFault1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCameraFault1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 7
|
|
|
+ if topic == masterConfig.TopicOfCh64wLScan && (len(masterConfig.RuleOfCh64wLScan1) > 0 || len(masterConfig.RuleOfCh64wLScan2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.LaserScan) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCh64wLScan1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCh64wLScan1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 8
|
|
|
+ if topic == masterConfig.TopicOfCh64wRLslidarPointCloud && (len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wRLslidarPointCloud2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 9
|
|
|
+ if topic == masterConfig.TopicOfCh64wRScan && (len(masterConfig.RuleOfCh64wRScan1) > 0 || len(masterConfig.RuleOfCh64wRScan2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.LaserScan) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCh64wRScan1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCh64wRScan1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- // 4
|
|
|
- if topic == masterConfig.TopicOfCanData && (len(masterConfig.RuleOfCanData1) > 0 || len(masterConfig.RuleOfCanData2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.Frame) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCanData1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCanData1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 10
|
|
|
+ if topic == masterConfig.TopicOfCicvLidarclusterMovingObjects && (len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 || len(masterConfig.RuleOfCicvLidarclusterMovingObjects2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.PerceptionCicvMovingObjects) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 11
|
|
|
+ if topic == masterConfig.TopicOfCicvAmrTrajectory && (len(masterConfig.RuleOfCicvAmrTrajectory1) > 0 || len(masterConfig.RuleOfCicvAmrTrajectory2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.Trajectory) {
|
|
|
+
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCicvAmrTrajectory1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCicvAmrTrajectory1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ mutexOfCicvAmrTrajectory.RUnlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 12
|
|
|
+ if topic == masterConfig.TopicOfCicvLocation && (len(masterConfig.RuleOfCicvLocation1) > 0 || len(masterConfig.RuleOfCicvLocation2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
|
|
|
+ // 更新共享变量
|
|
|
+ mutexOfCicvLocation.RLock()
|
|
|
+ {
|
|
|
+ pjisuvParam.VelocityXOfCicvLocation = data.VelocityX
|
|
|
+ pjisuvParam.VelocityYOfCicvLocation = data.VelocityY
|
|
|
+ pjisuvParam.VelocityZOfCicvLocation = data.VelocityZ
|
|
|
+ pjisuvParam.YawOfCicvLocation = data.Yaw
|
|
|
+ pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
|
|
|
+ pjisuvParam.PositionXOfCicvLocation = data.PositionX
|
|
|
+ pjisuvParam.PositionYOfCicvLocation = data.PositionY
|
|
|
+ }
|
|
|
+ mutexOfCicvLocation.RUnlock()
|
|
|
+
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCicvLocation1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCicvLocation1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- // 5
|
|
|
- if topic == masterConfig.TopicOfCh128x1LslidarPointCloud && (len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh128x1LslidarPointCloud2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 13
|
|
|
+ if topic == masterConfig.TopicOfCloudClusters && (len(masterConfig.RuleOfCloudClusters1) > 0 || len(masterConfig.RuleOfCloudClusters2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.AutowareCloudClusterArray) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCloudClusters1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCloudClusters1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfCloudClusters2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCloudClusters2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 6
|
|
|
- if topic == masterConfig.TopicOfCh64wLLslidarPointCloud && (len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wLLslidarPointCloud2) > 1) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 14
|
|
|
+ if topic == masterConfig.TopicOfHeartbeatInfo && (len(masterConfig.RuleOfHeartbeatInfo1) > 0 || len(masterConfig.RuleOfHeartbeatInfo2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.HeartBeatInfo) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfHeartbeatInfo1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfHeartbeatInfo1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- // 7
|
|
|
- if topic == masterConfig.TopicOfCh64wLScan && (len(masterConfig.RuleOfCh64wLScan1) > 0 || len(masterConfig.RuleOfCh64wLScan2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.LaserScan) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCh64wLScan1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wLScan1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 15
|
|
|
+ if topic == masterConfig.TopicOfLidarPretreatmentCost && (len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 || len(masterConfig.RuleOfLidarPretreatmentCost2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *geometry_msgs.Vector3Stamped) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLidarPretreatmentCost1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfLidarPretreatmentCost2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLidarPretreatmentCost2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 8
|
|
|
- if topic == masterConfig.TopicOfCh64wRLslidarPointCloud && (len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wRLslidarPointCloud2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 16
|
|
|
+ if topic == masterConfig.TopicOfLidarPretreatmentOdometry && (len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 || len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *nav_msgs.Odometry) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 9
|
|
|
- if topic == masterConfig.TopicOfCh64wRScan && (len(masterConfig.RuleOfCh64wRScan1) > 0 || len(masterConfig.RuleOfCh64wRScan2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.LaserScan) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCh64wRScan1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCh64wRScan1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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.RuleOfLidarPretreatmentOdometry2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
-
|
|
|
- // 10
|
|
|
- if topic == masterConfig.TopicOfCicvLidarclusterMovingObjects && (len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 || len(masterConfig.RuleOfCicvLidarclusterMovingObjects2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.PerceptionCicvMovingObjects) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 17
|
|
|
+ if topic == masterConfig.TopicOfLidarRoi && (len(masterConfig.RuleOfLidarRoi1) > 0 || len(masterConfig.RuleOfLidarRoi2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *geometry_msgs.PolygonStamped) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfLidarRoi1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLidarRoi1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 11
|
|
|
- if topic == masterConfig.TopicOfCicvAmrTrajectory && (len(masterConfig.RuleOfCicvAmrTrajectory1) > 0 || len(masterConfig.RuleOfCicvAmrTrajectory2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.Trajectory) {
|
|
|
-
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCicvAmrTrajectory1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCicvAmrTrajectory1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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.RuleOfLidarRoi2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLidarRoi2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- 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)))
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
- pjisuvParam.LastCurvaturesOfCicvAmrTrajectory = currentCurvateres
|
|
|
- }
|
|
|
- mutexOfCicvAmrTrajectory.RUnlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- // 12
|
|
|
- if topic == masterConfig.TopicOfCicvLocation && (len(masterConfig.RuleOfCicvLocation1) > 0 || len(masterConfig.RuleOfCicvLocation2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
|
|
|
- // 更新共享变量
|
|
|
- mutexOfCicvLocation.RLock()
|
|
|
- {
|
|
|
- pjisuvParam.VelocityXOfCicvLocation = data.VelocityX
|
|
|
- pjisuvParam.VelocityYOfCicvLocation = data.VelocityY
|
|
|
- pjisuvParam.VelocityZOfCicvLocation = data.VelocityZ
|
|
|
- pjisuvParam.YawOfCicvLocation = data.Yaw
|
|
|
- pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
|
|
|
- pjisuvParam.PositionXOfCicvLocation = data.PositionX
|
|
|
- pjisuvParam.PositionYOfCicvLocation = data.PositionY
|
|
|
- }
|
|
|
- mutexOfCicvLocation.RUnlock()
|
|
|
-
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCicvLocation1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCicvLocation1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 18
|
|
|
+ if topic == masterConfig.TopicOfLine1 && (len(masterConfig.RuleOfLine11) > 0 || len(masterConfig.RuleOfLine12) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *nav_msgs.Path) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfLine11) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLine11 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfLine12) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLine12 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 13
|
|
|
- if topic == masterConfig.TopicOfCloudClusters && (len(masterConfig.RuleOfCloudClusters1) > 0 || len(masterConfig.RuleOfCloudClusters2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.AutowareCloudClusterArray) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCloudClusters1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCloudClusters1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfCloudClusters2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCloudClusters2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 19
|
|
|
+ if topic == masterConfig.TopicOfLine2 && (len(masterConfig.RuleOfLine21) > 0 || len(masterConfig.RuleOfLine22) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *nav_msgs.Path) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfLine21) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLine21 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfLine22) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfLine22 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 14
|
|
|
- if topic == masterConfig.TopicOfHeartbeatInfo && (len(masterConfig.RuleOfHeartbeatInfo1) > 0 || len(masterConfig.RuleOfHeartbeatInfo2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.HeartBeatInfo) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfHeartbeatInfo1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfHeartbeatInfo1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 20
|
|
|
+ if topic == masterConfig.TopicOfMapPolygon && (len(masterConfig.RuleOfMapPolygon1) > 0 || len(masterConfig.RuleOfMapPolygon2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.PolygonStamped) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfMapPolygon1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfMapPolygon1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 15
|
|
|
- if topic == masterConfig.TopicOfLidarPretreatmentCost && (len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 || len(masterConfig.RuleOfLidarPretreatmentCost2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *geometry_msgs.Vector3Stamped) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarPretreatmentCost1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfLidarPretreatmentCost2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarPretreatmentCost2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
|
|
|
- // 16
|
|
|
- if topic == masterConfig.TopicOfLidarPretreatmentOdometry && (len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 || len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *nav_msgs.Odometry) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 21
|
|
|
+ if topic == masterConfig.TopicOfObstacleDisplay && (len(masterConfig.RuleOfObstacleDisplay1) > 0 || len(masterConfig.RuleOfObstacleDisplay2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *visualization_msgs.MarkerArray) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfObstacleDisplay1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfObstacleDisplay1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfObstacleDisplay2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfObstacleDisplay2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 17
|
|
|
- if topic == masterConfig.TopicOfLidarRoi && (len(masterConfig.RuleOfLidarRoi1) > 0 || len(masterConfig.RuleOfLidarRoi2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *geometry_msgs.PolygonStamped) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfLidarRoi1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLidarRoi1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
-
|
|
|
- // 18
|
|
|
- if topic == masterConfig.TopicOfLine1 && (len(masterConfig.RuleOfLine11) > 0 || len(masterConfig.RuleOfLine12) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *nav_msgs.Path) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfLine11) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLine11 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ // 22
|
|
|
+ if topic == masterConfig.TopicOfPjControlPub && (len(masterConfig.RuleOfPjControlPub1) > 0 || len(masterConfig.RuleOfPjControlPub2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
|
|
|
+ // 更新共享变量
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ mutexOfPjControlPub.RUnlock()
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfPjControlPub1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfPjControlPub1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfLine12) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLine12 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 23
|
|
|
+ if topic == masterConfig.TopicOfPointsCluster && (len(masterConfig.RuleOfPointsCluster1) > 0 || len(masterConfig.RuleOfPointsCluster2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfPointsCluster1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfPointsCluster1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfPointsCluster2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfPointsCluster2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 19
|
|
|
- if topic == masterConfig.TopicOfLine2 && (len(masterConfig.RuleOfLine21) > 0 || len(masterConfig.RuleOfLine22) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *nav_msgs.Path) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfLine21) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfLine21 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 24
|
|
|
+ if topic == masterConfig.TopicOfPointsConcat && (len(masterConfig.RuleOfPointsConcat1) > 0 || len(masterConfig.RuleOfPointsConcat2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfPointsConcat1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfPointsConcat1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 20
|
|
|
- if topic == masterConfig.TopicOfMapPolygon && (len(masterConfig.RuleOfMapPolygon1) > 0 || len(masterConfig.RuleOfMapPolygon2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.PolygonStamped) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfMapPolygon1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfMapPolygon1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfMapPolygon2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfMapPolygon2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
|
|
|
- // 21
|
|
|
- if topic == masterConfig.TopicOfObstacleDisplay && (len(masterConfig.RuleOfObstacleDisplay1) > 0 || len(masterConfig.RuleOfObstacleDisplay2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *visualization_msgs.MarkerArray) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfObstacleDisplay1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfObstacleDisplay1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfObstacleDisplay2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfObstacleDisplay2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 25
|
|
|
+ if topic == masterConfig.TopicOfReferenceDisplay && (len(masterConfig.RuleOfReferenceDisplay1) > 0 || len(masterConfig.RuleOfReferenceDisplay2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *nav_msgs.Path) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfReferenceDisplay1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfReferenceDisplay1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfReferenceDisplay2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfReferenceDisplay2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 22
|
|
|
- if topic == masterConfig.TopicOfPjControlPub && (len(masterConfig.RuleOfPjControlPub1) > 0 || len(masterConfig.RuleOfPjControlPub2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
|
|
|
- // 更新共享变量
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
- mutexOfPjControlPub.RUnlock()
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfPjControlPub1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjControlPub1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 26
|
|
|
+ if topic == masterConfig.TopicOfReferenceTrajectory && (len(masterConfig.RuleOfReferenceTrajectory1) > 0 || len(masterConfig.RuleOfReferenceTrajectory2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.Trajectory) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfReferenceTrajectory1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfReferenceTrajectory1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 23
|
|
|
- if topic == masterConfig.TopicOfPointsCluster && (len(masterConfig.RuleOfPointsCluster1) > 0 || len(masterConfig.RuleOfPointsCluster2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfPointsCluster1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPointsCluster1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfPointsCluster2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPointsCluster2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
|
|
|
- // 24
|
|
|
- if topic == masterConfig.TopicOfPointsConcat && (len(masterConfig.RuleOfPointsConcat1) > 0 || len(masterConfig.RuleOfPointsConcat2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfPointsConcat1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPointsConcat1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfPointsConcat2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPointsConcat2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 27
|
|
|
+ if topic == masterConfig.TopicOfRoiPoints && (len(masterConfig.RuleOfRoiPoints1) > 0 || len(masterConfig.RuleOfRoiPoints2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfRoiPoints1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfRoiPoints1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfRoiPoints2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfRoiPoints2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 25
|
|
|
- if topic == masterConfig.TopicOfReferenceDisplay && (len(masterConfig.RuleOfReferenceDisplay1) > 0 || len(masterConfig.RuleOfReferenceDisplay2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *nav_msgs.Path) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfReferenceDisplay1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfReferenceDisplay1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 28
|
|
|
+ if topic == masterConfig.TopicOfRoiPolygon && (len(masterConfig.RuleOfRoiPolygon1) > 0 || len(masterConfig.RuleOfRoiPolygon2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *nav_msgs.Path) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfRoiPolygon1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfRoiPolygon1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 26
|
|
|
- if topic == masterConfig.TopicOfReferenceTrajectory && (len(masterConfig.RuleOfReferenceTrajectory1) > 0 || len(masterConfig.RuleOfReferenceTrajectory2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.Trajectory) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfReferenceTrajectory1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfReferenceTrajectory1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfReferenceTrajectory2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfReferenceTrajectory2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
-
|
|
|
- // 27
|
|
|
- if topic == masterConfig.TopicOfRoiPoints && (len(masterConfig.RuleOfRoiPoints1) > 0 || len(masterConfig.RuleOfRoiPoints2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfRoiPoints1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfRoiPoints1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfRoiPoints2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfRoiPoints2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 29
|
|
|
+ if topic == masterConfig.TopicOfTf && (len(masterConfig.RuleOfTf1) > 0 || len(masterConfig.RuleOfTf2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *tf2_msgs.TFMessage) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfTf1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfTf1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfTf2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfTf2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 28
|
|
|
- if topic == masterConfig.TopicOfRoiPolygon && (len(masterConfig.RuleOfRoiPolygon1) > 0 || len(masterConfig.RuleOfRoiPolygon2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *nav_msgs.Path) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfRoiPolygon1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfRoiPolygon1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
-
|
|
|
- // 29
|
|
|
- if topic == masterConfig.TopicOfTf && (len(masterConfig.RuleOfTf1) > 0 || len(masterConfig.RuleOfTf2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *tf2_msgs.TFMessage) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfTf1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTf1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ // 30
|
|
|
+ if topic == masterConfig.TopicOfTpperception && (len(masterConfig.RuleOfTpperception1) > 0 || len(masterConfig.RuleOfTpperception2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.PerceptionObjects) {
|
|
|
+ // 更新共享变量
|
|
|
+ mutexOfTpperception.RLock()
|
|
|
+ {
|
|
|
+ for _, obj := range data.Objs {
|
|
|
+ if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
|
|
|
+ continue
|
|
|
}
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- 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
|
|
|
+ // 检查 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)
|
|
|
}
|
|
|
}
|
|
|
+ mutexOfTpperception.RUnlock()
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfTpperception1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfTpperception1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
-
|
|
|
- // 30
|
|
|
- if topic == masterConfig.TopicOfTpperception && (len(masterConfig.RuleOfTpperception1) > 0 || len(masterConfig.RuleOfTpperception2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.PerceptionObjects) {
|
|
|
- // 更新共享变量
|
|
|
- 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)
|
|
|
- }
|
|
|
- }
|
|
|
- mutexOfTpperception.RUnlock()
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfTpperception1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTpperception1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfTpperception2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTpperception2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- // -------- 触发后更新共享变量
|
|
|
- mutexOfTpperception.RLock()
|
|
|
- {
|
|
|
- for _, obj := range data.Objs {
|
|
|
- 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)
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
- }
|
|
|
- mutexOfTpperception.RUnlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
-
|
|
|
- // 31
|
|
|
- if topic == masterConfig.TopicOfTpperceptionVis && (len(masterConfig.RuleOfTpperceptionVis1) > 0 || len(masterConfig.RuleOfTpperceptionVis2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *visualization_msgs.MarkerArray) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfTpperceptionVis1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTpperceptionVis1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ // -------- 触发后更新共享变量
|
|
|
+ mutexOfTpperception.RLock()
|
|
|
+ {
|
|
|
+ for _, obj := range data.Objs {
|
|
|
+ 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)
|
|
|
}
|
|
|
}
|
|
|
+ mutexOfTpperception.RUnlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfTpperceptionVis2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTpperceptionVis2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 31
|
|
|
+ if topic == masterConfig.TopicOfTpperceptionVis && (len(masterConfig.RuleOfTpperceptionVis1) > 0 || len(masterConfig.RuleOfTpperceptionVis2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *visualization_msgs.MarkerArray) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfTpperceptionVis1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfTpperceptionVis1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ if len(masterConfig.RuleOfTpperceptionVis2) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfTpperceptionVis2 {
|
|
|
+ faultLabel = f(data, &pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 32
|
|
|
- if topic == masterConfig.TopicOfTprouteplan && (len(masterConfig.RuleOfTprouteplan1) > 0 || len(masterConfig.RuleOfTprouteplan2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.RoutePlan) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfTprouteplan1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTprouteplan1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
- 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
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 32
|
|
|
+ if topic == masterConfig.TopicOfTprouteplan && (len(masterConfig.RuleOfTprouteplan1) > 0 || len(masterConfig.RuleOfTprouteplan2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.RoutePlan) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfTprouteplan1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfTprouteplan1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 33
|
|
|
- if topic == masterConfig.TopicOfTrajectoryDisplay && (len(masterConfig.RuleOfTrajectoryDisplay1) > 0 || len(masterConfig.RuleOfTrajectoryDisplay2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *nav_msgs.Path) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfTrajectoryDisplay1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTrajectoryDisplay1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfTrajectoryDisplay2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfTrajectoryDisplay2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 33
|
|
|
+ if topic == masterConfig.TopicOfTrajectoryDisplay && (len(masterConfig.RuleOfTrajectoryDisplay1) > 0 || len(masterConfig.RuleOfTrajectoryDisplay2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *nav_msgs.Path) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfTrajectoryDisplay1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfTrajectoryDisplay1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 34
|
|
|
- if topic == masterConfig.TopicOfUngroundCloudpoints && (len(masterConfig.RuleOfUngroundCloudpoints1) > 0 || len(masterConfig.RuleOfUngroundCloudpoints2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfUngroundCloudpoints1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfUngroundCloudpoints1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfUngroundCloudpoints2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfUngroundCloudpoints2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 34
|
|
|
+ if topic == masterConfig.TopicOfUngroundCloudpoints && (len(masterConfig.RuleOfUngroundCloudpoints1) > 0 || len(masterConfig.RuleOfUngroundCloudpoints2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.PointCloud2) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfUngroundCloudpoints1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfUngroundCloudpoints1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 35
|
|
|
- if topic == masterConfig.TopicOfCameraImage && (len(masterConfig.RuleOfCameraImage1) > 0 || len(masterConfig.RuleOfCameraImage2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *sensor_msgs.Image) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfCameraImage1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCameraImage1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfCameraImage2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfCameraImage2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
- // 36
|
|
|
- if topic == masterConfig.TopicOfDataRead && (len(masterConfig.RuleOfDataRead1) > 0 || len(masterConfig.RuleOfDataRead2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- 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()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfDataRead1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfDataRead1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- if len(masterConfig.RuleOfDataRead2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfDataRead2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ // 35
|
|
|
+ if topic == masterConfig.TopicOfCameraImage && (len(masterConfig.RuleOfCameraImage1) > 0 || len(masterConfig.RuleOfCameraImage2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *sensor_msgs.Image) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfCameraImage1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfCameraImage1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
- // 37
|
|
|
- if topic == masterConfig.TopicOfPjiGps && (len(masterConfig.RuleOfPjiGps1) > 0 || len(masterConfig.RuleOfPjiGps2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfPjiGps1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjiGps1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if len(masterConfig.RuleOfPjiGps2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjiGps2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
}
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
+ // 36
|
|
|
+ if topic == masterConfig.TopicOfDataRead && (len(masterConfig.RuleOfDataRead1) > 0 || len(masterConfig.RuleOfDataRead2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ 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()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfDataRead1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfDataRead1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
+ // 37
|
|
|
+ if topic == masterConfig.TopicOfPjiGps && (len(masterConfig.RuleOfPjiGps1) > 0 || len(masterConfig.RuleOfPjiGps2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfPjiGps1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfPjiGps1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
+ // 39
|
|
|
+ if topic == masterConfig.TopicOfPjVehicleFdbPub && (len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 || len(masterConfig.RuleOfPjVehicleFdbPub2) > 0) {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.VehicleFdb) {
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ faultLabel := ""
|
|
|
+ if len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 {
|
|
|
+ for _, f := range masterConfig.RuleOfPjVehicleFdbPub1 {
|
|
|
+ faultLabel = f(data)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
|
|
- // 39
|
|
|
- if topic == masterConfig.TopicOfPjVehicleFdbPub && (len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 || len(masterConfig.RuleOfPjVehicleFdbPub2) > 0) {
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
- Node: commonConfig.RosNode,
|
|
|
- Topic: topic,
|
|
|
- Callback: func(data *pjisuv_msgs.VehicleFdb) {
|
|
|
- subscribersTimeMutexes[i].Lock()
|
|
|
- if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
|
|
|
- subscribersMutexes[i].Lock()
|
|
|
- faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
- lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
- faultLabel := ""
|
|
|
- if len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjVehicleFdbPub1 {
|
|
|
- faultLabel = f(data)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
-
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
- goto TriggerSuccess
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- if len(masterConfig.RuleOfPjVehicleFdbPub2) > 0 {
|
|
|
- for _, f := range masterConfig.RuleOfPjVehicleFdbPub2 {
|
|
|
- faultLabel = f(data, &pjisuvParam)
|
|
|
- if faultLabel != "" {
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ 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
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ goto TriggerSuccess
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
- TriggerSuccess:
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
- }
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
- },
|
|
|
- })
|
|
|
- }
|
|
|
|
|
|
- if err != nil {
|
|
|
- c_log.GlobalLogger.Info("创建订阅者报错：", err)
|
|
|
- continue
|
|
|
+ TriggerSuccess:
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
+ if err != nil {
|
|
|
+ c_log.GlobalLogger.Info("创建订阅者报错，可能由于节点未启动，再次尝试：", err)
|
|
|
+ time.Sleep(time.Duration(2) * time.Second)
|
|
|
+ continue
|
|
|
+ } else {
|
|
|
+ break
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
|
zwh