commit

aarch64/kinglong/master/package/config/master_trigger_cfg.go

@@ -6,26 +6,43 @@ import (
 	"cicv-data-closedloop/common/entity"
 	"cicv-data-closedloop/common/util"
 	"cicv-data-closedloop/kinglong_msgs"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
 	"plugin"
 	"strconv"
 )
 
 var (
+	// 扩展
 	TopicOfCicvExtend = "/cicv_extend"
 	RuleOfCicvExtend  []func(param entity.KinglongParam) string
 
+	// 1
+	TopicOfNodeFaultInfo = "/nodefault_info"
+	RuleOfNodefaultInfo  []func(data *kinglong_msgs.FaultInfo) string
+	// 2
+	TopicOfCicvLocation = "/cicv_location"
+	RuleOfCicvLocation  []func(data *kinglong_msgs.PerceptionLocalization) string
+	// 3
+	TopicOfTpperception = "/tpperception"
+	RuleOfTpperception  []func(data *kinglong_msgs.PerceptionObjects, param entity.KinglongParam) string
+	// 4
+	TopicOfFaultInfo = "/fault_info"
+	RuleOfFaultInfo  []func(data *kinglong_msgs.FaultVec) string
+	// 5
+	TopicOfDataRead = "/data_read"
+	RuleOfDataRead  []func(data *kinglong_msgs.Retrieval) string
+	// 6
 	TopicOfJinlongControlPub = "/jinlong_control_pub"
 	RuleOfJinlongControlPub  []func(data *kinglong_msgs.JinlongControlCommand) string
-	TopicOfNodeFaultInfo     = "/nodefault_info"
-	RuleOfNodefaultInfo      []func(data *kinglong_msgs.FaultInfo) string
-	TopicOfCicvLocation      = "/cicv_location"
-	RuleOfCicvLocation       []func(data *kinglong_msgs.PerceptionLocalization) string
-	TopicOfTpperception      = "/tpperception"
-	RuleOfTpperception       []func(data *kinglong_msgs.PerceptionObjects, param entity.KinglongParam) string
-	TopicOfFaultInfo         = "/fault_info"
-	RuleOfFaultInfo          []func(data *kinglong_msgs.FaultVec) string
-	TopicOfDataRead          = "/data_read"
-	RuleOfDataRead           []func(data *kinglong_msgs.Retrieval) string
+	// 7
+	TopicOfFailureLidar = "/failure/lidar"
+	RuleOfFailureLidar  []func(data *std_msgs.Bool) string
+	// 8
+	TopicOfFailureRadar = "/failure/radar"
+	RuleOfFailureRadar  []func(data *std_msgs.Bool) string
+	// 9
+	TopicOfFailureCamera = "/failure/camera"
+	RuleOfFailureCamera  []func(data *std_msgs.Bool) string
 
 	LabelMapTriggerId = make(map[string]string)
 )
@@ -73,41 +90,69 @@ func InitTriggerConfig() {
 				continue
 			}
 			RuleOfCicvExtend = append(RuleOfCicvExtend, f)
-		} else if TopicOfNodeFaultInfo == topic2 {
+		} else if TopicOfNodeFaultInfo == topic2 { // 1
 			f, ok := rule.(func(data *kinglong_msgs.FaultInfo) string)
 			if ok != true {
 				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.FaultInfo) string):", err)
 				continue
 			}
 			RuleOfNodefaultInfo = append(RuleOfNodefaultInfo, f)
-		} else if TopicOfCicvLocation == topic2 {
+		} else if TopicOfCicvLocation == topic2 { // 2
 			f, ok := rule.(func(data *kinglong_msgs.PerceptionLocalization) string)
 			if ok != true {
 				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.PerceptionLocalization) string):", err)
 				continue
 			}
 			RuleOfCicvLocation = append(RuleOfCicvLocation, f)
-		} else if TopicOfTpperception == topic2 {
+		} else if TopicOfTpperception == topic2 { // 3
 			f, ok := rule.(func(data *kinglong_msgs.PerceptionObjects, param entity.KinglongParam) string)
 			if ok != true {
 				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.PerceptionObjects, param entity.KinglongParam) string):", err)
 				continue
 			}
 			RuleOfTpperception = append(RuleOfTpperception, f)
-		} else if TopicOfFaultInfo == topic2 {
+		} else if TopicOfFaultInfo == topic2 { // 4
 			f, ok := rule.(func(data *kinglong_msgs.FaultVec) string)
 			if ok != true {
 				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.FaultVec) string):", err)
 				continue
 			}
 			RuleOfFaultInfo = append(RuleOfFaultInfo, f)
-		} else if TopicOfDataRead == topic2 {
+		} else if TopicOfDataRead == topic2 { // 5
 			f, ok := rule.(func(data *kinglong_msgs.Retrieval) string)
 			if ok != true {
 				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.Retrieval) string):", err)
 				continue
 			}
 			RuleOfDataRead = append(RuleOfDataRead, f)
+		} else if TopicOfJinlongControlPub == topic2 { // 6
+			f, ok := rule.(func(data *kinglong_msgs.JinlongControlCommand) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *std_msgs.Bool) string):", err)
+				continue
+			}
+			RuleOfJinlongControlPub = append(RuleOfJinlongControlPub, f)
+		} else if TopicOfFailureLidar == topic2 { // 7
+			f, ok := rule.(func(data *std_msgs.Bool) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *std_msgs.Bool) string):", err)
+				continue
+			}
+			RuleOfFailureLidar = append(RuleOfFailureLidar, f)
+		} else if TopicOfFailureRadar == topic2 { // 8
+			f, ok := rule.(func(data *std_msgs.Bool) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *std_msgs.Bool) string):", err)
+				continue
+			}
+			RuleOfFailureRadar = append(RuleOfFailureRadar, f)
+		} else if TopicOfFailureCamera == topic2 { // 9
+			f, ok := rule.(func(data *std_msgs.Bool) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.JinlongControlCommand) string):", err)
+				continue
+			}
+			RuleOfFailureCamera = append(RuleOfFailureCamera, f)
 		} else {
 			c_log.GlobalLogger.Error("未知的topic：", topic2)
 			continue

aarch64/kinglong/master/package/service/produce_window.go

@@ -9,6 +9,8 @@ import (
 	"cicv-data-closedloop/common/util"
 	"cicv-data-closedloop/kinglong_msgs"
 	"github.com/bluenviron/goroslib/v2"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
+	"math"
 	"sync"
 	"time"
 )
@@ -20,7 +22,9 @@ var (
 	// /tpperception
 	mutexOfTpperception sync.RWMutex
 	// /pj_control_pub
-	mutexOfPjControlPub sync.RWMutex
+	mutexOfJinlongControlPub sync.RWMutex
+	// /data_read
+	mutexOfDataRead sync.RWMutex
 )
 
 // PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
@@ -33,7 +37,7 @@ func PrepareTimeWindowProducerQueue() {
 	subscribersTimeMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
 	subscribersMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
 	for i, topic := range commonConfig.SubscribeTopics {
-		// !!!扩展的定时任务监听，牛逼的设计!!!扩展性拉满啦
+		// !!!扩展的定时任务监听，牛逼!!!
 		if topic == masterConfig.TopicOfCicvExtend {
 			for {
 				time.Sleep(time.Duration(3500) * time.Millisecond)
@@ -48,6 +52,7 @@ func PrepareTimeWindowProducerQueue() {
 		}
 
 		c_log.GlobalLogger.Info("创建订阅者订阅话题：" + topic)
+
 		if topic == masterConfig.TopicOfCicvLocation && len(masterConfig.RuleOfCicvLocation) > 0 {
 			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
 				Node:  commonConfig.RosNode,
@@ -88,6 +93,21 @@ func PrepareTimeWindowProducerQueue() {
 				Node:  commonConfig.RosNode,
 				Topic: topic,
 				Callback: func(data *kinglong_msgs.PerceptionObjects) {
+					// 更新共享变量
+					mutexOfTpperception.RLock()
+					{
+						for _, obj := range data.Objs {
+							if (obj.Type != 1 && obj.Type != 0) || obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
+								continue
+							}
+							if _, ok := extendParam.ObjDicOfTpperception[obj.Id]; !ok {
+								extendParam.ObjDicOfTpperception[obj.Id] = []float32{}
+							}
+							extendParam.ObjDicOfTpperception[obj.Id] = append(extendParam.ObjDicOfTpperception[obj.Id], obj.Y)
+						}
+					}
+					mutexOfTpperception.RUnlock()
+
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
@@ -108,11 +128,24 @@ func PrepareTimeWindowProducerQueue() {
 			})
 		}
 
+		// 5
 		if topic == masterConfig.TopicOfDataRead {
 			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
 				Node:  commonConfig.RosNode,
 				Topic: topic,
 				Callback: func(data *kinglong_msgs.Retrieval) {
+					// 更新共享变量
+					mutexOfDataRead.RLock()
+					{
+						extendParam.NumCountDataReadOfDataRead++
+						if extendParam.NumCountDataReadOfDataRead == 10 {
+							extendParam.EgoSteeringRealOfDataRead = append(extendParam.EgoSteeringRealOfDataRead, data.StrgAngleRealValue)
+							extendParam.EgoThrottleRealOfDataRead = append(extendParam.EgoThrottleRealOfDataRead, data.VcuAccelPosValue)
+							extendParam.EgoBrakeRealOfDataRead = append(extendParam.EgoBrakeRealOfDataRead, data.VcuBrkPelPosValue)
+							extendParam.NumCountDataReadOfDataRead = 0
+						}
+					}
+					mutexOfDataRead.RUnlock()
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
@@ -134,6 +167,123 @@ func PrepareTimeWindowProducerQueue() {
 			})
 		}
 
+		// 6
+		if topic == masterConfig.TopicOfJinlongControlPub && len(masterConfig.RuleOfJinlongControlPub) > 0 {
+			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+				Node:  commonConfig.RosNode,
+				Topic: topic,
+				Callback: func(data *kinglong_msgs.JinlongControlCommand) {
+					// 更新共享变量
+					mutexOfJinlongControlPub.RLock()
+					{
+						extendParam.NumCountJinlongControlCommandOfPjControlPub++
+						if extendParam.NumCountJinlongControlCommandOfPjControlPub == 10 {
+							extendParam.EgoSteeringCmdOfJinlongControlPub = append(extendParam.EgoSteeringCmdOfJinlongControlPub, data.ASStrgAngleReq)
+							extendParam.EgoThrottleCmdOfJinlongControlPub = append(extendParam.EgoThrottleCmdOfJinlongControlPub, data.ASAutoDAccelPosReq)
+							extendParam.EgoBrakeCmdOfJinlongControlPub = append(extendParam.EgoBrakeCmdOfJinlongControlPub, data.ASAutoDBrkPelPosReq)
+							extendParam.NumCountJinlongControlCommandOfPjControlPub = 0
+						}
+					}
+					mutexOfJinlongControlPub.RUnlock()
+
+					subscribersTimeMutexes[i].Lock()
+					if time.Since(subscribersTimes[i]).Seconds() > 1 {
+						subscribersMutexes[i].Lock()
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
+						var faultLabel string
+						for _, f := range masterConfig.RuleOfJinlongControlPub {
+							faultLabel = f(data)
+							if faultLabel != "" {
+								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+								break
+							}
+						}
+						subscribersMutexes[i].Unlock()
+					}
+					subscribersTimeMutexes[i].Unlock()
+				},
+			})
+		}
+
+		// 7
+		if topic == masterConfig.TopicOfFailureLidar && len(masterConfig.RuleOfFailureLidar) > 0 {
+			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+				Node:  commonConfig.RosNode,
+				Topic: topic,
+				Callback: func(data *std_msgs.Bool) {
+					subscribersTimeMutexes[i].Lock()
+					if time.Since(subscribersTimes[i]).Seconds() > 1 {
+						subscribersMutexes[i].Lock()
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
+						var faultLabel string
+						for _, f := range masterConfig.RuleOfFailureLidar {
+							faultLabel = f(data)
+							if faultLabel != "" {
+								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+								break
+							}
+						}
+						subscribersMutexes[i].Unlock()
+					}
+					subscribersTimeMutexes[i].Unlock()
+				},
+			})
+		}
+
+		// 8
+		if topic == masterConfig.TopicOfFailureRadar && len(masterConfig.RuleOfFailureRadar) > 0 {
+			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+				Node:  commonConfig.RosNode,
+				Topic: topic,
+				Callback: func(data *std_msgs.Bool) {
+					subscribersTimeMutexes[i].Lock()
+					if time.Since(subscribersTimes[i]).Seconds() > 1 {
+						subscribersMutexes[i].Lock()
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
+						var faultLabel string
+						for _, f := range masterConfig.RuleOfFailureRadar {
+							faultLabel = f(data)
+							if faultLabel != "" {
+								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+								break
+							}
+						}
+						subscribersMutexes[i].Unlock()
+					}
+					subscribersTimeMutexes[i].Unlock()
+				},
+			})
+		}
+
+		// 9
+		if topic == masterConfig.TopicOfFailureCamera && len(masterConfig.RuleOfFailureLidar) > 0 {
+			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+				Node:  commonConfig.RosNode,
+				Topic: topic,
+				Callback: func(data *std_msgs.Bool) {
+					subscribersTimeMutexes[i].Lock()
+					if time.Since(subscribersTimes[i]).Seconds() > 1 {
+						subscribersMutexes[i].Lock()
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
+						var faultLabel string
+						for _, f := range masterConfig.RuleOfFailureLidar {
+							faultLabel = f(data)
+							if faultLabel != "" {
+								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+								break
+							}
+						}
+						subscribersMutexes[i].Unlock()
+					}
+					subscribersTimeMutexes[i].Unlock()
+				},
+			})
+		}
+
 		if err != nil {
 			c_log.GlobalLogger.Info("创建订阅者报错：", err)
 			//TODO 如何回传日志
@@ -141,180 +291,6 @@ func PrepareTimeWindowProducerQueue() {
 		}
 	}
 
-	////创建订阅者0订阅主题 nodefault_info
-	//c_log.GlobalLogger.Info("创建订阅者订阅话题 ", masterConfig.TopicOfNodeFaultInfo)
-	//subscriber0, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
-	//	Node:  commonConfig.RosNode,
-	//	Topic: masterConfig.TopicOfNodeFaultInfo,
-	//	Callback: func(data *kinglong_msgs.FaultInfo) {
-	//		if len(masterConfig.RuleOfNodefaultInfo) == 0 {
-	//			//c_log.GlobalLogger.Info("话题 nodefault_info没有触发器")
-	//			return
-	//		}
-	//		commonEntity.Subscriber0TimeMutex.Lock()
-	//		if time.Since(commonEntity.Subscriber0Time).Seconds() > 1 {
-	//			commonEntity.Subscriber0TimeMutex.Unlock()
-	//			subscriber0Mutex.Lock()
-	//			faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-	//			lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
-	//			var faultLabel string
-	//			for _, f := range masterConfig.RuleOfNodefaultInfo {
-	//				faultLabel = f(data)
-	//				if faultLabel != "" {
-	//					saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
-	//					commonEntity.Subscriber0Time = time.Now()
-	//					break
-	//				}
-	//			}
-	//			subscriber0Mutex.Unlock()
-	//		}
-	//		commonEntity.Subscriber0TimeMutex.Unlock()
-	//	}})
-	//if err != nil {
-	//	c_log.GlobalLogger.Info("创建订阅者发生故障：", err)
-	//	os.Exit(-1)
-	//}
-	//// 创建订阅者1订阅主题 cicv_location
-	//c_log.GlobalLogger.Info("创建订阅者订阅话题 ", masterConfig.TopicOfCicvLocation)
-	//subscriber1, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
-	//	Node:  commonConfig.RosNode,
-	//	Topic: masterConfig.TopicOfCicvLocation,
-	//	Callback: func(data *kinglong_msgs.PerceptionLocalization) {
-	//		m.RLock()
-	//		velocityX = data.VelocityX
-	//		velocityY = data.VelocityY
-	//		yaw = data.Yaw
-	//		m.RUnlock()
-	//
-	//		if len(masterConfig.RuleOfCicvLocation) == 0 {
-	//			c_log.GlobalLogger.Info("话题 cicv_location 没有触发器")
-	//			return
-	//		}
-	//		commonEntity.Subscriber1TimeMutex.Lock()
-	//		if time.Since(commonEntity.Subscriber1Time).Seconds() > 1 {
-	//			subscriber1Mutex.Lock()
-	//			faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-	//			lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
-	//			// 更新共享变量
-	//			var faultLabel string
-	//			for _, f := range masterConfig.RuleOfCicvLocation {
-	//				faultLabel = f(data)
-	//				if faultLabel != "" {
-	//					saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
-	//					commonEntity.Subscriber1Time = time.Now()
-	//					break
-	//				}
-	//			}
-	//			subscriber1Mutex.Unlock()
-	//		}
-	//		commonEntity.Subscriber1TimeMutex.Unlock()
-	//	},
-	//})
-	//if err != nil {
-	//	c_log.GlobalLogger.Info("创建订阅者发生故障：", err)
-	//	os.Exit(-1)
-	//}
-	//c_log.GlobalLogger.Info("创建订阅者订阅话题 ", masterConfig.TopicOfTpperception)
-	//subscriber2, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
-	//	Node:  commonConfig.RosNode,
-	//	Topic: masterConfig.TopicOfTpperception,
-	//	Callback: func(data *kinglong_msgs.PerceptionObjects) {
-	//		if len(masterConfig.RuleOfTpperception) == 0 {
-	//			c_log.GlobalLogger.Info("话题 tpperception 没有触发器")
-	//			return
-	//		}
-	//		commonEntity.Subscriber2TimeMutex.Lock()
-	//		// 判断是否是连续故障码
-	//		if time.Since(commonEntity.Subscriber2Time).Seconds() > 1 {
-	//			// 2 不是连续故障码
-	//			subscriber2Mutex.Lock()
-	//			faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-	//			lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
-	//			var faultLabel string
-	//			//c_log.GlobalLogger.Infof("TTC数据为：【velocityX】=%v，【velocityY】=%v", velocityX, velocityY)
-	//			for _, f := range masterConfig.RuleOfTpperception {
-	//				faultLabel = f(data, velocityX, velocityY, yaw)
-	//				if faultLabel != "" {
-	//					saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
-	//					commonEntity.Subscriber2Time = time.Now()
-	//					break
-	//				}
-	//			}
-	//			subscriber2Mutex.Unlock()
-	//		}
-	//		commonEntity.Subscriber2TimeMutex.Unlock()
-	//	}})
-	//if err != nil {
-	//	c_log.GlobalLogger.Info("创建订阅者2发生故障：", err)
-	//	os.Exit(-1)
-	//}
-	//// 创建订阅者3订阅主题 fault_info
-	//c_log.GlobalLogger.Info("创建订阅者3订阅话题 ", masterConfig.TopicOfFaultInfo)
-	//subscriber3, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
-	//	Node:  commonConfig.RosNode,
-	//	Topic: masterConfig.TopicOfFaultInfo,
-	//	Callback: func(data *kinglong_msgs.FaultVec) {
-	//		if len(masterConfig.RuleOfFaultInfo) == 0 {
-	//			c_log.GlobalLogger.Info("话题 fault_info 没有触发器")
-	//			return
-	//		}
-	//		commonEntity.Subscriber3TimeMutex.Lock()
-	//		if time.Since(commonEntity.Subscriber3Time).Seconds() > 1 {
-	//			// 2 不是连续故障码
-	//			subscriber3Mutex.Lock()
-	//			faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-	//			lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
-	//			var faultLabel string
-	//			for _, f := range masterConfig.RuleOfFaultInfo {
-	//				faultLabel = f(data)
-	//				if faultLabel != "" {
-	//					saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
-	//					commonEntity.Subscriber3Time = time.Now()
-	//					break
-	//				}
-	//			}
-	//			subscriber3Mutex.Unlock()
-	//		}
-	//		commonEntity.Subscriber3TimeMutex.Unlock()
-	//	}})
-	//if err != nil {
-	//	c_log.GlobalLogger.Info("创建订阅者3发生故障：", err)
-	//	os.Exit(-1)
-	//}
-	//// 创建订阅者4订阅主题 data_read
-	//// TODO 高频率触发
-	//c_log.GlobalLogger.Info("创建订阅者4订阅话题 ", masterConfig.TopicOfDataRead)
-	//subscriber4, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
-	//	Node:  commonConfig.RosNode,
-	//	Topic: masterConfig.TopicOfDataRead,
-	//	Callback: func(data *kinglong_msgs.Retrieval) {
-	//		if len(masterConfig.RuleOfDataRead) == 0 {
-	//			//c_log.GlobalLogger.Info("话题 data_read 没有触发器")
-	//			return
-	//		}
-	//		commonEntity.Subscriber4TimeMutex.Lock()
-	//		if time.Since(commonEntity.Subscriber4Time).Seconds() > 1 {
-	//			subscriber4Mutex.Lock()
-	//			faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-	//			lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
-	//			var faultLabel string
-	//			for _, f := range masterConfig.RuleOfDataRead {
-	//				faultLabel = f(data)
-	//				if faultLabel != "" {
-	//					saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
-	//					commonEntity.Subscriber4Time = time.Now()
-	//					break
-	//				}
-	//			}
-	//			subscriber4Mutex.Unlock()
-	//		}
-	//		commonEntity.Subscriber4TimeMutex.Unlock()
-	//	},
-	//})
-	//if err != nil {
-	//	c_log.GlobalLogger.Info("创建订阅者3发生故障：", err)
-	//	os.Exit(-1)
-	//}
 	select {
 	case signal := <-service.ChannelKillWindowProducer:
 		if signal == 1 {

aarch64/pjisuv/master/service/produce_window.go

@@ -29,6 +29,8 @@ var (
 	mutexOfTpperception sync.RWMutex
 	// /pj_control_pub
 	mutexOfPjControlPub sync.RWMutex
+	// /data_read
+	mutexOfDataRead sync.RWMutex
 )
 
 // PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
@@ -871,7 +873,7 @@ func PrepareTimeWindowProducerQueue() {
 					mutexOfTpperception.RLock()
 					{
 						for _, obj := range data.Objs {
-							if (obj.Type != 1 && obj.Type != 0) || obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
+							if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
 								continue
 							}
 							if _, ok := extendParam.ObjDicOfTpperception[obj.Id]; !ok {
@@ -1042,7 +1044,7 @@ func PrepareTimeWindowProducerQueue() {
 				Topic: topic,
 				Callback: func(data *pjisuv_msgs.Retrieval) {
 					// 更新共享变量
-					mutexOfPjControlPub.RLock()
+					mutexOfDataRead.RLock()
 					{
 						extendParam.NumCountDataReadOfDataRead++
 						if extendParam.NumCountDataReadOfDataRead == 10 {
@@ -1052,7 +1054,7 @@ func PrepareTimeWindowProducerQueue() {
 						}
 						extendParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
 					}
-					mutexOfPjControlPub.RUnlock()
+					mutexOfDataRead.RUnlock()
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()

common/entity/extend_param.go

@@ -32,12 +32,13 @@ type KinglongParam struct {
 	ObjDicOfTpperception map[uint32][]float32
 
 	// /pji_control_pub
-	NumCountPjiControlCommandOfPjControlPub int
-	EgoSteeringCmdOfPjControlPub            []float64
-	EgoThrottleCmdOfPjControlPub            []float64
+	NumCountJinlongControlCommandOfPjControlPub int
+	EgoSteeringCmdOfJinlongControlPub           []float64
+	EgoThrottleCmdOfJinlongControlPub           []float64
+	EgoBrakeCmdOfJinlongControlPub              []float64
 	// /data_read
-	NumCountDataReadOfDataRead   int
-	EgoSteeringRealOfDataRead    []float64
-	EgoThrottleRealOfDataRead    []float64
-	StrgAngleRealValueOfDataRead float64
+	NumCountDataReadOfDataRead int
+	EgoSteeringRealOfDataRead  []float64
+	EgoThrottleRealOfDataRead  []float64
+	EgoBrakeRealOfDataRead     []float64
 }

kinglong_msgs/control_msgs.go

@@ -0,0 +1,53 @@
+package kinglong_msgs
+
+import (
+	"github.com/bluenviron/goroslib/v2/pkg/msg"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
+)
+
+type JinlongControlCommand struct {
+	msg.Package                   `ros:"autoware_msgs"`
+	Header                        std_msgs.Header
+	ASStrgAngleReq                float64 `rosname:"AS_Strg_Angle_Req"`
+	ASStrgWorkModeReq             int16   `rosname:"AS_Strg_WorkMode_Req"`
+	ASStrg0Enable                 int16   `rosname:"AS_Strg0_Enable"`
+	ASSteeringTorqueSuperposition float64 `rosname:"AS_Steering_torque_superposition"`
+	ASStrgSpdReq                  float64 `rosname:"AS_Strg_Spd_Req"`
+	ASStrg1Enable                 int16   `rosname:"AS_Strg1_Enable"`
+	ASStrgLifeSignal              int16   `rosname:"AS_Strg_Life_Signal"`
+	ASAutoDReq                    int16   `rosname:"AS_AutoD_Req"`
+	ASLongitCtrlmode              int16   `rosname:"AS_Longit_Ctrlmode"`
+	ASAutoDEmergBrkRelease        int16   `rosname:"AS_AutoD_EmergBrk_Release"`
+	ASAutoDCollisionRelease       int16   `rosname:"AS_AutoD_Collision_Release"`
+	ASAutoDAccelPosReq            float64 `rosname:"AS_AutoD_Accel_Pos_Req"`
+	ASAutoDShiftReq               int16   `rosname:"AS_AutoD_Shift_Req"`
+	ASAutoDPShiftReq              int16   `rosname:"AS_AutoD_P_Shift_Req"`
+	ASAutoDBrkModeReq             int16   `rosname:"AS_AutoD_BrkMode_Req"`
+	ASAutoDBrkPelPosReq           float64 `rosname:"AS_AutoD_BrkPelPos_Req"`
+	ASAutoDSpdLimit               float64 `rosname:"AS_AutoD_Spd_Limit"`
+	ASAutoAccelerationReq         float64 `rosname:"AS_Auto_Acceleration_Req"`
+	ASAutoDLifeSignal             int16   `rosname:"AS_AutoD_Life_Signal"`
+	ASAutoDSpdReq                 float64 `rosname:"AS_AutoD_Spd_Req"`
+	ASSpdLifeSignal               int16   `rosname:"AS_Spd_Life_Signal"`
+	ASAlarmLampReq                int16   `rosname:"AS_AlarmLamp_Req"`
+	ASFrontDoorCtrlReq            int16   `rosname:"AS_Front_Door_Ctrl_Req"`
+	ASLowBeamStartReq             int16   `rosname:"AS_LowBeam_Start_Req"`
+	ASTurnLeftLightStartReq       int16   `rosname:"AS_TurnLeftLight_Start_Req"`
+	ASTurnRightLightStartReq      int16   `rosname:"AS_TurnRightLight_Start_Req"`
+	ASMiniLightStartReq           int16   `rosname:"AS_MiniLight_Start_Req"`
+	ASHornStartReq                int16   `rosname:"AS_Horn_Start_Req"`
+	ASTreadleOperatingReq         int16   `rosname:"AS_Treadle_Operating_Req"`
+	ASFDoorButtonLockReq          int16   `rosname:"AS_FDoorButtonLock_Req"`
+	ASMidDoorCtrlReq              int16   `rosname:"AS_Mid_Door_Ctrl_Req"`
+	ASMDoorButtonLockReq          int16   `rosname:"AS_MDoorButtonLock_Req"`
+	ASRearDoorCtrlReq             int16   `rosname:"AS_Rear_Door_Ctrl_Req"`
+	ASRDoorButtonLockReq          int16   `rosname:"AS_RDoorButtonLock_Req"`
+	ASFrontFogReq                 int16   `rosname:"AS_FrontFog_Req"`
+	ASRearFogReq                  int16   `rosname:"AS_RearFog_Req"`
+	ASWarninglightReq             int16   `rosname:"AS_Warninglight_Req"`
+	ASHighBeamStartReq            int16   `rosname:"AS_HighBeam_Start_Req"`
+	TargetX                       float64
+	TargetY                       float64
+	Velocity                      float64
+	AutoMode                      int16
+}

resource/cut_trigger_jinlong.py

@@ -183,8 +183,6 @@ def final_callback(event):
             if len(obj_value) <= 5:
                 continue
             cutin_flag=if_cutin(obj_value)
-            if cutin_flag  :
-                continue                
 
             
         # 再判断前车是否切出     来自 /tpperception
@@ -192,9 +190,7 @@ def final_callback(event):
             if len(obj_value) <= 5:
                 continue
             cutout_flag=if_cutout(obj_value)
-            if cutout_flag:
-                continue  
-            
+
         with difference_lock:    
             if cutin_flag:
                 print('The cutting-in behavior of the front car was detected')

trigger/kinglong/cicv_extend/cutin_difference/main/cutin_difference.go

@@ -0,0 +1,101 @@
+package main
+
+import (
+	"cicv-data-closedloop/common/entity"
+	"math"
+	"sort"
+)
+
+func Topic() string {
+	return "/cicv_extend"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "brake"
+}
+
+func Rule(param entity.KinglongParam) string {
+	for _, objValue := range param.ObjDicOfTpperception {
+		if len(objValue) <= 5 || !isCuttingIn(objValue, param) || !isDifference(param) {
+			continue
+		}
+		return "cutin_difference"
+	}
+	return ""
+}
+
+func isCuttingIn(objYList []float32, param entity.KinglongParam) bool {
+	for i, objY := range objYList {
+		if math.Abs(float64(objY)) >= 1.3 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
+			for j := 0; j < len(objYList)-i-1; j++ {
+				if math.Abs(float64(objYList[1+i+j])) <= 0.7 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+
+// 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
+func isDifference(param entity.KinglongParam) bool {
+	_, pValueSteering := mannWhitneyUTest(param.EgoSteeringRealOfDataRead, param.EgoSteeringCmdOfJinlongControlPub)
+	_, pValueThrottle := mannWhitneyUTest(param.EgoThrottleRealOfDataRead, param.EgoThrottleRealOfDataRead)
+	_, pValueBrake := mannWhitneyUTest(param.EgoBrakeRealOfDataRead, param.EgoBrakeCmdOfJinlongControlPub)
+	if pValueSteering < 0.05 || pValueThrottle < 0.05 || pValueBrake < 0.05 {
+		return true
+	}
+	return false
+}
+
+func mannWhitneyUTest(group1, group2 []float64) (float64, float64) {
+	// 统计样本大小
+	n1 := float64(len(group1))
+	n2 := float64(len(group2))
+
+	// 合并样本并排序
+	combined := append(group1, group2...)
+	sort.Float64s(combined)
+
+	// 计算秩和
+	var rankSum1, rankSum2 float64
+	for i := range combined {
+		if i < len(group1) {
+			rankSum1 += float64(i + 1)
+		} else {
+			rankSum2 += float64(i + 1)
+		}
+	}
+
+	// 计算 U 统计量
+	u1 := rankSum1 - (n1 * (n1 + 1) / 2)
+	u2 := rankSum2 - (n2 * (n2 + 1) / 2)
+	u := math.Min(u1, u2)
+
+	// 计算 p 值
+	meanU := (n1 * n2 / 2)
+	stdDevU := math.Sqrt((n1 * n2 * (n1 + n2 + 1)) / 12)
+	z := (u - meanU) / stdDevU
+	p := 2 * (1 - normalCDF(z))
+
+	return u, p
+}
+
+func normalCDF(x float64) float64 {
+	const (
+		b1 = 0.319381530
+		b2 = -0.356563782
+		b3 = 1.781477937
+		b4 = -1.821255978
+		b5 = 1.330274429
+		p  = 0.2316419
+		c  = 0.39894228
+	)
+	if x >= 0.0 {
+		t := 1.0 / (1.0 + p*x)
+		return 1.0 - c*t*(t*(t*(t*(t*b5+b4)+b3)+b2)+b1)
+	}
+	t := 1.0 / (1.0 - p*x)
+	return c * t * (t*(t*(t*(t*b5+b4)+b3)+b2) + b1)
+}

trigger/kinglong/cicv_extend/cutout_difference/main/cutout_difference.go

@@ -0,0 +1,101 @@
+package main
+
+import (
+	"cicv-data-closedloop/common/entity"
+	"math"
+	"sort"
+)
+
+func Topic() string {
+	return "/cicv_extend"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "cutout_difference"
+}
+
+func Rule(param entity.KinglongParam) string {
+	for _, objValue := range param.ObjDicOfTpperception {
+		if len(objValue) <= 5 || !isCuttingOut(objValue, param) || !isDifference(param) {
+			continue
+		}
+		return "cutout_difference"
+	}
+	return ""
+}
+
+func isCuttingOut(objYList []float32, param entity.KinglongParam) bool {
+	for i, objY := range objYList {
+		if math.Abs(float64(objY)) <= 0.6 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
+			for j := 0; j < len(objYList)-i-1; j++ {
+				if math.Abs(float64(objYList[1+i+j])) >= 1.2 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+
+// 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
+func isDifference(param entity.KinglongParam) bool {
+	_, pValueSteering := mannWhitneyUTest(param.EgoSteeringRealOfDataRead, param.EgoSteeringCmdOfJinlongControlPub)
+	_, pValueThrottle := mannWhitneyUTest(param.EgoThrottleRealOfDataRead, param.EgoThrottleRealOfDataRead)
+	_, pValueBrake := mannWhitneyUTest(param.EgoBrakeRealOfDataRead, param.EgoBrakeCmdOfJinlongControlPub)
+	if pValueSteering < 0.05 || pValueThrottle < 0.05 || pValueBrake < 0.05 {
+		return true
+	}
+	return false
+}
+
+func mannWhitneyUTest(group1, group2 []float64) (float64, float64) {
+	// 统计样本大小
+	n1 := float64(len(group1))
+	n2 := float64(len(group2))
+
+	// 合并样本并排序
+	combined := append(group1, group2...)
+	sort.Float64s(combined)
+
+	// 计算秩和
+	var rankSum1, rankSum2 float64
+	for i := range combined {
+		if i < len(group1) {
+			rankSum1 += float64(i + 1)
+		} else {
+			rankSum2 += float64(i + 1)
+		}
+	}
+
+	// 计算 U 统计量
+	u1 := rankSum1 - (n1 * (n1 + 1) / 2)
+	u2 := rankSum2 - (n2 * (n2 + 1) / 2)
+	u := math.Min(u1, u2)
+
+	// 计算 p 值
+	meanU := n1 * n2 / 2
+	stdDevU := math.Sqrt((n1 * n2 * (n1 + n2 + 1)) / 12)
+	z := (u - meanU) / stdDevU
+	p := 2 * (1 - normalCDF(z))
+
+	return u, p
+}
+
+func normalCDF(x float64) float64 {
+	const (
+		b1 = 0.319381530
+		b2 = -0.356563782
+		b3 = 1.781477937
+		b4 = -1.821255978
+		b5 = 1.330274429
+		p  = 0.2316419
+		c  = 0.39894228
+	)
+	if x >= 0.0 {
+		t := 1.0 / (1.0 + p*x)
+		return 1.0 - c*t*(t*(t*(t*(t*b5+b4)+b3)+b2)+b1)
+	}
+	t := 1.0 / (1.0 - p*x)
+	return c * t * (t*(t*(t*(t*b5+b4)+b3)+b2) + b1)
+}

trigger/kinglong/failure/camera/main/camera.go

@@ -0,0 +1,25 @@
+package main
+
+import (
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
+)
+
+var (
+	topic = "/failure/camera"
+	label = "failurecamera"
+)
+
+func Topic() string {
+	return "/failure/camera"
+}
+
+func Label() string {
+	return label
+}
+
+func Rule(data *std_msgs.Bool) string {
+	if data.Data {
+		return label
+	}
+	return ""
+}

trigger/kinglong/failure/lidar/main/lidar.go

@@ -0,0 +1,25 @@
+package main
+
+import (
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
+)
+
+var (
+	topic = "/failure/lidar"
+	label = "failurelidar"
+)
+
+func Topic() string {
+	return "/failure/lidar"
+}
+
+func Label() string {
+	return label
+}
+
+func Rule(data *std_msgs.Bool) string {
+	if data.Data {
+		return label
+	}
+	return ""
+}

trigger/kinglong/failure/radar/main/radar.go

@@ -0,0 +1,25 @@
+package main
+
+import (
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
+)
+
+var (
+	topic = "/failure/radar"
+	label = "failureradar"
+)
+
+func Topic() string {
+	return "/failure/radar"
+}
+
+func Label() string {
+	return label
+}
+
+func Rule(data *std_msgs.Bool) string {
+	if data.Data {
+		return label
+	}
+	return ""
+}