commit

aarch64/pjisuv/master/config/master_trigger_config.go

@@ -3,6 +3,7 @@ package config
 import (
 	"cicv-data-closedloop/aarch64/pjisuv/common/config"
 	"cicv-data-closedloop/common/config/c_log"
+	"cicv-data-closedloop/common/entity"
 	"cicv-data-closedloop/common/util"
 	"cicv-data-closedloop/pjisuv_msgs"
 	"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
@@ -16,6 +17,10 @@ import (
 )
 
 var (
+	// 扩展
+	TopicOfCicvExtend = "/cicv_extend"
+	RuleOfCicvExtend  []func(param entity.ExtendParam) string
+
 	//1
 	TopicOfAmrPose = "/amr_pose"
 	RuleOfAmrPose  []func(data *visualization_msgs.MarkerArray) string
@@ -134,7 +139,7 @@ var (
 
 	//30
 	TopicOfTpperception = "/tpperception"
-	RuleOfTpperception  []func(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string
+	RuleOfTpperception  []func(data *pjisuv_msgs.PerceptionObjects, extendParam entity.ExtendParam) string
 
 	//31
 	TopicOfTpperceptionVis = "/tpperception/vis"
@@ -203,7 +208,15 @@ func InitTriggerConfig() {
 			c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Rule方法失败。", err)
 			continue
 		}
-		if TopicOfAmrPose == topic2 { //1
+
+		if TopicOfCicvExtend == topic2 { // 自定义扩展
+			f, ok := rule.(func(param entity.ExtendParam) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(param entity.ExtendParam):", err)
+				continue
+			}
+			RuleOfCicvExtend = append(RuleOfCicvExtend, f)
+		} else if TopicOfAmrPose == topic2 { //1
 			f, ok := rule.(func(data *visualization_msgs.MarkerArray) string)
 			if ok != true {
 				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *visualization_msgs.MarkerArray) string):", err)
@@ -407,9 +420,9 @@ func InitTriggerConfig() {
 			}
 			RuleOfTf = append(RuleOfTf, f)
 		} else if TopicOfTpperception == topic2 { //30
-			f, ok := rule.(func(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string)
+			f, ok := rule.(func(data *pjisuv_msgs.PerceptionObjects, param entity.ExtendParam) string)
 			if ok != true {
-				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string):", err)
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.PerceptionObjects, param entity.ExtendParam) string):", err)
 				continue
 			}
 			RuleOfTpperception = append(RuleOfTpperception, f)

aarch64/pjisuv/master/service/produce_window.go

@@ -5,7 +5,7 @@ import (
 	"cicv-data-closedloop/aarch64/pjisuv/common/service"
 	masterConfig "cicv-data-closedloop/aarch64/pjisuv/master/config"
 	"cicv-data-closedloop/common/config/c_log"
-	"cicv-data-closedloop/common/entity"
+	commonEntity "cicv-data-closedloop/common/entity"
 	"cicv-data-closedloop/common/util"
 	"cicv-data-closedloop/pjisuv_msgs"
 	"github.com/bluenviron/goroslib/v2"
@@ -14,15 +14,21 @@ import (
 	"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"
 )
 
+// 所有共享变量
+// todo 如果触发器订阅了多个topic，需要定制化缓存指定字段；也可以将所有topic的所有字段值实时缓存，工作量太大，暂时搁置
 var (
-	m         sync.RWMutex
-	velocityX float64
-	velocityY float64
-	yaw       float64
+	extendParam commonEntity.ExtendParam
+	// /cicv_location
+	mutexOfCicvLocation sync.RWMutex
+	// /tpperception
+	mutexOfTpperception sync.RWMutex
+	// /pj_control_pub
+	mutexOfPjControlPub sync.RWMutex
 )
 
 // PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
@@ -35,6 +41,21 @@ 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)
+				for _, f := range masterConfig.RuleOfCicvExtend {
+					label := f(extendParam)
+					if label != "" {
+						saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow())
+						break
+					}
+				}
+			}
+		}
+
+		// 其他常规监听器
 		c_log.GlobalLogger.Info("创建订阅者订阅话题：" + topic)
 
 		// 1
@@ -46,8 +67,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfAmrPose {
 							faultLabel = f(data)
@@ -73,8 +94,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfBoundingBoxesFast {
 							faultLabel = f(data)
@@ -100,8 +121,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCameraFault {
 							faultLabel = f(data)
@@ -127,8 +148,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCanData {
 							faultLabel = f(data)
@@ -154,8 +175,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud {
 							faultLabel = f(data)
@@ -181,8 +202,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud {
 							faultLabel = f(data)
@@ -208,8 +229,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCh64wLScan {
 							faultLabel = f(data)
@@ -235,8 +256,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud {
 							faultLabel = f(data)
@@ -262,8 +283,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCh64wRScan {
 							faultLabel = f(data)
@@ -289,8 +310,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects {
 							faultLabel = f(data)
@@ -316,8 +337,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCicvAmrTrajectory {
 							faultLabel = f(data)
@@ -341,17 +362,20 @@ func PrepareTimeWindowProducerQueue() {
 				Topic: topic,
 				Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
 					// 更新共享变量
-					m.RLock()
-					velocityX = data.VelocityX
-					velocityY = data.VelocityY
-					yaw = data.Yaw
-					m.RUnlock()
+					mutexOfCicvLocation.RLock()
+					{
+						extendParam.VelocityYOfCicvLocation = data.VelocityX
+						extendParam.VelocityYOfCicvLocation = data.VelocityY
+						extendParam.YawOfCicvLocation = data.Yaw
+						extendParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
+					}
+					mutexOfCicvLocation.RUnlock()
 
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCicvLocation {
 							faultLabel = f(data)
@@ -376,8 +400,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCloudClusters {
 							faultLabel = f(data)
@@ -403,8 +427,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfHeartbeatInfo {
 							faultLabel = f(data)
@@ -430,8 +454,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfLidarPretreatmentCost {
 							faultLabel = f(data)
@@ -457,8 +481,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry {
 							faultLabel = f(data)
@@ -484,8 +508,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfLidarRoi {
 							faultLabel = f(data)
@@ -511,8 +535,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfLine1 {
 							faultLabel = f(data)
@@ -538,8 +562,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfLine2 {
 							faultLabel = f(data)
@@ -565,8 +589,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfMapPolygon {
 							faultLabel = f(data)
@@ -592,8 +616,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfObstacleDisplay {
 							faultLabel = f(data)
@@ -616,11 +640,22 @@ func PrepareTimeWindowProducerQueue() {
 				Node:  commonConfig.RosNode,
 				Topic: topic,
 				Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
+					// 更新共享变量
+					mutexOfPjControlPub.RLock()
+					{
+						extendParam.NumCountPjiControlCommandOfPjControlPub++
+						if extendParam.NumCountPjiControlCommandOfPjControlPub == 10 {
+							extendParam.EgoSteeringCmdOfPjControlPub = append(extendParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
+							extendParam.EgoThrottleCmdOfPjControlPub = append(extendParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
+							extendParam.NumCountPjiControlCommandOfPjControlPub = 0
+						}
+					}
+					mutexOfPjControlPub.RUnlock()
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfPjControlPub {
 							faultLabel = f(data)
@@ -646,8 +681,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfPointsCluster {
 							faultLabel = f(data)
@@ -673,8 +708,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfPointsConcat {
 							faultLabel = f(data)
@@ -700,8 +735,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfReferenceDisplay {
 							faultLabel = f(data)
@@ -727,8 +762,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfReferenceTrajectory {
 							faultLabel = f(data)
@@ -754,8 +789,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfRoiPoints {
 							faultLabel = f(data)
@@ -781,8 +816,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfRoiPolygon {
 							faultLabel = f(data)
@@ -808,8 +843,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfTf {
 							faultLabel = f(data)
@@ -832,14 +867,28 @@ func PrepareTimeWindowProducerQueue() {
 				Node:  commonConfig.RosNode,
 				Topic: topic,
 				Callback: func(data *pjisuv_msgs.PerceptionObjects) {
+					// 更新共享变量
+					mutexOfTpperception.RLock()
+					{
+						for _, obj := range data.Objs {
+							if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
+								continue
+							}
+							if _, 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()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfTpperception {
-							faultLabel = f(data, velocityX, velocityY, yaw)
+							faultLabel = f(data, extendParam)
 							if faultLabel != "" {
 								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
 								break
@@ -861,8 +910,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfTpperceptionVis {
 							faultLabel = f(data)
@@ -888,8 +937,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfTprouteplan {
 							faultLabel = f(data)
@@ -915,8 +964,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfTrajectoryDisplay {
 							faultLabel = f(data)
@@ -942,8 +991,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfUngroundCloudpoints {
 							faultLabel = f(data)
@@ -969,8 +1018,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCameraImage {
 							faultLabel = f(data)
@@ -992,11 +1041,23 @@ func PrepareTimeWindowProducerQueue() {
 				Node:  commonConfig.RosNode,
 				Topic: topic,
 				Callback: func(data *pjisuv_msgs.Retrieval) {
+					// 更新共享变量
+					mutexOfPjControlPub.RLock()
+					{
+						extendParam.NumCountDataReadOfDataRead++
+						if extendParam.NumCountDataReadOfDataRead == 10 {
+							extendParam.EgoSteeringRealOfDataRead = append(extendParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
+							extendParam.EgoThrottleRealOfDataRead = append(extendParam.EgoThrottleRealOfDataRead, data.AccPed2)
+							extendParam.NumCountDataReadOfDataRead = 0
+						}
+						extendParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
+					}
+					mutexOfPjControlPub.RUnlock()
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfDataRead {
 							faultLabel = f(data)
@@ -1021,8 +1082,8 @@ func PrepareTimeWindowProducerQueue() {
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
-						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
-						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfPjiGps {
 							faultLabel = f(data)
@@ -1056,11 +1117,11 @@ func PrepareTimeWindowProducerQueue() {
 	}
 }
 
-func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *entity.TimeWindow) {
+func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *commonEntity.TimeWindow) {
 	masterTopics, slaveTopics := getTopicsOfNode(faultLabel)
 	if lastTimeWindow == nil || util.TimeCustom1GreaterTimeCustom2(faultHappenTime, lastTimeWindow.TimeWindowEnd) {
 		// 2-1 如果是不在旧故障窗口内，添加一个新窗口
-		newTimeWindow := entity.TimeWindow{
+		newTimeWindow := commonEntity.TimeWindow{
 			FaultTime:       faultHappenTime,
 			TimeWindowBegin: util.TimeCustomChange(faultHappenTime, -commonConfig.PlatformConfig.TaskBeforeTime),
 			TimeWindowEnd:   util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime),
@@ -1071,11 +1132,11 @@ func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *e
 		}
 		c_log.GlobalLogger.Infof("不在旧故障窗口内，向生产者队列添加一个新窗口，【Lable】=%v，【FaultTime】=%v，【Length】=%v", newTimeWindow.Labels, newTimeWindow.FaultTime, newTimeWindow.Length)
 
-		entity.AddTimeWindowToTimeWindowProducerQueue(newTimeWindow)
+		commonEntity.AddTimeWindowToTimeWindowProducerQueue(newTimeWindow)
 	} else {
 		// 2-2 如果在旧故障窗口内
-		entity.TimeWindowProducerQueueMutex.RLock()
-		defer entity.TimeWindowProducerQueueMutex.RUnlock()
+		commonEntity.TimeWindowProducerQueueMutex.RLock()
+		defer commonEntity.TimeWindowProducerQueueMutex.RUnlock()
 		// 2-2-1 更新故障窗口end时间
 		maxEnd := util.TimeCustomChange(lastTimeWindow.TimeWindowBegin, commonConfig.PlatformConfig.TaskMaxTime)
 		expectEnd := util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime)

common/entity/extend_param.go

@@ -0,0 +1,22 @@
+package entity
+
+type ExtendParam struct {
+	// /cicv_location
+	VelocityXOfCicvLocation        float64
+	VelocityYOfCicvLocation        float64
+	YawOfCicvLocation              float64
+	AngularVelocityZOfCicvLocation float64
+
+	// /tpperception
+	ObjDicOfTpperception map[uint32][]float32
+
+	// /pji_control_pub
+	NumCountPjiControlCommandOfPjControlPub int
+	EgoSteeringCmdOfPjControlPub            []float64
+	EgoThrottleCmdOfPjControlPub            []float64
+	// /data_read
+	NumCountDataReadOfDataRead   int
+	EgoSteeringRealOfDataRead    []float64
+	EgoThrottleRealOfDataRead    []float64
+	StrgAngleRealValueOfDataRead float64
+}

go.mod

@@ -39,6 +39,12 @@ require (
 	github.com/goccy/go-json v0.10.2 // indirect
 	github.com/golang/mock v1.6.0 // indirect
 	github.com/golang/protobuf v1.5.3 // indirect
+	github.com/gonum/blas v0.0.0-20181208220705-f22b278b28ac // indirect
+	github.com/gonum/floats v0.0.0-20181209220543-c233463c7e82 // indirect
+	github.com/gonum/internal v0.0.0-20181124074243-f884aa714029 // indirect
+	github.com/gonum/lapack v0.0.0-20181123203213-e4cdc5a0bff9 // indirect
+	github.com/gonum/matrix v0.0.0-20181209220409-c518dec07be9 // indirect
+	github.com/gonum/stat v0.0.0-20181125101827-41a0da705a5b // indirect
 	github.com/google/go-cmp v0.6.0 // indirect
 	github.com/gookit/color v1.5.4 // indirect
 	github.com/jmespath/go-jmespath v0.0.0-20180206201540-c2b33e8439af // indirect
@@ -74,6 +80,7 @@ require (
 	golang.org/x/sys v0.16.0 // indirect
 	golang.org/x/text v0.14.0 // indirect
 	golang.org/x/time v0.4.0 // indirect
+	gonum.org/v1/gonum v0.15.0 // indirect
 	google.golang.org/genproto v0.0.0-20230410155749-daa745c078e1 // indirect
 	google.golang.org/grpc v1.56.3 // indirect
 	google.golang.org/protobuf v1.32.0 // indirect

go.sum

@@ -158,6 +158,18 @@ github.com/golang/protobuf v1.5.0/go.mod h1:FsONVRAS9T7sI+LIUmWTfcYkHO4aIWwzhcaS
 github.com/golang/protobuf v1.5.2/go.mod h1:XVQd3VNwM+JqD3oG2Ue2ip4fOMUkwXdXDdiuN0vRsmY=
 github.com/golang/protobuf v1.5.3 h1:KhyjKVUg7Usr/dYsdSqoFveMYd5ko72D+zANwlG1mmg=
 github.com/golang/protobuf v1.5.3/go.mod h1:XVQd3VNwM+JqD3oG2Ue2ip4fOMUkwXdXDdiuN0vRsmY=
+github.com/gonum/blas v0.0.0-20181208220705-f22b278b28ac h1:Q0Jsdxl5jbxouNs1TQYt0gxesYMU4VXRbsTlgDloZ50=
+github.com/gonum/blas v0.0.0-20181208220705-f22b278b28ac/go.mod h1:P32wAyui1PQ58Oce/KYkOqQv8cVw1zAapXOl+dRFGbc=
+github.com/gonum/floats v0.0.0-20181209220543-c233463c7e82 h1:EvokxLQsaaQjcWVWSV38221VAK7qc2zhaO17bKys/18=
+github.com/gonum/floats v0.0.0-20181209220543-c233463c7e82/go.mod h1:PxC8OnwL11+aosOB5+iEPoV3picfs8tUpkVd0pDo+Kg=
+github.com/gonum/internal v0.0.0-20181124074243-f884aa714029 h1:8jtTdc+Nfj9AR+0soOeia9UZSvYBvETVHZrugUowJ7M=
+github.com/gonum/internal v0.0.0-20181124074243-f884aa714029/go.mod h1:Pu4dmpkhSyOzRwuXkOgAvijx4o+4YMUJJo9OvPYMkks=
+github.com/gonum/lapack v0.0.0-20181123203213-e4cdc5a0bff9 h1:7qnwS9+oeSiOIsiUMajT+0R7HR6hw5NegnKPmn/94oI=
+github.com/gonum/lapack v0.0.0-20181123203213-e4cdc5a0bff9/go.mod h1:XA3DeT6rxh2EAE789SSiSJNqxPaC0aE9J8NTOI0Jo/A=
+github.com/gonum/matrix v0.0.0-20181209220409-c518dec07be9 h1:V2IgdyerlBa/MxaEFRbV5juy/C3MGdj4ePi+g6ePIp4=
+github.com/gonum/matrix v0.0.0-20181209220409-c518dec07be9/go.mod h1:0EXg4mc1CNP0HCqCz+K4ts155PXIlUywf0wqN+GfPZw=
+github.com/gonum/stat v0.0.0-20181125101827-41a0da705a5b h1:fbskpz/cPqWH8VqkQ7LJghFkl2KPAiIFUHrTJ2O3RGk=
+github.com/gonum/stat v0.0.0-20181125101827-41a0da705a5b/go.mod h1:Z4GIJBJO3Wa4gD4vbwQxXXZ+WHmW6E9ixmNrwvs0iZs=
 github.com/google/btree v0.0.0-20180813153112-4030bb1f1f0c/go.mod h1:lNA+9X1NB3Zf8V7Ke586lFgjr2dZNuvo3lPJSGZ5JPQ=
 github.com/google/btree v1.0.0/go.mod h1:lNA+9X1NB3Zf8V7Ke586lFgjr2dZNuvo3lPJSGZ5JPQ=
 github.com/google/go-cmp v0.2.0/go.mod h1:oXzfMopK8JAjlY9xF4vHSVASa0yLyX7SntLO5aqRK0M=
@@ -526,6 +538,8 @@ golang.org/x/xerrors v0.0.0-20190717185122-a985d3407aa7/go.mod h1:I/5z698sn9Ka8T
 golang.org/x/xerrors v0.0.0-20191011141410-1b5146add898/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0=
 golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0=
 golang.org/x/xerrors v0.0.0-20200804184101-5ec99f83aff1/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0=
+gonum.org/v1/gonum v0.15.0 h1:2lYxjRbTYyxkJxlhC+LvJIx3SsANPdRybu1tGj9/OrQ=
+gonum.org/v1/gonum v0.15.0/go.mod h1:xzZVBJBtS+Mz4q0Yl2LJTk+OxOg4jiXZ7qBoM0uISGo=
 google.golang.org/api v0.4.0/go.mod h1:8k5glujaEP+g9n7WNsDg8QP6cUVNI86fCNMcbazEtwE=
 google.golang.org/api v0.7.0/go.mod h1:WtwebWUNSVBH/HAw79HIFXZNqEvBhG+Ra+ax0hx3E3M=
 google.golang.org/api v0.8.0/go.mod h1:o4eAsZoiT+ibD93RtjEohWalFOjRDx6CVaqeizhEnKg=

readme.md

@@ -3,6 +3,7 @@
 1.22.0
 
 # oss 配置
+
 ```
 cname：http://open-bucket.oss.icvdc.com
 内网endpoint: oss-cn-beijing-gqzl-d01-a.ops.gqzl-cloud.com
@@ -14,16 +15,31 @@ secret：xZ2Fozoarpfw0z28FUhtg8cu0yDc5d
 ```
 
 # 编译
+
 ## 一、进入容器
+
 docker exec -it aarch64ubuntu18 /bin/bash
+
 ## 二、拉取代码
+
 cd /root/cicv-data-closedloop
 git pull
+
 ## 三、编译
+
 sh build.sh
+
 ## 四、宿主机复制
+
 cd /root/mlx/
 docker cp 23e9164d44d7:/root/cicv-data-closedloop ./
 
 # 启动 arm64 容器
-sudo docker run -itd -p 12340:12340 -p 9849:22 --rm --name aarch64ubuntu18 --platform linux/arm64/v8 rosbag-handle-compile:v20240117.4 /bin/bash
+
+sudo docker run -itd -p 12340:12340 -p 9849:22 --rm --name aarch64ubuntu18 --platform linux/arm64/v8
+rosbag-handle-compile:v20240117.4 /bin/bash
+
+# 项目亮点
+
+1. 时间窗口设计
+2. 扩展定时任务监听设计

resource/cut_trigger_jinlong.py

@@ -78,7 +78,7 @@ def callback_data_read(data):
     if num_count_data_read == 10:
         ego_Steering_real.append(data.Strg_Angle_Real_Value)
         ego_throttle_real.append(data.VCU_Accel_Pos_Value)
-        ego_brake_real.append(data.VCU_Brkpel_Pos_Value)
+        ego_brake_real.append(data.VCU_BrkPel_Pos_Value)
         num_count_data_read=0 
     
     #Strg_Angle_Real_Value=data.ActStrWhAng     
@@ -187,7 +187,7 @@ def final_callback(event):
                 continue                
 
             
-        # 再判断前车是否切出    
+        # 再判断前车是否切出     来自 /tpperception
         for obj_id, obj_value in obj_dic.items():           
             if len(obj_value) <= 5:
                 continue

resource/cut_trigger_pji.py

@@ -161,8 +161,6 @@ def final_callback(event):
             if len(obj_value) <= 5:
                 continue
             cutin_flag=if_cutin(obj_value)
-            if cutin_flag  :
-                continue                
 
             
         # 再判断前车是否切出    
@@ -170,9 +168,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')
@@ -216,7 +212,7 @@ def final_callback(event):
         
 def listener():
     rospy.init_node('listener', anonymous=True)
-    rospy.Subscriber("/cicv_location",PerceptionLocalization,callback_cicv_location)
+    rospy.Subscriber("/cicv_location",PerceptionLocalization,callback_cicv_location) # done
     rospy.Subscriber("/tpperception",PerceptionObjects,callback_tpperception)
     rospy.Subscriber("/pj_control_pub",VehicleCmd,callback_pji_Control_Command)
     rospy.Subscriber("/data_read",Retrieval,callback_data_read)

trigger/pjisuv/cicv_extend/cutin_difference/main/cutin_difference.go

@@ -0,0 +1,100 @@
+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.ExtendParam) 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.ExtendParam) bool {
+	for i, objY := range objYList {
+		if math.Abs(float64(objY)) >= 1.1 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
+			for j := 0; j < len(objYList)-i-1; j++ {
+				if math.Abs(float64(objYList[1+i+j])) <= 0.6 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+
+// 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
+func isDifference(param entity.ExtendParam) bool {
+	_, pValueSteering := mannWhitneyUTest(param.EgoSteeringRealOfDataRead, param.EgoSteeringCmdOfPjControlPub)
+	_, pValueThrottle := mannWhitneyUTest(param.EgoThrottleRealOfDataRead, param.EgoThrottleCmdOfPjControlPub)
+	if pValueSteering < 0.05 || pValueThrottle < 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/pjisuv/cicv_extend/cutout_difference/main/cutout_difference.go

@@ -0,0 +1,100 @@
+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.ExtendParam) 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.ExtendParam) 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.ExtendParam) bool {
+	_, pValueSteering := mannWhitneyUTest(param.EgoSteeringRealOfDataRead, param.EgoSteeringCmdOfPjControlPub)
+	_, pValueThrottle := mannWhitneyUTest(param.EgoThrottleRealOfDataRead, param.EgoThrottleCmdOfPjControlPub)
+	if pValueSteering < 0.05 || pValueThrottle < 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/pjisuv/tpperception/TTC/main/TTC.go

@@ -1,6 +1,7 @@
 package main
 
 import (
+	"cicv-data-closedloop/common/entity"
 	"cicv-data-closedloop/pjisuv_msgs"
 	"math"
 )
@@ -14,11 +15,11 @@ func Label() string {
 	return "TTC"
 }
 
-func Rule(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string {
+func Rule(data *pjisuv_msgs.PerceptionObjects, param entity.ExtendParam) string {
 	for _, obj := range data.Objs {
 		if math.Abs(float64(obj.Y)) <= 2 && obj.X >= 8 {
-			theta := DegreesToRadians(yaw)
-			vxrel := (float64(obj.Vxabs)-velocityX)*math.Cos(theta) + (float64(obj.Vyabs)-velocityY)*math.Sin(theta)
+			theta := DegreesToRadians(param.YawOfCicvLocation)
+			vxrel := (float64(obj.Vxabs)-param.VelocityXOfCicvLocation)*math.Cos(theta) + (float64(obj.Vyabs)-param.VelocityYOfCicvLocation)*math.Sin(theta)
 			ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
 			//fmt.Println("TTC值为：", ttc)
 			if ttc >= 0 && ttc <= 5 {