package service 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" commonEntity "cicv-data-closedloop/common/entity" "cicv-data-closedloop/common/util" "cicv-data-closedloop/pjisuv_msgs" "cicv-data-closedloop/pjisuv_param" "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" ) // 所有共享变量 var ( latestTimeWindowEnd = util.GetTimeCustom(time.Now()) pjisuvParam = pjisuv_param.PjisuvParam{ ObjDicOfTpperception: make(map[uint32][]float32), ObjTypeDicOfTpperception: make(map[uint32]uint8), ObjSpeedDicOfTpperception: make(map[uint32]float64), } // 每个触发器5秒触发一次 triggerInterval = 3.0 // 保存时间窗口需要锁,防止数据竟态 saveTimeWindowMutex sync.Mutex // /cicv_location mutexOfCicvLocation sync.RWMutex // /tpperception mutexOfTpperception sync.RWMutex // /pj_control_pub mutexOfPjControlPub sync.RWMutex // /data_read mutexOfDataRead sync.RWMutex // /pj_vehicle_fdb_pub mutexOfPjVehicleFdbPub sync.RWMutex // /pj_vehicle_fdb_pub mutexOfCicvAmrTrajectory sync.RWMutex ) // 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 { for { // 增加了可扩展性 if topic == masterConfig.TopicOfCicvExtend { go func() { for { time.Sleep(time.Duration(3500) * time.Millisecond) for _, f := range masterConfig.RuleOfCicvExtend { label := f(pjisuvParam) if label != "" { saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow()) subscribersTimes[i] = time.Now() break } } } }() } // 其他常规监听器 c_log.GlobalLogger.Info("创建订阅者订阅话题:" + topic) // 1 if topic == masterConfig.TopicOfAmrPose && (len(masterConfig.RuleOfAmrPose1) > 0 || len(masterConfig.RuleOfAmrPose2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *visualization_msgs.MarkerArray) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfAmrPose1) > 0 { for _, f := range masterConfig.RuleOfAmrPose1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfAmrPose2) > 0 { for _, f := range masterConfig.RuleOfAmrPose2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 2 if topic == masterConfig.TopicOfBoundingBoxesFast && (len(masterConfig.RuleOfBoundingBoxesFast1) > 0 || len(masterConfig.RuleOfBoundingBoxesFast2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.BoundingBoxArray) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfBoundingBoxesFast1) > 0 { for _, f := range masterConfig.RuleOfBoundingBoxesFast1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfBoundingBoxesFast2) > 0 { for _, f := range masterConfig.RuleOfBoundingBoxesFast2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 3 if topic == masterConfig.TopicOfCameraFault && (len(masterConfig.RuleOfCameraFault1) > 0 || len(masterConfig.RuleOfCameraFault2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.FaultVec) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCameraFault1) > 0 { for _, f := range masterConfig.RuleOfCameraFault1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCameraFault2) > 0 { for _, f := range masterConfig.RuleOfCameraFault2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 4 if topic == masterConfig.TopicOfCanData && (len(masterConfig.RuleOfCanData1) > 0 || len(masterConfig.RuleOfCanData2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.Frame) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCanData1) > 0 { for _, f := range masterConfig.RuleOfCanData1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCanData2) > 0 { for _, f := range masterConfig.RuleOfCanData2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 5 if topic == masterConfig.TopicOfCh128x1LslidarPointCloud && (len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh128x1LslidarPointCloud2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.PointCloud2) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 { for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCh128x1LslidarPointCloud2) > 0 { for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 6 if topic == masterConfig.TopicOfCh64wLLslidarPointCloud && (len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wLLslidarPointCloud2) > 1) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.PointCloud2) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 { for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCh64wLLslidarPointCloud2) > 0 { for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 7 if topic == masterConfig.TopicOfCh64wLScan && (len(masterConfig.RuleOfCh64wLScan1) > 0 || len(masterConfig.RuleOfCh64wLScan2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.LaserScan) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCh64wLScan1) > 0 { for _, f := range masterConfig.RuleOfCh64wLScan1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCh64wLScan2) > 0 { for _, f := range masterConfig.RuleOfCh64wLScan2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 8 if topic == masterConfig.TopicOfCh64wRLslidarPointCloud && (len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wRLslidarPointCloud2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.PointCloud2) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 { for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCh64wRLslidarPointCloud2) > 0 { for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 9 if topic == masterConfig.TopicOfCh64wRScan && (len(masterConfig.RuleOfCh64wRScan1) > 0 || len(masterConfig.RuleOfCh64wRScan2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.LaserScan) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCh64wRScan1) > 0 { for _, f := range masterConfig.RuleOfCh64wRScan1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCh64wRScan2) > 0 { for _, f := range masterConfig.RuleOfCh64wRScan2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 10 if topic == masterConfig.TopicOfCicvLidarclusterMovingObjects && (len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 || len(masterConfig.RuleOfCicvLidarclusterMovingObjects2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.PerceptionCicvMovingObjects) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 { for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCicvLidarclusterMovingObjects2) > 0 { for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 11 if topic == masterConfig.TopicOfCicvAmrTrajectory && (len(masterConfig.RuleOfCicvAmrTrajectory1) > 0 || len(masterConfig.RuleOfCicvAmrTrajectory2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.Trajectory) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCicvAmrTrajectory1) > 0 { for _, f := range masterConfig.RuleOfCicvAmrTrajectory1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCicvAmrTrajectory2) > 0 { for _, f := range masterConfig.RuleOfCicvAmrTrajectory2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() // 触发后更新共享变量 mutexOfCicvAmrTrajectory.RLock() { var currentCurvateres []float64 for _, point := range data.Trajectoryinfo.Trajectorypoints { currentCurvateres = append(currentCurvateres, math.Abs(float64(point.Curvature))) } pjisuvParam.LastCurvaturesOfCicvAmrTrajectory = currentCurvateres } mutexOfCicvAmrTrajectory.RUnlock() }, }) } // 12 if topic == masterConfig.TopicOfCicvLocation && (len(masterConfig.RuleOfCicvLocation1) > 0 || len(masterConfig.RuleOfCicvLocation2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.PerceptionLocalization) { // 更新共享变量 mutexOfCicvLocation.RLock() { pjisuvParam.VelocityXOfCicvLocation = data.VelocityX pjisuvParam.VelocityYOfCicvLocation = data.VelocityY pjisuvParam.VelocityZOfCicvLocation = data.VelocityZ pjisuvParam.YawOfCicvLocation = data.Yaw pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ pjisuvParam.PositionXOfCicvLocation = data.PositionX pjisuvParam.PositionYOfCicvLocation = data.PositionY } mutexOfCicvLocation.RUnlock() subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCicvLocation1) > 0 { for _, f := range masterConfig.RuleOfCicvLocation1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCicvLocation2) > 0 { for _, f := range masterConfig.RuleOfCicvLocation2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 13 if topic == masterConfig.TopicOfCloudClusters && (len(masterConfig.RuleOfCloudClusters1) > 0 || len(masterConfig.RuleOfCloudClusters2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.AutowareCloudClusterArray) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCloudClusters1) > 0 { for _, f := range masterConfig.RuleOfCloudClusters1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCloudClusters2) > 0 { for _, f := range masterConfig.RuleOfCloudClusters2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 14 if topic == masterConfig.TopicOfHeartbeatInfo && (len(masterConfig.RuleOfHeartbeatInfo1) > 0 || len(masterConfig.RuleOfHeartbeatInfo2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.HeartBeatInfo) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfHeartbeatInfo1) > 0 { for _, f := range masterConfig.RuleOfHeartbeatInfo1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfHeartbeatInfo2) > 0 { for _, f := range masterConfig.RuleOfHeartbeatInfo2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 15 if topic == masterConfig.TopicOfLidarPretreatmentCost && (len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 || len(masterConfig.RuleOfLidarPretreatmentCost2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *geometry_msgs.Vector3Stamped) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 { for _, f := range masterConfig.RuleOfLidarPretreatmentCost1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfLidarPretreatmentCost2) > 0 { for _, f := range masterConfig.RuleOfLidarPretreatmentCost2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 16 if topic == masterConfig.TopicOfLidarPretreatmentOdometry && (len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 || len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *nav_msgs.Odometry) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 { for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0 { for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 17 if topic == masterConfig.TopicOfLidarRoi && (len(masterConfig.RuleOfLidarRoi1) > 0 || len(masterConfig.RuleOfLidarRoi2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *geometry_msgs.PolygonStamped) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfLidarRoi1) > 0 { for _, f := range masterConfig.RuleOfLidarRoi1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfLidarRoi2) > 0 { for _, f := range masterConfig.RuleOfLidarRoi2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 18 if topic == masterConfig.TopicOfLine1 && (len(masterConfig.RuleOfLine11) > 0 || len(masterConfig.RuleOfLine12) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *nav_msgs.Path) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfLine11) > 0 { for _, f := range masterConfig.RuleOfLine11 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfLine12) > 0 { for _, f := range masterConfig.RuleOfLine12 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 19 if topic == masterConfig.TopicOfLine2 && (len(masterConfig.RuleOfLine21) > 0 || len(masterConfig.RuleOfLine22) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *nav_msgs.Path) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfLine21) > 0 { for _, f := range masterConfig.RuleOfLine21 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfLine22) > 0 { for _, f := range masterConfig.RuleOfLine22 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 20 if topic == masterConfig.TopicOfMapPolygon && (len(masterConfig.RuleOfMapPolygon1) > 0 || len(masterConfig.RuleOfMapPolygon2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.PolygonStamped) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfMapPolygon1) > 0 { for _, f := range masterConfig.RuleOfMapPolygon1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfMapPolygon2) > 0 { for _, f := range masterConfig.RuleOfMapPolygon2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 21 if topic == masterConfig.TopicOfObstacleDisplay && (len(masterConfig.RuleOfObstacleDisplay1) > 0 || len(masterConfig.RuleOfObstacleDisplay2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *visualization_msgs.MarkerArray) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfObstacleDisplay1) > 0 { for _, f := range masterConfig.RuleOfObstacleDisplay1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfObstacleDisplay2) > 0 { for _, f := range masterConfig.RuleOfObstacleDisplay2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 22 if topic == masterConfig.TopicOfPjControlPub && (len(masterConfig.RuleOfPjControlPub1) > 0 || len(masterConfig.RuleOfPjControlPub2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.CommonVehicleCmd) { // 更新共享变量 mutexOfPjControlPub.RLock() { pjisuvParam.NumCountPjiControlCommandOfPjControlPub++ if pjisuvParam.NumCountPjiControlCommandOfPjControlPub == 10 { pjisuvParam.EgoSteeringCmdOfPjControlPub = append(pjisuvParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle) pjisuvParam.EgoThrottleCmdOfPjControlPub = append(pjisuvParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct) pjisuvParam.NumCountPjiControlCommandOfPjControlPub = 0 } } mutexOfPjControlPub.RUnlock() subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfPjControlPub1) > 0 { for _, f := range masterConfig.RuleOfPjControlPub1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfPjControlPub2) > 0 { for _, f := range masterConfig.RuleOfPjControlPub2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 23 if topic == masterConfig.TopicOfPointsCluster && (len(masterConfig.RuleOfPointsCluster1) > 0 || len(masterConfig.RuleOfPointsCluster2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.PointCloud2) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfPointsCluster1) > 0 { for _, f := range masterConfig.RuleOfPointsCluster1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfPointsCluster2) > 0 { for _, f := range masterConfig.RuleOfPointsCluster2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 24 if topic == masterConfig.TopicOfPointsConcat && (len(masterConfig.RuleOfPointsConcat1) > 0 || len(masterConfig.RuleOfPointsConcat2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.PointCloud2) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfPointsConcat1) > 0 { for _, f := range masterConfig.RuleOfPointsConcat1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfPointsConcat2) > 0 { for _, f := range masterConfig.RuleOfPointsConcat2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 25 if topic == masterConfig.TopicOfReferenceDisplay && (len(masterConfig.RuleOfReferenceDisplay1) > 0 || len(masterConfig.RuleOfReferenceDisplay2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *nav_msgs.Path) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfReferenceDisplay1) > 0 { for _, f := range masterConfig.RuleOfReferenceDisplay1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfReferenceDisplay2) > 0 { for _, f := range masterConfig.RuleOfReferenceDisplay2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 26 if topic == masterConfig.TopicOfReferenceTrajectory && (len(masterConfig.RuleOfReferenceTrajectory1) > 0 || len(masterConfig.RuleOfReferenceTrajectory2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.Trajectory) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfReferenceTrajectory1) > 0 { for _, f := range masterConfig.RuleOfReferenceTrajectory1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfReferenceTrajectory2) > 0 { for _, f := range masterConfig.RuleOfReferenceTrajectory2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 27 if topic == masterConfig.TopicOfRoiPoints && (len(masterConfig.RuleOfRoiPoints1) > 0 || len(masterConfig.RuleOfRoiPoints2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.PointCloud2) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfRoiPoints1) > 0 { for _, f := range masterConfig.RuleOfRoiPoints1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfRoiPoints2) > 0 { for _, f := range masterConfig.RuleOfRoiPoints2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 28 if topic == masterConfig.TopicOfRoiPolygon && (len(masterConfig.RuleOfRoiPolygon1) > 0 || len(masterConfig.RuleOfRoiPolygon2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *nav_msgs.Path) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfRoiPolygon1) > 0 { for _, f := range masterConfig.RuleOfRoiPolygon1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfRoiPolygon2) > 0 { for _, f := range masterConfig.RuleOfRoiPolygon2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 29 if topic == masterConfig.TopicOfTf && (len(masterConfig.RuleOfTf1) > 0 || len(masterConfig.RuleOfTf2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *tf2_msgs.TFMessage) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfTf1) > 0 { for _, f := range masterConfig.RuleOfTf1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfTf2) > 0 { for _, f := range masterConfig.RuleOfTf2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 30 if topic == masterConfig.TopicOfTpperception && (len(masterConfig.RuleOfTpperception1) > 0 || len(masterConfig.RuleOfTpperception2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.PerceptionObjects) { // 更新共享变量 mutexOfTpperception.RLock() { for _, obj := range data.Objs { if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 { continue } // 检查 ObjDicOfTpperception 是否为 nil,如果是,则初始化它 if pjisuvParam.ObjDicOfTpperception == nil { pjisuvParam.ObjDicOfTpperception = make(map[uint32][]float32) } if _, ok := pjisuvParam.ObjDicOfTpperception[obj.Id]; !ok { pjisuvParam.ObjDicOfTpperception[obj.Id] = []float32{} } pjisuvParam.ObjDicOfTpperception[obj.Id] = append(pjisuvParam.ObjDicOfTpperception[obj.Id], obj.Y) } } mutexOfTpperception.RUnlock() subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfTpperception1) > 0 { for _, f := range masterConfig.RuleOfTpperception1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfTpperception2) > 0 { for _, f := range masterConfig.RuleOfTpperception2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() // -------- 触发后更新共享变量 mutexOfTpperception.RLock() { for _, obj := range data.Objs { pjisuvParam.ObjTypeDicOfTpperception[obj.Id] = obj.Type pjisuvParam.ObjSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5) } } mutexOfTpperception.RUnlock() }, }) } // 31 if topic == masterConfig.TopicOfTpperceptionVis && (len(masterConfig.RuleOfTpperceptionVis1) > 0 || len(masterConfig.RuleOfTpperceptionVis2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *visualization_msgs.MarkerArray) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfTpperceptionVis1) > 0 { for _, f := range masterConfig.RuleOfTpperceptionVis1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfTpperceptionVis2) > 0 { for _, f := range masterConfig.RuleOfTpperceptionVis2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 32 if topic == masterConfig.TopicOfTprouteplan && (len(masterConfig.RuleOfTprouteplan1) > 0 || len(masterConfig.RuleOfTprouteplan2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.RoutePlan) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfTprouteplan1) > 0 { for _, f := range masterConfig.RuleOfTprouteplan1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfTprouteplan2) > 0 { for _, f := range masterConfig.RuleOfTprouteplan2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 33 if topic == masterConfig.TopicOfTrajectoryDisplay && (len(masterConfig.RuleOfTrajectoryDisplay1) > 0 || len(masterConfig.RuleOfTrajectoryDisplay2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *nav_msgs.Path) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfTrajectoryDisplay1) > 0 { for _, f := range masterConfig.RuleOfTrajectoryDisplay1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfTrajectoryDisplay2) > 0 { for _, f := range masterConfig.RuleOfTrajectoryDisplay2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 34 if topic == masterConfig.TopicOfUngroundCloudpoints && (len(masterConfig.RuleOfUngroundCloudpoints1) > 0 || len(masterConfig.RuleOfUngroundCloudpoints2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.PointCloud2) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfUngroundCloudpoints1) > 0 { for _, f := range masterConfig.RuleOfUngroundCloudpoints1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfUngroundCloudpoints2) > 0 { for _, f := range masterConfig.RuleOfUngroundCloudpoints2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 35 if topic == masterConfig.TopicOfCameraImage && (len(masterConfig.RuleOfCameraImage1) > 0 || len(masterConfig.RuleOfCameraImage2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *sensor_msgs.Image) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfCameraImage1) > 0 { for _, f := range masterConfig.RuleOfCameraImage1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfCameraImage2) > 0 { for _, f := range masterConfig.RuleOfCameraImage2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 36 if topic == masterConfig.TopicOfDataRead && (len(masterConfig.RuleOfDataRead1) > 0 || len(masterConfig.RuleOfDataRead2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.Retrieval) { // 更新共享变量 mutexOfDataRead.RLock() { pjisuvParam.NumCountDataReadOfDataRead++ if pjisuvParam.NumCountDataReadOfDataRead == 10 { pjisuvParam.EgoSteeringRealOfDataRead = append(pjisuvParam.EgoSteeringRealOfDataRead, data.ActStrWhAng) pjisuvParam.EgoThrottleRealOfDataRead = append(pjisuvParam.EgoThrottleRealOfDataRead, data.AccPed2) pjisuvParam.NumCountDataReadOfDataRead = 0 } pjisuvParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng } mutexOfDataRead.RUnlock() subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfDataRead1) > 0 { for _, f := range masterConfig.RuleOfDataRead1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfDataRead2) > 0 { for _, f := range masterConfig.RuleOfDataRead2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 37 if topic == masterConfig.TopicOfPjiGps && (len(masterConfig.RuleOfPjiGps1) > 0 || len(masterConfig.RuleOfPjiGps2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.PerceptionLocalization) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfPjiGps1) > 0 { for _, f := range masterConfig.RuleOfPjiGps1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfPjiGps2) > 0 { for _, f := range masterConfig.RuleOfPjiGps2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } // 39 if topic == masterConfig.TopicOfPjVehicleFdbPub && (len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 || len(masterConfig.RuleOfPjVehicleFdbPub2) > 0) { subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{ Node: commonConfig.RosNode, Topic: topic, Callback: func(data *pjisuv_msgs.VehicleFdb) { subscribersTimeMutexes[i].Lock() if time.Since(subscribersTimes[i]).Seconds() > triggerInterval { subscribersMutexes[i].Lock() faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间 lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口 faultLabel := "" if len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 { for _, f := range masterConfig.RuleOfPjVehicleFdbPub1 { faultLabel = f(data) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } if len(masterConfig.RuleOfPjVehicleFdbPub2) > 0 { for _, f := range masterConfig.RuleOfPjVehicleFdbPub2 { faultLabel = f(data, &pjisuvParam) if faultLabel != "" { saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow) subscribersTimes[i] = time.Now() goto TriggerSuccess } } } TriggerSuccess: subscribersMutexes[i].Unlock() } subscribersTimeMutexes[i].Unlock() }, }) } if err != nil { c_log.GlobalLogger.Info("创建订阅者报错,可能由于节点未启动,再次尝试:", err) time.Sleep(time.Duration(2) * time.Second) continue } else { break } } } select { case signal := <-service.ChannelKillWindowProducer: if signal == 1 { commonConfig.RosNode.Close() service.AddKillTimes("3") return } } } func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *commonEntity.TimeWindow) { saveTimeWindowMutex.Lock() defer saveTimeWindowMutex.Unlock() masterTopics, slaveTopics := getTopicsOfNode(faultLabel) if lastTimeWindow == nil || util.TimeCustom1GreaterTimeCustom2(faultHappenTime, lastTimeWindow.TimeWindowEnd) { // 如果是不在旧故障窗口内,添加一个新窗口 exceptBegin := util.TimeCustomChange(faultHappenTime, -commonConfig.PlatformConfig.TaskBeforeTime) finalTimeWindowBegin := "" if util.TimeCustom1LessEqualThanTimeCustom2(exceptBegin, latestTimeWindowEnd) { // 窗口最早时间不能早于上一个窗口结束时间 finalTimeWindowBegin = latestTimeWindowEnd } else { finalTimeWindowBegin = exceptBegin } latestTimeWindowEnd = util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.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, commonConfig.PlatformConfig.TaskAfterTime) // 窗口期望关闭时间是触发时间加上后置时间 expectLength := util.CalculateDifferenceOfTimeCustom(lastTimeWindow.TimeWindowBegin, expectEnd) if expectLength < commonConfig.PlatformConfig.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 }