commit

aarch64/pjisuv/master/config/master_trigger_config.go

@@ -63,7 +63,7 @@ var (
 
 	//11
 	TopicOfCicvAmrTrajectory = "/cicv_amr_trajectory"
-	RuleOfCicvAmrTrajectory  []func(data *pjisuv_msgs.Trajectory) string
+	RuleOfCicvAmrTrajectory  []func(data *pjisuv_msgs.Trajectory, param entity.PjisuvParam) string
 
 	//12
 	TopicOfCicvLocation = "/cicv_location"
@@ -295,9 +295,9 @@ func InitTriggerConfig() {
 			}
 			RuleOfCicvLidarclusterMovingObjects = append(RuleOfCicvLidarclusterMovingObjects, f)
 		} else if TopicOfCicvAmrTrajectory == topic2 { //11
-			f, ok := rule.(func(data *pjisuv_msgs.Trajectory) string)
+			f, ok := rule.(func(data *pjisuv_msgs.Trajectory, param entity.PjisuvParam) string)
 			if ok != true {
-				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Trajectory) string):", err)
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Trajectory,param entity.PjisuvParam) string):", err)
 				continue
 			}
 			RuleOfCicvAmrTrajectory = append(RuleOfCicvAmrTrajectory, f)

aarch64/pjisuv/master/service/produce_window.go

@@ -32,6 +32,8 @@ var (
 	mutexOfDataRead sync.RWMutex
 	// /pj_vehicle_fdb_pub
 	mutexOfPjVehicleFdbPub sync.RWMutex
+	// /pj_vehicle_fdb_pub
+	mutexOfCicvAmrTrajectory sync.RWMutex
 )
 
 // PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
@@ -339,6 +341,7 @@ func PrepareTimeWindowProducerQueue() {
 				Node:  commonConfig.RosNode,
 				Topic: topic,
 				Callback: func(data *pjisuv_msgs.Trajectory) {
+
 					subscribersTimeMutexes[i].Lock()
 					if time.Since(subscribersTimes[i]).Seconds() > 1 {
 						subscribersMutexes[i].Lock()
@@ -346,7 +349,7 @@ func PrepareTimeWindowProducerQueue() {
 						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfCicvAmrTrajectory {
-							faultLabel = f(data)
+							faultLabel = f(data, pjisuvParam)
 							if faultLabel != "" {
 								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
 								subscribersTimes[i] = time.Now()
@@ -356,6 +359,17 @@ func PrepareTimeWindowProducerQueue() {
 						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()
 				},
 			})
 		}

common/entity/extend_param.go

@@ -26,6 +26,9 @@ type PjisuvParam struct {
 	StrgAngleRealValueOfDataRead float64
 	// /pj_vehicle_fdb_pub
 	AutomodeOfPjVehicleFdbPub int16
+
+	// /cicv_amr_trajectory
+	LastCurvaturesOfCicvAmrTrajectory []float64
 }
 
 type KinglongParam struct {

trigger/pjisuv/cicv_amr_trajectory/main/cicv_amr_trajectory.go

@@ -0,0 +1,47 @@
+package main
+
+import (
+	"cicv-data-closedloop/common/entity"
+	"cicv-data-closedloop/pjisuv_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "controljump"
+}
+
+func Rule(data *pjisuv_msgs.Trajectory, param entity.PjisuvParam) string {
+	var currentCurvateres []float64
+	for _, point := range data.Trajectoryinfo.Trajectorypoints {
+		currentCurvateres = append(currentCurvateres, math.Abs(float64(point.Curvature)))
+	}
+	if len(currentCurvateres) != 0 {
+		maxCurrent := findMax(currentCurvateres)
+		maxLast := findMax(param.LastCurvaturesOfCicvAmrTrajectory)
+		a := math.Abs(maxCurrent - maxLast)
+		if a >= 0.099 {
+			return "controljump"
+		}
+	}
+	return ""
+}
+
+// findMax 函数接收一个float64类型的切片，并返回其中的最大值
+func findMax(numbers []float64) float64 {
+	if len(numbers) == 0 {
+		// 如果没有元素，可以根据需要处理，这里直接返回0作为示例
+		return 0
+	}
+	maxTemp := numbers[0] // 假设第一个元素就是最大的
+	for _, number := range numbers {
+		if number > maxTemp {
+			maxTemp = number
+		}
+	}
+	return maxTemp
+}

trigger/pjisuv/tpperception/perceptionspeeddiffer/main/perceptionspeeddiffer.go

@@ -28,7 +28,7 @@ func Topic() string {
 
 // Label todo 禁止存在下划线_
 func Label() string {
-	return "TTC"
+	return "perceptionspeeddiffer"
 }
 
 func Rule(data *pjisuv_msgs.PerceptionObjects, param entity.PjisuvParam) string {

trigger/pjisuv/tpperception/perceptiontypediffer/main/perceptiontypediffer.go

@@ -22,7 +22,7 @@ func Topic() string {
 
 // Label todo 禁止存在下划线_
 func Label() string {
-	return "TTC"
+	return "perceptiontypediffer"
 }
 
 func Rule(data *pjisuv_msgs.PerceptionObjects, param entity.PjisuvParam) string {