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

package service

import (
	commonConfig "cicv-data-closedloop/aarch64/jili/common/config"
	"cicv-data-closedloop/aarch64/jili/common/service"
	masterConfig "cicv-data-closedloop/aarch64/jili/master/config"
	"cicv-data-closedloop/common/config/c_log"
	commonEntity "cicv-data-closedloop/common/entity"
	"cicv-data-closedloop/common/util"
	"cicv-data-closedloop/pjisuv_msgs"
	"cicv-data-closedloop/pjisuv_ticker"
	"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"
	"math"
	"sync"
	"time"
)

type Weather struct {
	WeatherID   []int
	temperature float64
	humidity    float64
}
type Point struct {
	x, y float64
}
type Location struct {
	Latitude  float64
	Longitude float64
}

var (
	LeftCurveFlag  bool = false
	RightCurveFlag bool = false //用来判断车辆是左转还是右转
	//定义了车间四个边界点，将车间抽象成一个四边形
	LocationCount = 0
	vertices      = []Point{
		{x: 456128.413, y: 4397847.78},
		{x: 456288.257, y: 4397953.51},
		{x: 456359.022, y: 4397822.84},
		{x: 456191.065, y: 4397733.3},
	}
	//定义园区部门Ｔ字路口的经纬度坐标值
	point3 = Location{39.73040966605621, 116.48995329696209}
	point4 = Location{39.73083727413453, 116.49079780188244}
	point5 = Location{39.72976753711939, 116.49043130389033}
	point6 = Location{39.73012466515933, 116.49128381717591}
	point7 = Location{39.729251498328246, 116.49077484625299}
	point8 = Location{39.72964529630643, 116.49164592200161}

	pointlist = []Location{point3, point4, point5, point6, point7, point8}

	cicvLocationTime = time.Now()
	// -----------------------------共享变量
	//cicv_location
	//NumOfCicvLocation = 0
	SpeedSlice   = []float64{}
	AccelXSlice  = []float64{}
	JunctionFlag = false
	//Yowslice          = make([]float64, 0)
	//AngleSlice        = make([][]float64, 0)
	// /tpperception
	countSideBySide           int     = 0
	Frame                     float32 = 0.0
	ObjDicOfTpperception              = make(map[uint32][][]float32)
	objTypeDicOfTpperception          = make(map[uint32]uint8)
	objSpeedDicOfTpperception         = make(map[uint32]float64)
	PreCloseTargetSlice               = []uint32{}
	// /pji_control_pub
	numCountPjiControlCommandOfPjControlPub int
	egoSteeringCmdOfPjControlPub            []float64
	egoThrottleCmdOfPjControlPub            []float64
	// /data_read
	numCountDataReadOfDataRead int
	egoSteeringRealOfDataRead  []float64
	egoThrottleRealOfDataRead  []float64
	GearPosSlice               = []int16{}
	// --------------------------------------------------
	shareVars           = new(sync.Map)
	saveTimeWindowMutex sync.Mutex // 保存时间窗口需要锁，防止数据竟态
	latestTimeWindowEnd = util.GetTimeCustom(time.Now())
	triggerInterval     = 3.0 // 每个触发器3秒触发一次
)

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

	var err error
	subscribers := make([]*goroslib.Subscriber, masterConfig.AllTopicsNumber)
	subscribersTimes := make([]time.Time, masterConfig.AllTopicsNumber)
	subscribersTimeMutexes := make([]sync.Mutex, masterConfig.AllTopicsNumber)
	subscribersMutexes := make([]sync.Mutex, masterConfig.AllTopicsNumber)
	for i, topic := range masterConfig.AllTopics {
		for {
			// 定时器，区别于订阅者
			if topic == pjisuv_ticker.TickerTopic {
				// 1 把所有触发器函数执行一遍，触发器内部创建额外的定时任务goroutine
				for _, f := range masterConfig.RuleOfCicvTicker {
					f(shareVars)
				}
				// 2 创建goroutine接收定时任务触发器返回的Label和Time，执行触发逻辑
				go func() {
					for {
						time.Sleep(time.Duration(1)) // 降低循环频率
						select {
						case tickInfo := <-pjisuv_ticker.TickerChan:
							faultLabel := tickInfo.FaultLabel
							faultHappenTime := tickInfo.FaultHappenTime
							lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
							saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
						default:
						}
					}
				}()
			}
			create := false // 判断是否创建成功，用于打印日志
			// 1
			if topic == masterConfig.TopicOfAmrPose && (len(masterConfig.RuleOfAmrPose1) > 0 || len(masterConfig.RuleOfAmrPose3) > 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.RuleOfAmrPose3) > 0 {
								for _, f := range masterConfig.RuleOfAmrPose3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 2
			if topic == masterConfig.TopicOfBoundingBoxesFast &&
				(len(masterConfig.RuleOfBoundingBoxesFast1) > 0 ||
					len(masterConfig.RuleOfBoundingBoxesFast3) > 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.RuleOfBoundingBoxesFast3) > 0 {
								for _, f := range masterConfig.RuleOfBoundingBoxesFast3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 3
			if topic == masterConfig.TopicOfCameraFault &&
				(len(masterConfig.RuleOfCameraFault1) > 0 ||
					len(masterConfig.RuleOfCameraFault3) > 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.RuleOfCameraFault3) > 0 {
								for _, f := range masterConfig.RuleOfCameraFault3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 4
			if topic == masterConfig.TopicOfCanData &&
				(len(masterConfig.RuleOfCanData1) > 0 ||
					len(masterConfig.RuleOfCanData3) > 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.RuleOfCanData3) > 0 {
								for _, f := range masterConfig.RuleOfCanData3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 5
			if topic == masterConfig.TopicOfCh128x1LslidarPointCloud &&
				(len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 ||
					len(masterConfig.RuleOfCh128x1LslidarPointCloud3) > 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.RuleOfCh128x1LslidarPointCloud3) > 0 {
								for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 6
			if topic == masterConfig.TopicOfCh64wLLslidarPointCloud &&
				(len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 ||
					len(masterConfig.RuleOfCh64wLLslidarPointCloud3) > 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.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.RuleOfCh64wLLslidarPointCloud3) > 0 {
								for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 7
			if topic == masterConfig.TopicOfCh64wLScan &&
				(len(masterConfig.RuleOfCh64wLScan1) > 0 ||
					len(masterConfig.RuleOfCh64wLScan3) > 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.RuleOfCh64wLScan3) > 0 {
								for _, f := range masterConfig.RuleOfCh64wLScan3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 8
			if topic == masterConfig.TopicOfCh64wRLslidarPointCloud &&
				(len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 ||
					len(masterConfig.RuleOfCh64wRLslidarPointCloud3) > 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.RuleOfCh64wRLslidarPointCloud3) > 0 {
								for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 9
			if topic == masterConfig.TopicOfCh64wRScan &&
				(len(masterConfig.RuleOfCh64wRScan1) > 0 ||
					len(masterConfig.RuleOfCh64wRScan3) > 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.RuleOfCh64wRScan3) > 0 {
								for _, f := range masterConfig.RuleOfCh64wRScan3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 10
			if topic == masterConfig.TopicOfCicvLidarclusterMovingObjects &&
				(len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 ||
					len(masterConfig.RuleOfCicvLidarclusterMovingObjects3) > 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.RuleOfCicvLidarclusterMovingObjects3) > 0 {
								for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 11 有共享变量的订阅者必须被创建
			if topic == masterConfig.TopicOfCicvAmrTrajectory {
				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.RuleOfCicvAmrTrajectory3) > 0 {
								for _, f := range masterConfig.RuleOfCicvAmrTrajectory3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
						// 更新共享变量

						Trajectorypoints := data.Trajectoryinfo.Trajectorypoints
						if len(Trajectorypoints) > 2 {
							StartHeading := Trajectorypoints[0].Heading
							EndHeading := Trajectorypoints[len(Trajectorypoints)-1].Heading
							diffHeading := StartHeading - EndHeading
							//fmt.Println(diffHeading)

							if diffHeading < -1.0 && diffHeading > -3.0 {
								LeftCurveFlag = true
								//fmt.Println(diffHeading)
							} else {
								LeftCurveFlag = false
							}
							if diffHeading > 1.0 && diffHeading < 3.0 {
								RightCurveFlag = true
								//fmt.Println(diffHeading)
							} else {
								RightCurveFlag = false
							}

						} else {
							LeftCurveFlag = false
							RightCurveFlag = false
						}
						shareVars.Store("LeftCurveFlag", LeftCurveFlag)
						shareVars.Store("RightCurveFlag", RightCurveFlag)

						currentCurvateres := make([]float64, 0)
						for _, point := range data.Trajectoryinfo.Trajectorypoints {
							currentCurvateres = append(currentCurvateres, math.Abs(float64(point.Curvature)))
						}
						shareVars.Store("LastCurvaturesOfCicvAmrTrajectory", currentCurvateres)
						shareVars.Store("DecisionType", data.Trajectoryinfo.DecisionType)
					},
				})
				if err == nil {
					create = true
				}
			}

			// 12 有共享变量的订阅者必须被创建
			if topic == masterConfig.TopicOfCicvLocation {
				LocationCount++
				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.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.RuleOfCicvLocation3) > 0 {
								for _, f := range masterConfig.RuleOfCicvLocation3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
						// 更新共享变量
						if LocationCount%10 == 0 {
							enterflag := IfEnter(pointlist, 12.0, data.Latitude, data.Longitude)
							shareVars.Store("EnterJunctionFlag", enterflag)
							LocationCount = 0
						}

						AbsSpeed := math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))
						AccelXSlice = append(AccelXSlice, data.AccelX)
						shareVars.Store("AbsSpeed", AbsSpeed)
						shareVars.Store("AccelXSlice", AccelXSlice)
						shareVars.Store("VelocityXOfCicvLocation", data.VelocityX)
						shareVars.Store("VelocityYOfCicvLocation", data.VelocityY)
						shareVars.Store("VelocityZOfCicvLocation", data.VelocityZ)
						shareVars.Store("YawOfCicvLocation", data.Yaw)
						shareVars.Store("AngularVelocityZOfCicvLocation", data.AngularVelocityZ)
						shareVars.Store("PositionXOfCicvLocation", data.PositionX)
						shareVars.Store("PositionYOfCicvLocation", data.PositionY)
						shareVars.Store("Latitude", data.Latitude)
						shareVars.Store("Longitude", data.Longitude)
						//SpeedSlice 用于自车频繁起停触发器-FrequentStartsAndStops
						SpeedSlice = append(SpeedSlice, AbsSpeed)
						if len(SpeedSlice) > 6000 {
							SpeedSlice = SpeedSlice[1:]
						}
						shareVars.Store("SpeedSlice", SpeedSlice)
						/*用于 陡坡-DescendingSteepHill/ClimbingSteepHill 侧翻预警-RolloverWarning 触发器
						if NumOfCicvLocation%10==0{
							AngleSlice[0] = append(AngleSlice[0], data.Qx)
							AngleSlice[1] = append(AngleSlice[1], data.Qy)
							AngleSlice[2] = append(AngleSlice[2], data.Qz)
							AngleSlice[3] = append(AngleSlice[3], data.Qw)
							shareVars.Store("AngleSlice", AngleSlice)
							NumOfCicvLocation=1
						}
						NumOfCicvLocation++
						*/

						/*用于 掉头TurnAround触发器
						Yowslice = append(Yowslice, data.Yaw)
						if len(Yowslice) >= 600 {
							Yowslice = Yowslice[1:]
						}
						shareVars.Store("Yowslice", Yowslice)
						*/

						// 用于判断是否在车间内
						if time.Since(cicvLocationTime).Seconds() > 1 {
							p := Point{x: data.PositionX, y: data.PositionY}
							OutsideWorkshopFlag := isPointInPolygon(p, vertices) //在车间返回０，不在车间返回１
							shareVars.Store("OutsideWorkshopFlag", OutsideWorkshopFlag)
							cicvLocationTime = time.Now()
						}
					},
				})
				if err == nil {
					create = true
				}
			}

			// 13
			if topic == masterConfig.TopicOfCloudClusters &&
				(len(masterConfig.RuleOfCloudClusters1) > 0 ||
					len(masterConfig.RuleOfCloudClusters3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfCloudClusters3) > 0 {
								for _, f := range masterConfig.RuleOfCloudClusters3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 14
			if topic == masterConfig.TopicOfHeartbeatInfo &&
				(len(masterConfig.RuleOfHeartbeatInfo1) > 0 ||
					len(masterConfig.RuleOfHeartbeatInfo3) > 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.RuleOfHeartbeatInfo3) > 0 {
								for _, f := range masterConfig.RuleOfHeartbeatInfo3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 15
			if topic == masterConfig.TopicOfLidarPretreatmentCost &&
				(len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 ||
					len(masterConfig.RuleOfLidarPretreatmentCost3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfLidarPretreatmentCost3) > 0 {
								for _, f := range masterConfig.RuleOfLidarPretreatmentCost3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 16
			if topic == masterConfig.TopicOfLidarPretreatmentOdometry &&
				(len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 ||
					len(masterConfig.RuleOfLidarPretreatmentOdometry3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfLidarPretreatmentOdometry3) > 0 {
								for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}

						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 17
			if topic == masterConfig.TopicOfLidarRoi &&
				(len(masterConfig.RuleOfLidarRoi1) > 0 ||
					len(masterConfig.RuleOfLidarRoi3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfLidarRoi3) > 0 {
								for _, f := range masterConfig.RuleOfLidarRoi3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 18
			if topic == masterConfig.TopicOfLine1 &&
				(len(masterConfig.RuleOfLine11) > 0 ||
					len(masterConfig.RuleOfLine13) > 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
									}
								}
							}
							if len(masterConfig.RuleOfLine13) > 0 {
								for _, f := range masterConfig.RuleOfLine13 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 19
			if topic == masterConfig.TopicOfLine2 &&
				(len(masterConfig.RuleOfLine21) > 0 ||
					len(masterConfig.RuleOfLine23) > 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
									}
								}
							}
							if len(masterConfig.RuleOfLine23) > 0 {
								for _, f := range masterConfig.RuleOfLine23 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 20
			if topic == masterConfig.TopicOfMapPolygon &&
				(len(masterConfig.RuleOfMapPolygon1) > 0 ||
					len(masterConfig.RuleOfMapPolygon3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfMapPolygon3) > 0 {
								for _, f := range masterConfig.RuleOfMapPolygon3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 21
			if topic == masterConfig.TopicOfObstacleDisplay &&
				(len(masterConfig.RuleOfObstacleDisplay1) > 0 ||
					len(masterConfig.RuleOfObstacleDisplay3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfObstacleDisplay3) > 0 {
								for _, f := range masterConfig.RuleOfObstacleDisplay3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 22 有共享变量的订阅者必须被创建
			if topic == masterConfig.TopicOfPjControlPub {
				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() > 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.RuleOfPjControlPub3) > 0 {
								for _, f := range masterConfig.RuleOfPjControlPub3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
						// 更新共享变量
						numCountPjiControlCommandOfPjControlPub++
						if numCountPjiControlCommandOfPjControlPub == 10 {
							egoSteeringCmdOfPjControlPub = append(egoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
							egoThrottleCmdOfPjControlPub = append(egoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
							numCountPjiControlCommandOfPjControlPub = 0
						}
						shareVars.Store("NumCountPjiControlCommandOfPjControlPub", numCountPjiControlCommandOfPjControlPub)
						shareVars.Store("EgoSteeringCmdOfPjControlPub", egoSteeringCmdOfPjControlPub)
						shareVars.Store("EgoThrottleCmdOfPjControlPub", egoThrottleCmdOfPjControlPub)
					},
				})
				if err == nil {
					create = true
				}
			}

			// 23
			if topic == masterConfig.TopicOfPointsCluster &&
				(len(masterConfig.RuleOfPointsCluster1) > 0 ||
					len(masterConfig.RuleOfPointsCluster3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfPointsCluster3) > 0 {
								for _, f := range masterConfig.RuleOfPointsCluster3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 24
			if topic == masterConfig.TopicOfPointsConcat &&
				(len(masterConfig.RuleOfPointsConcat1) > 0 ||
					len(masterConfig.RuleOfPointsConcat3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfPointsConcat3) > 0 {
								for _, f := range masterConfig.RuleOfPointsConcat3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 25
			if topic == masterConfig.TopicOfReferenceDisplay &&
				(len(masterConfig.RuleOfReferenceDisplay1) > 0 ||
					len(masterConfig.RuleOfReferenceDisplay3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfReferenceDisplay3) > 0 {
								for _, f := range masterConfig.RuleOfReferenceDisplay3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 26
			if topic == masterConfig.TopicOfReferenceTrajectory &&
				(len(masterConfig.RuleOfReferenceTrajectory1) > 0 ||
					len(masterConfig.RuleOfReferenceTrajectory3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfReferenceTrajectory3) > 0 {
								for _, f := range masterConfig.RuleOfReferenceTrajectory3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 27
			if topic == masterConfig.TopicOfRoiPoints &&
				(len(masterConfig.RuleOfRoiPoints1) > 0 ||
					len(masterConfig.RuleOfRoiPoints3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfRoiPoints3) > 0 {
								for _, f := range masterConfig.RuleOfRoiPoints3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 28
			if topic == masterConfig.TopicOfRoiPolygon &&
				(len(masterConfig.RuleOfRoiPolygon1) > 0 ||
					len(masterConfig.RuleOfRoiPolygon3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfRoiPolygon3) > 0 {
								for _, f := range masterConfig.RuleOfRoiPolygon3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 29
			if topic == masterConfig.TopicOfTf &&
				(len(masterConfig.RuleOfTf1) > 0 ||
					len(masterConfig.RuleOfTf3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfTf3) > 0 {
								for _, f := range masterConfig.RuleOfTf3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 30 有共享变量的订阅者必须被创建
			if topic == masterConfig.TopicOfTpperception {
				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() > 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
									}
								}
							}
							if len(masterConfig.RuleOfTpperception3) > 0 {
								for _, f := range masterConfig.RuleOfTpperception3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()

						// 更新共享变量
						for _, obj := range data.Objs {
							YawOfCicvLocation, _ := shareVars.Load("YawOfCicvLocation")
							diffh := float64(obj.Heading) - YawOfCicvLocation.(float64)
							if diffh < -180.0 {
								diffh = 360.0 + diffh
							} else if diffh > 180.0 {
								diffh = 360.0 - diffh
							} else {
								diffh = math.Abs(diffh)
							}

							if math.Abs(float64(obj.X)) >= 30 || math.Abs(float64(obj.Y)) >= 30 {
								continue
							}
							if _, ok := ObjDicOfTpperception[obj.Id]; !ok {
								ObjDicOfTpperception[obj.Id] = [][]float32{{}, {}, {}, {}, {}, {}, {}, {}}
							}
							ObjDicOfTpperception[obj.Id][0] = append(ObjDicOfTpperception[obj.Id][0], obj.X)
							ObjDicOfTpperception[obj.Id][1] = append(ObjDicOfTpperception[obj.Id][1], obj.Y)
							ObjDicOfTpperception[obj.Id][2] = append(ObjDicOfTpperception[obj.Id][2], obj.Vxrel)
							absspeed := math.Sqrt(math.Pow(float64(obj.Vxabs), 2) + math.Pow(float64(obj.Vyabs), 2))
							ObjDicOfTpperception[obj.Id][3] = append(ObjDicOfTpperception[obj.Id][3], float32(absspeed))
							ObjDicOfTpperception[obj.Id][4] = append(ObjDicOfTpperception[obj.Id][4], obj.Heading)
							ObjDicOfTpperception[obj.Id][5] = append(ObjDicOfTpperception[obj.Id][5], Frame)
							ObjDicOfTpperception[obj.Id][6] = append(ObjDicOfTpperception[obj.Id][6], float32(obj.Type))
							ObjDicOfTpperception[obj.Id][7] = append(ObjDicOfTpperception[obj.Id][7], float32(diffh))
							if len(ObjDicOfTpperception[obj.Id][0]) >= 100 {
								ObjDicOfTpperception[obj.Id][0] = ObjDicOfTpperception[obj.Id][0][1:]
								ObjDicOfTpperception[obj.Id][1] = ObjDicOfTpperception[obj.Id][1][1:]
								ObjDicOfTpperception[obj.Id][2] = ObjDicOfTpperception[obj.Id][2][1:]
								ObjDicOfTpperception[obj.Id][3] = ObjDicOfTpperception[obj.Id][3][1:]
								ObjDicOfTpperception[obj.Id][4] = ObjDicOfTpperception[obj.Id][4][1:]
								ObjDicOfTpperception[obj.Id][5] = ObjDicOfTpperception[obj.Id][5][1:]
								ObjDicOfTpperception[obj.Id][6] = ObjDicOfTpperception[obj.Id][6][1:]
								ObjDicOfTpperception[obj.Id][7] = ObjDicOfTpperception[obj.Id][7][1:]
							}

							objTypeDicOfTpperception[obj.Id] = obj.Type
							objSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
						}
						shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
						shareVars.Store("ObjTypeDicOfTpperception", objTypeDicOfTpperception)
						shareVars.Store("ObjSpeedDicOfTpperception", objSpeedDicOfTpperception)
						shareVars.Store("countSideBySide", countSideBySide)

						Frame++
					},
				})
				if err == nil {
					create = true
				}
			}

			// 31
			if topic == masterConfig.TopicOfTpperceptionVis &&
				(len(masterConfig.RuleOfTpperceptionVis1) > 0 ||
					len(masterConfig.RuleOfTpperceptionVis3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfTpperceptionVis3) > 0 {
								for _, f := range masterConfig.RuleOfTpperceptionVis3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 32
			if topic == masterConfig.TopicOfTprouteplan &&
				(len(masterConfig.RuleOfTprouteplan1) > 0 ||
					len(masterConfig.RuleOfTprouteplan3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfTprouteplan3) > 0 {
								for _, f := range masterConfig.RuleOfTprouteplan3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 33
			if topic == masterConfig.TopicOfTrajectoryDisplay &&
				(len(masterConfig.RuleOfTrajectoryDisplay1) > 0 ||
					len(masterConfig.RuleOfTrajectoryDisplay3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfTrajectoryDisplay3) > 0 {
								for _, f := range masterConfig.RuleOfTrajectoryDisplay3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 34
			if topic == masterConfig.TopicOfUngroundCloudpoints &&
				(len(masterConfig.RuleOfUngroundCloudpoints1) > 0 ||
					len(masterConfig.RuleOfUngroundCloudpoints3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfUngroundCloudpoints3) > 0 {
								for _, f := range masterConfig.RuleOfUngroundCloudpoints3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 35
			if topic == masterConfig.TopicOfCameraImage &&
				(len(masterConfig.RuleOfCameraImage1) > 0 ||
					len(masterConfig.RuleOfCameraImage3) > 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
									}
								}
							}
							if len(masterConfig.RuleOfCameraImage3) > 0 {
								for _, f := range masterConfig.RuleOfCameraImage3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}

			// 36 有共享变量的订阅者必须被创建
			if topic == masterConfig.TopicOfDataRead {
				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() > 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.RuleOfDataRead3) > 0 {
								for _, f := range masterConfig.RuleOfDataRead3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
						// 更新共享变量
						numCountDataReadOfDataRead++
						if numCountDataReadOfDataRead == 10 {
							egoSteeringRealOfDataRead = append(egoSteeringRealOfDataRead, data.ActStrWhAng)
							egoThrottleRealOfDataRead = append(egoThrottleRealOfDataRead, data.AccPed2)
							numCountDataReadOfDataRead = 0
						}
						GearPosSlice = append(GearPosSlice, data.GearPos)
						shareVars.Store("NumCountDataReadOfDataRead", numCountDataReadOfDataRead)
						shareVars.Store("EgoSteeringRealOfDataRead", egoSteeringRealOfDataRead)
						shareVars.Store("EgoThrottleRealOfDataRead", egoThrottleRealOfDataRead)
						shareVars.Store("ActStrWhAngOfDataRead", data.ActStrWhAng)
						shareVars.Store("GearPosSlice", GearPosSlice)
					},
				})
				if err == nil {
					create = true
				}
			}
			// 37
			if topic == masterConfig.TopicOfPjiGps &&
				(len(masterConfig.RuleOfPjiGps1) > 0 ||
					len(masterConfig.RuleOfPjiGps3) > 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.RuleOfPjiGps3) > 0 {
								for _, f := range masterConfig.RuleOfPjiGps3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}
			// 38
			if topic == masterConfig.TopicOfFaultInfo &&
				(len(masterConfig.RuleOfFaultInfo1) > 0 ||
					len(masterConfig.RuleOfFaultInfo3) > 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.RuleOfFaultInfo1) > 0 {
								for _, f := range masterConfig.RuleOfFaultInfo1 {
									faultLabel = f(data)
									if faultLabel != "" {
										for _, infoVec := range data.InfoVec {
											c_log.GlobalLogger.Info("接收到故障码：", infoVec.ErrorCode)
										}
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
							if len(masterConfig.RuleOfFaultInfo3) > 0 {
								for _, f := range masterConfig.RuleOfFaultInfo3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										for _, infoVec := range data.InfoVec {
											c_log.GlobalLogger.Info("接收到故障码：", infoVec.ErrorCode)
										}
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
					},
				})
				if err == nil {
					create = true
				}
			}
			// 39 有共享变量的订阅者必须被创建
			if topic == masterConfig.TopicOfPjVehicleFdbPub {
				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
									}
								}
							}
							if len(masterConfig.RuleOfPjVehicleFdbPub3) > 0 {
								for _, f := range masterConfig.RuleOfPjVehicleFdbPub3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()
						// 更新共享变量
						shareVars.Store("AutomodeOfPjVehicleFdbPub", data.Automode)
					},
				})
				if err == nil {
					create = true
				}
			}
			// 40 有共享变量的订阅者必须被创建
			if topic == masterConfig.TopicOfEndPointMessage {
				subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
					Node:  commonConfig.RosNode,
					Topic: topic,
					Callback: func(data *geometry_msgs.Point) {
						subscribersTimeMutexes[i].Lock()
						if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
							subscribersMutexes[i].Lock()
							faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
							lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
							faultLabel := ""
							if len(masterConfig.RuleOfEndPointMessage1) > 0 {
								for _, f := range masterConfig.RuleOfEndPointMessage1 {
									faultLabel = f(data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
							if len(masterConfig.RuleOfEndPointMessage3) > 0 {
								for _, f := range masterConfig.RuleOfEndPointMessage3 {
									faultLabel = f(shareVars, data)
									if faultLabel != "" {
										saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
										subscribersTimes[i] = time.Now()
										goto TriggerSuccess
									}
								}
							}
						TriggerSuccess:
							subscribersMutexes[i].Unlock()
						}
						subscribersTimeMutexes[i].Unlock()

						// 更新共享变量
						shareVars.Store("EndPointX", data.X)
						shareVars.Store("EndPointY", data.Y)
					},
				})
				if err == nil {
					create = true
				}
			}
			if err != nil {
				c_log.GlobalLogger.Infof("创建订阅者报错，可能由于节点未启动，再次尝试【%v】", err)
				time.Sleep(time.Duration(2) * time.Second)
				continue
			} else {
				if create {
					c_log.GlobalLogger.Infof("创建订阅者订阅话题【%v】", topic)
				}
				break
			}
		}

	}
	c_log.GlobalLogger.Infof("全部订阅者创建完成。")
	select {
	case signal := <-service.ChannelKillWindowProducer:
		if signal == 1 {
			commonConfig.RosNode.Close()
			service.AddKillTimes("3")
			return
		}
	}
}

func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *commonEntity.TimeWindow) {
	var taskBeforeTime, taskAfterTime, taskMaxTime int
	{
		//if commonConfig.PlatformConfig.TaskBeforeTime == 0 {
		//	taskBeforeTime = 5
		//} else {
		//	taskBeforeTime = commonConfig.PlatformConfig.TaskBeforeTime
		//}
		//if commonConfig.PlatformConfig.TaskAfterTime == 0 {
		//	taskAfterTime = 4
		//} else {
		//	taskAfterTime = commonConfig.PlatformConfig.TaskAfterTime
		//}
		//if commonConfig.PlatformConfig.TaskMaxTime == 0 {
		//	taskMaxTime = 60
		//} else {
		//	taskMaxTime = commonConfig.PlatformConfig.TaskMaxTime
		//}

		// todo 这里基于任务设置taskBeforeTime和taskAfterTime的做法后面需要修改为根据触发器设置
		if len(commonConfig.CurrTask.TaskTriggers) > 0 {
			trigger := commonConfig.CurrTask.TaskTriggers[0]
			taskBeforeTime = trigger.CollectionPreSeconds
			taskAfterTime = trigger.CollectionAfterSeconds
		} else {
			taskBeforeTime = 5
			taskAfterTime = 4
		}

		taskMaxTime = 60
	}
	saveTimeWindowMutex.Lock()
	defer saveTimeWindowMutex.Unlock()
	masterTopics, slaveTopics := getTopicsOfNode(faultLabel)
	if lastTimeWindow == nil || util.TimeCustom1GreaterTimeCustom2(faultHappenTime, lastTimeWindow.TimeWindowEnd) { // 如果是不在旧故障窗口内，添加一个新窗口
		exceptBegin := util.TimeCustomChange(faultHappenTime, -taskBeforeTime)
		finalTimeWindowBegin := ""
		if util.TimeCustom1LessEqualThanTimeCustom2(exceptBegin, latestTimeWindowEnd) { // 窗口最早时间不能早于上一个窗口结束时间
			finalTimeWindowBegin = latestTimeWindowEnd
		} else {
			finalTimeWindowBegin = exceptBegin
		}
		latestTimeWindowEnd = util.TimeCustomChange(faultHappenTime, taskAfterTime)
		newTimeWindow := commonEntity.TimeWindow{
			FaultTime:       faultHappenTime,
			TimeWindowBegin: finalTimeWindowBegin,
			TimeWindowEnd:   latestTimeWindowEnd,
			Length:          util.CalculateDifferenceOfTimeCustom(finalTimeWindowBegin, latestTimeWindowEnd),
			Labels:          []string{faultLabel},
			MasterTopics:    masterTopics,
			SlaveTopics:     slaveTopics,
		}
		c_log.GlobalLogger.Infof("不在旧故障窗口内，向生产者队列添加一个新窗口，【Lable】=%v，【FaultTime】=%v，【Length】=%v", newTimeWindow.Labels, newTimeWindow.FaultTime, newTimeWindow.Length)

		commonEntity.AddTimeWindowToTimeWindowProducerQueue(newTimeWindow)
	} else { // 如果在旧故障窗口内
		commonEntity.TimeWindowProducerQueueMutex.RLock()
		defer commonEntity.TimeWindowProducerQueueMutex.RUnlock()
		// 更新故障窗口end时间
		expectEnd := util.TimeCustomChange(faultHappenTime, taskAfterTime) // 窗口期望关闭时间是触发时间加上后置时间
		expectLength := util.CalculateDifferenceOfTimeCustom(lastTimeWindow.TimeWindowBegin, expectEnd)
		if expectLength < taskMaxTime {
			latestTimeWindowEnd = expectEnd
			lastTimeWindow.TimeWindowEnd = latestTimeWindowEnd
			lastTimeWindow.Length = util.CalculateDifferenceOfTimeCustom(lastTimeWindow.TimeWindowBegin, lastTimeWindow.TimeWindowEnd)
		}
		// 更新label
		labels := lastTimeWindow.Labels
		lastTimeWindow.Labels = util.AppendIfNotExists(labels, faultLabel)
		// 更新 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
}
func isPointInPolygon(p Point, vertices []Point) bool {
	intersections := 0
	for i := 0; i < len(vertices); i++ {
		j := (i + 1) % len(vertices)
		if rayIntersectsSegment(p, vertices[i], vertices[j]) {
			intersections++
		}
	}
	return !(intersections%2 == 1)
}

func rayIntersectsSegment(p, p1, p2 Point) bool {
	if p1.y > p2.y {
		p1, p2 = p2, p1
	}
	if p.y == p1.y || p.y == p2.y {
		p.y += 0.0001
	}

	if p.y < p1.y || p.y > p2.y {
		return false
	}

	if p.x > max(p1.x, p2.x) {
		return false
	}

	if p.x < min(p1.x, p2.x) {
		return true
	}

	blueSlope := (p.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y) + p1.x
	return p.x < blueSlope
}

func min(a, b float64) float64 {
	if a < b {
		return a
	}
	return b
}

func max(a, b float64) float64 {
	if a > b {
		return a
	}
	return b
}

// 判断Ego是否在交叉口，在的话返回true
func IfEnter(pointlist []Location, radius float64, lat, lon float64) bool {
	// 判断是否进入点列表中的区域
	point1 := Location{Latitude: lat, Longitude: lon}
	for _, point := range pointlist {
		d := distance(point1, point)
		if d <= radius {
			return true
		}
	}
	return false
}

// 计算两点之间的距离（米）
func distance(point1, point2 Location) float64 {
	// 经纬度转弧度
	lat1 := point1.Latitude * math.Pi / 180
	lon1 := point1.Longitude * math.Pi / 180
	lat2 := point2.Latitude * math.Pi / 180
	lon2 := point2.Longitude * math.Pi / 180

	// 计算距离
	dlon := lon2 - lon1
	dlat := lat2 - lat1
	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
	d := 6371000 * c
	return d
}