cicv_data_closed_loop/aarch64/pjisuv/master/service/produce_window.go

package svc

import (
	commonConfig "cicv-data-closedloop/aarch64/pjisuv/common/config"
	"cicv-data-closedloop/aarch64/pjisuv/common/service"
	masterConfig "cicv-data-closedloop/aarch64/pjisuv/master/config"
	"cicv-data-closedloop/common/config/c_log"
	"cicv-data-closedloop/common/entity"
	"cicv-data-closedloop/common/util"
	"cicv-data-closedloop/pjisuv_msgs"
	"github.com/bluenviron/goroslib/v2"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
	"sync"
	"time"
)

var (
	m         sync.RWMutex
	velocityX float64
	velocityY float64
	yaw       float64
)

// PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
func PrepareTimeWindowProducerQueue() {
	c_log.GlobalLogger.Info("订阅者 goroutine，启动。")

	var err error
	subscribers := make([]*goroslib.Subscriber, len(commonConfig.SubscribeTopics))
	subscribersTimes := make([]time.Time, len(commonConfig.SubscribeTopics))
	subscribersTimeMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
	subscribersMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
	for i, topic := range commonConfig.SubscribeTopics {
		c_log.GlobalLogger.Info("创建订阅者订阅话题：" + topic)

		// 1
		if topic == masterConfig.TopicOfAmrPose && len(masterConfig.RuleOfAmrPose) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfAmrPose {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 2
		if topic == masterConfig.TopicOfBoundingBoxesFast && len(masterConfig.RuleOfBoundingBoxesFast) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfBoundingBoxesFast {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 3
		if topic == masterConfig.TopicOfCameraFault && len(masterConfig.RuleOfCameraFault) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCameraFault {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 4
		if topic == masterConfig.TopicOfCanData && len(masterConfig.RuleOfCanData) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCanData {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 5
		if topic == masterConfig.TopicOfCh128x1LslidarPointCloud && len(masterConfig.RuleOfCh128x1LslidarPointCloud) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 6
		if topic == masterConfig.TopicOfCh64wLLslidarPointCloud && len(masterConfig.RuleOfCh64wLLslidarPointCloud) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 7
		if topic == masterConfig.TopicOfCh64wLScan && len(masterConfig.RuleOfCh64wLScan) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCh64wLScan {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 8
		if topic == masterConfig.TopicOfCh64wRLslidarPointCloud && len(masterConfig.RuleOfCh64wRLslidarPointCloud) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 9
		if topic == masterConfig.TopicOfCh64wRScan && len(masterConfig.RuleOfCh64wRScan) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCh64wRScan {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 10
		if topic == masterConfig.TopicOfCicvLidarclusterMovingObjects && len(masterConfig.RuleOfCicvLidarclusterMovingObjects) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 11
		if topic == masterConfig.TopicOfCicvAmrTrajectory && len(masterConfig.RuleOfCicvAmrTrajectory) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCicvAmrTrajectory {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 12
		if topic == masterConfig.TopicOfCicvLocation && len(masterConfig.RuleOfCicvLocation) > 0 {
			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  commonConfig.RosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
					// 更新共享变量
					m.RLock()
					velocityX = data.VelocityX
					velocityY = data.VelocityY
					yaw = data.Yaw
					m.RUnlock()

					subscribersTimeMutexes[i].Lock()
					if time.Since(subscribersTimes[i]).Seconds() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCicvLocation {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 13
		if topic == masterConfig.TopicOfCloudClusters && len(masterConfig.RuleOfCloudClusters) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCloudClusters {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 14
		if topic == masterConfig.TopicOfHeartbeatInfo && len(masterConfig.RuleOfHeartbeatInfo) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfHeartbeatInfo {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 15
		if topic == masterConfig.TopicOfLidarPretreatmentCost && len(masterConfig.RuleOfLidarPretreatmentCost) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfLidarPretreatmentCost {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 16
		if topic == masterConfig.TopicOfLidarPretreatmentOdometry && len(masterConfig.RuleOfLidarPretreatmentOdometry) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 17
		if topic == masterConfig.TopicOfLidarRoi && len(masterConfig.RuleOfLidarRoi) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfLidarRoi {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 18
		if topic == masterConfig.TopicOfLine1 && len(masterConfig.RuleOfLine1) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfLine1 {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 19
		if topic == masterConfig.TopicOfLine2 && len(masterConfig.RuleOfLine2) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfLine2 {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 20
		if topic == masterConfig.TopicOfMapPolygon && len(masterConfig.RuleOfMapPolygon) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfMapPolygon {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 21
		if topic == masterConfig.TopicOfObstacleDisplay && len(masterConfig.RuleOfObstacleDisplay) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfObstacleDisplay {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 22
		if topic == masterConfig.TopicOfPjControlPub && len(masterConfig.RuleOfPjControlPub) > 0 {
			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  commonConfig.RosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
					subscribersTimeMutexes[i].Lock()
					if time.Since(subscribersTimes[i]).Seconds() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfPjControlPub {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 23
		if topic == masterConfig.TopicOfPointsCluster && len(masterConfig.RuleOfPointsCluster) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfPointsCluster {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 24
		if topic == masterConfig.TopicOfPointsConcat && len(masterConfig.RuleOfPointsConcat) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfPointsConcat {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 25
		if topic == masterConfig.TopicOfReferenceDisplay && len(masterConfig.RuleOfReferenceDisplay) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfReferenceDisplay {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 26
		if topic == masterConfig.TopicOfReferenceTrajectory && len(masterConfig.RuleOfReferenceTrajectory) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfReferenceTrajectory {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 27
		if topic == masterConfig.TopicOfRoiPoints && len(masterConfig.RuleOfRoiPoints) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfRoiPoints {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 28
		if topic == masterConfig.TopicOfRoiPolygon && len(masterConfig.RuleOfRoiPolygon) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfRoiPolygon {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 29
		if topic == masterConfig.TopicOfTf && len(masterConfig.RuleOfTf) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfTf {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 30
		if topic == masterConfig.TopicOfTpperception && len(masterConfig.RuleOfTpperception) > 0 {
			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  commonConfig.RosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.PerceptionObjects) {
					subscribersTimeMutexes[i].Lock()
					if time.Since(subscribersTimes[i]).Seconds() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfTpperception {
							faultLabel = f(data, velocityX, velocityY, yaw)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 31
		if topic == masterConfig.TopicOfTpperceptionVis && len(masterConfig.RuleOfTpperceptionVis) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfTpperceptionVis {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 32
		if topic == masterConfig.TopicOfTprouteplan && len(masterConfig.RuleOfTprouteplan) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfTprouteplan {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 33
		if topic == masterConfig.TopicOfTrajectoryDisplay && len(masterConfig.RuleOfTrajectoryDisplay) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfTrajectoryDisplay {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 34
		if topic == masterConfig.TopicOfUngroundCloudpoints && len(masterConfig.RuleOfUngroundCloudpoints) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfUngroundCloudpoints {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		// 35
		if topic == masterConfig.TopicOfCameraImage && len(masterConfig.RuleOfCameraImage) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfCameraImage {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}
		// 36
		if topic == masterConfig.TopicOfDataRead && len(masterConfig.RuleOfDataRead) > 0 {
			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
				Node:  commonConfig.RosNode,
				Topic: topic,
				Callback: func(data *pjisuv_msgs.Retrieval) {
					subscribersTimeMutexes[i].Lock()
					if time.Since(subscribersTimes[i]).Seconds() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfDataRead {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}
		// 37
		if topic == masterConfig.TopicOfPjiGps && len(masterConfig.RuleOfPjiGps) > 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() > 1 {
						subscribersMutexes[i].Lock()
						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
						var faultLabel string
						for _, f := range masterConfig.RuleOfPjiGps {
							faultLabel = f(data)
							if faultLabel != "" {
								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
								subscribersTimes[i] = time.Now()
								break
							}
						}
						subscribersMutexes[i].Unlock()
					}
					subscribersTimeMutexes[i].Unlock()
				},
			})
		}

		if err != nil {
			c_log.GlobalLogger.Info("创建订阅者报错：", err)
			//TODO 如何回传日志
			continue
		}
	}

	select {
	case signal := <-service.ChannelKillWindowProducer:
		if signal == 1 {
			commonConfig.RosNode.Close()
			service.AddKillTimes("3")
			return
		}
	}
}

func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *entity.TimeWindow) {
	masterTopics, slaveTopics := getTopicsOfNode(faultLabel)
	if lastTimeWindow == nil || util.TimeCustom1GreaterTimeCustom2(faultHappenTime, lastTimeWindow.TimeWindowEnd) {
		// 2-1 如果是不在旧故障窗口内，添加一个新窗口
		newTimeWindow := entity.TimeWindow{
			FaultTime:       faultHappenTime,
			TimeWindowBegin: util.TimeCustomChange(faultHappenTime, -commonConfig.PlatformConfig.TaskBeforeTime),
			TimeWindowEnd:   util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime),
			Length:          commonConfig.PlatformConfig.TaskBeforeTime + commonConfig.PlatformConfig.TaskAfterTime + 1,
			Labels:          []string{faultLabel},
			MasterTopics:    masterTopics,
			SlaveTopics:     slaveTopics,
		}
		c_log.GlobalLogger.Infof("不在旧故障窗口内，向生产者队列添加一个新窗口，【Lable】=%v，【FaultTime】=%v，【Length】=%v", newTimeWindow.Labels, newTimeWindow.FaultTime, newTimeWindow.Length)

		entity.AddTimeWindowToTimeWindowProducerQueue(newTimeWindow)
	} else {
		// 2-2 如果在旧故障窗口内
		entity.TimeWindowProducerQueueMutex.RLock()
		defer entity.TimeWindowProducerQueueMutex.RUnlock()
		// 2-2-1 更新故障窗口end时间
		maxEnd := util.TimeCustomChange(lastTimeWindow.TimeWindowBegin, commonConfig.PlatformConfig.TaskMaxTime)
		expectEnd := util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime)
		if util.TimeCustom1GreaterTimeCustom2(expectEnd, maxEnd) {
			lastTimeWindow.TimeWindowEnd = maxEnd
			lastTimeWindow.Length = commonConfig.PlatformConfig.TaskMaxTime
		} else {
			if util.TimeCustom1GreaterTimeCustom2(expectEnd, lastTimeWindow.TimeWindowEnd) {
				lastTimeWindow.TimeWindowEnd = expectEnd
				lastTimeWindow.Length = util.CalculateDifferenceOfTimeCustom(lastTimeWindow.TimeWindowBegin, expectEnd)
			}
		}
		// 2-2-2 更新label
		labels := lastTimeWindow.Labels
		lastTimeWindow.Labels = util.AppendIfNotExists(labels, faultLabel)
		// 2-2-3 更新 topic
		sourceMasterTopics := lastTimeWindow.MasterTopics
		lastTimeWindow.MasterTopics = util.MergeSlice(sourceMasterTopics, masterTopics)
		sourceSlaveTopics := lastTimeWindow.SlaveTopics
		lastTimeWindow.SlaveTopics = util.MergeSlice(sourceSlaveTopics, slaveTopics)
		c_log.GlobalLogger.Infof("在旧故障窗口内，更新生产者队列最新的窗口，【Lable】=%v，【FaultTime】=%v，【Length】=%v", lastTimeWindow.Labels, lastTimeWindow.FaultTime, lastTimeWindow.Length)

	}
}

func getTopicsOfNode(faultLabel string) (masterTopics []string, slaveTopics []string) {
	// 获取所有需要采集的topic
	var faultCodeTopics []string
	for _, code := range commonConfig.CloudConfig.Triggers {
		if code.Label == faultLabel {
			faultCodeTopics = code.Topics
		}
	}
	// 根据不同节点采集的topic进行分配采集
	for _, acceptTopic := range faultCodeTopics {
		for _, host := range commonConfig.CloudConfig.Hosts {
			for _, topic := range host.Topics {
				if host.Name == commonConfig.CloudConfig.Hosts[0].Name && acceptTopic == topic {
					masterTopics = append(masterTopics, acceptTopic)
				}
				if host.Name == commonConfig.CloudConfig.Hosts[1].Name && acceptTopic == topic {
					slaveTopics = append(slaveTopics, acceptTopic)
				}
			}
		}
	}
	return masterTopics, slaveTopics
}