commit

trigger/jili/cicv_extend/cut_trigger_jinlong.py

@@ -0,0 +1,266 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from __future__ import print_function
+import rospy
+import sys
+import math
+from perception_msgs.msg import PerceptionLocalization
+from perception_msgs.msg import PerceptionObjects
+from perception_msgs.msg import Object
+from control_msgs.msg import Jinlong_Control_Command
+from common_msgs.msg import Retrieval 
+from std_msgs.msg import Bool
+
+import math
+
+import rosbag
+import threading
+import time
+import subprocess
+from datetime import datetime
+from subprocess import Popen, PIPE
+import signal
+import os
+import datetime
+from threading import Thread
+from fnmatch import fnmatch
+from fnmatch import fnmatch, fnmatchcase 
+from rosbag import Bag
+from scipy import stats
+
+
+
+obj_lock = threading.Lock()
+difference_lock = threading.Lock()
+
+
+ego_Steering_real=[1]
+ego_throttle_real=[1]
+ego_brake_real=[1]
+ego_Steering_cmd=[1]
+ego_throttle_cmd=[1]
+ego_brake_cmd=[1]
+obj_dic={}
+#Strg_Angle_Real_Value=0
+num_count_data_read=0
+angular_velocity_z=0
+num_count_jinlong_Control_Command=0
+
+
+def callback_cicv_location(data):
+    global angular_velocity_z
+    angular_velocity_z=data.angular_velocity_z
+
+    
+def callback_Jinlong_Control_Command(data):
+    global ego_Steering_cmd
+    global ego_throttle_cmd 
+    global ego_brake_cmd
+    global num_count_jinlong_Control_Command
+    
+    num_count_jinlong_Control_Command+=1
+    if num_count_jinlong_Control_Command == 10:
+        ego_Steering_cmd.append(data.AS_Strg_Angle_Req)
+        ego_throttle_cmd.append(data.AS_AutoD_Accel_Pos_Req)   
+        ego_brake_cmd.append(data.AS_AutoD_BrkPelPos_Req)
+        num_count_jinlong_Control_Command=0
+           
+    
+def callback_data_read(data):  
+    #global Strg_Angle_Real_Value
+    global ego_Steering_real
+    global ego_throttle_real
+    global ego_brake_real
+    global num_count_data_read 
+    
+    num_count_data_read+=1
+    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)
+        num_count_data_read=0 
+    
+    #Strg_Angle_Real_Value=data.ActStrWhAng     
+ 
+def callback_tpperception(data):
+    global obj_dic
+    obj_list=data.objs
+    for obj in obj_list:
+
+        
+        if  (obj.type !=1 and  obj.type !=0 ) or obj.x<=5  or abs(obj.y)>=10: #  (obj.type !=1 and  obj.type !=0 ) 
+            continue       
+        #print(f"obj.type={obj.type},obj.id={obj.id},obj.x={obj.x},obj.y={obj.y}")
+        if obj.id not in obj_dic:
+            obj_dic[obj.id]=[]
+        obj_dic[obj.id].append(obj.y)
+    #print("----------------------------------------------------")
+        
+    
+def if_cutin(obj_y_list):
+    global Strg_Angle_Real_Value
+    global angular_velocity_z
+
+    print(obj_y_list)
+    flag_cutin=False
+
+    for i, obj_y in enumerate(obj_y_list):
+        if abs(obj_y)>=1.3 and abs(angular_velocity_z)<=0.6:
+            for j in range(len(obj_y_list)-i-1):
+                if abs(obj_y_list[1+i+j])<=0.7 and abs(angular_velocity_z)<=0.6:
+                    flag_cutin=True
+                    print('front car cut in')
+                    return flag_cutin
+    return flag_cutin
+
+
+def if_cutout(obj_y_list):
+    global Strg_Angle_Real_Value
+
+    global angular_velocity_z
+    #print(angular_velocity_z)
+    #print(obj_y_list)
+    #print('----------------------------------------------------------------------------')
+   
+    flag_cutout=False
+    for i, obj_y in enumerate(obj_y_list):
+        if abs(obj_y)<=0.6 and abs(angular_velocity_z)<=0.6:
+            for j in range(len(obj_y_list)-i-1):
+                if abs(obj_y_list[1+i+j])>=1.2 and abs(angular_velocity_z)<=0.6:
+                    flag_cutout=True
+                    print('front car cut out')
+                    return flag_cutout
+    return flag_cutout           
+
+
+def if_difference(ego_Steering_real,ego_throttle_real,ego_brake_real,
+                  ego_Steering_cmd,ego_throttle_cmd,ego_brake_cmd): # 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
+    
+    difference_flag=False
+
+    statistic_Steering, p_value_Steering = stats.mannwhitneyu(ego_Steering_real, ego_Steering_cmd)
+    statistic_throttle, p_value_throttle = stats.mannwhitneyu(ego_throttle_real, ego_throttle_cmd)
+    statistic_brake, p_value_brake = stats.mannwhitneyu(ego_brake_real, ego_brake_cmd)
+    if p_value_Steering < 0.05 or p_value_throttle < 0.05 or p_value_brake < 0.05:
+        difference_flag=True
+
+    return difference_flag
+            
+def callback_failure_lidar(data):
+    
+    if data.data: #为true,代表该传感器未匹配的目标过多
+        event_label='failurelidar'
+        
+        
+def callback_failure_radar(data):
+    if data.data: #为true,代表该传感器未匹配的目标过多
+        event_label='failureradar'
+
+def callback_failure_camera(data):
+    if data.data: #为true,代表该传感器未匹配的目标过多
+        event_label='failurecamera'
+
+
+def final_callback(event):
+    global obj_dic
+    global ego_Steering_real
+    global ego_throttle_real
+    global ego_brake_real
+    global ego_Steering_cmd
+    global ego_throttle_cmd
+    global ego_brake_cmd
+    cutin_flag = False
+    cutout_flag = False
+    flag_difference = False
+    #print('ego_Steering_real=')
+    #print(ego_Steering_real)
+    #print('--------------------------------------------------------------')
+   
+    with obj_lock:
+        # 先判断前车是否切入
+        for obj_id, obj_value in obj_dic.items():           
+            if len(obj_value) <= 5:
+                continue
+            cutin_flag=if_cutin(obj_value)
+
+            
+        # 再判断前车是否切出     来自 /tpperception
+        for obj_id, obj_value in obj_dic.items():           
+            if len(obj_value) <= 5:
+                continue
+            cutout_flag=if_cutout(obj_value)
+
+        with difference_lock:    
+            if cutin_flag:
+                print('The cutting-in behavior of the front car was detected')
+                flag_difference=if_difference(ego_Steering_real,ego_throttle_real,ego_brake_real,
+                                              ego_Steering_cmd,ego_throttle_cmd,ego_brake_cmd)
+                if flag_difference:
+                    event_label='cutin_difference'
+                    print('********cutin_difference********')
+               
+            if cutout_flag:
+                print('The cutting-out behavior of the front car was detected')
+                flag_difference=if_difference(ego_Steering_real,ego_throttle_real,ego_brake_real,
+                                              ego_Steering_cmd,ego_throttle_cmd,ego_brake_cmd)
+                if flag_difference:
+                    event_label='cutout_difference'
+                    print('********cutout_difference********')
+                    '''
+                    print('ego_Steering_real=')
+                    print(ego_Steering_real)
+                    print('--------------------------------------------------------------')
+                    print('ego_Steering_cmd=')
+                    print(ego_Steering_cmd)
+                    print('--------------------------------------------------------------')
+                    print('ego_throttle_real=')
+                    print(ego_throttle_real)
+                    print('--------------------------------------------------------------')
+                    print('ego_throttle_cmd=')
+                    print(ego_throttle_cmd)
+                    print('--------------------------------------------------------------')
+                #print('----------------------------------------------------------------------------')
+                    '''
+            #ego_Steering_real.clear()
+            #ego_throttle_real.clear()
+            #ego_Steering_cmd.clear()
+            #ego_throttle_cmd.clear()
+            ego_Steering_real=[1]
+            ego_throttle_real=[1]
+            ego_Steering_cmd=[1]
+            ego_throttle_cmd=[1]          
+        obj_dic.clear()
+
+        
+def listener():
+    rospy.init_node('listener', anonymous=True)
+    rospy.Subscriber("/cicv_location",PerceptionLocalization,callback_cicv_location)
+    rospy.Subscriber("/tpperception",PerceptionObjects,callback_tpperception)
+    rospy.Subscriber("/jinlong_control_pub",Jinlong_Control_Command,callback_Jinlong_Control_Command)
+    rospy.Subscriber("/data_read",Retrieval,callback_data_read)
+    rospy.Subscriber("/failure/lidar",Bool,callback_failure_lidar)
+    rospy.Subscriber("/failure/radar",Bool,callback_failure_radar)
+    rospy.Subscriber("/failure/camera",Bool,callback_failure_camera)
+    
+    timer1 = rospy.Timer(rospy.Duration(4), final_callback)
+    rospy.spin()
+         
+
+if __name__ == '__main__':
+
+    listener()
+
+
+ 
+    
+    
+   
+
+
+        
+
+
+
+
+

trigger/jili/cicv_extend/cut_trigger_pji.py

@@ -0,0 +1,239 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from __future__ import print_function
+import rospy
+import sys
+import math
+from perception_msgs.msg import PerceptionLocalization
+from perception_msgs.msg import PerceptionObjects
+from perception_msgs.msg import Object
+from common_msgs.msg import VehicleCmd
+from common_msgs.msg import Retrieval 
+
+import math
+
+import rosbag
+import threading
+import time
+import subprocess
+from datetime import datetime
+from subprocess import Popen, PIPE
+import signal
+import os
+import datetime
+from threading import Thread
+from fnmatch import fnmatch
+from fnmatch import fnmatch, fnmatchcase 
+from rosbag import Bag
+from scipy import stats
+
+
+
+obj_lock = threading.Lock()
+difference_lock = threading.Lock()
+
+
+ego_Steering_real=[]
+ego_throttle_real=[]
+ego_Brake_real=[]
+ego_Steering_cmd=[]
+ego_throttle_cmd=[]
+ego_Brake_cmd=[]
+obj_dic={}
+#Strg_Angle_Real_Value=0
+num_count_data_read=0
+angular_velocity_z=0
+num_count_pji_Control_Command=0
+
+
+def callback_cicv_location(data):
+    global angular_velocity_z
+
+    angular_velocity_z=data.angular_velocity_z
+
+    
+def callback_pji_Control_Command(data):
+    global ego_Steering_cmd
+    global ego_throttle_cmd 
+    global num_count_pji_Control_Command
+    
+    num_count_pji_Control_Command+=1
+    if num_count_pji_Control_Command == 10:
+        ego_Steering_cmd.append(data.ICPV_Cmd_StrAngle)
+        ego_throttle_cmd.append(data.ICPV_Cmd_AccPelPosAct)
+        num_count_pji_Control_Command=0
+           
+    
+def callback_data_read(data):  
+    global Strg_Angle_Real_Value
+    global ego_Steering_real
+    global ego_throttle_real
+    global num_count_data_read 
+    
+    num_count_data_read+=1
+    if num_count_data_read == 10:
+        ego_Steering_real.append(data.ActStrWhAng)
+        ego_throttle_real.append(data.AccPed2)
+        num_count_data_read=0 
+    
+    Strg_Angle_Real_Value=data.ActStrWhAng     
+ 
+def callback_tpperception(data):
+    global obj_dic
+    obj_list=data.objs
+    for obj in obj_list:
+
+        
+        if  obj.x<=5  or abs(obj.y)>=10: #  (obj.type !=1 and  obj.type !=0 ) 
+            continue       
+        #print(f"obj.type={obj.type},obj.id={obj.id},obj.x={obj.x},obj.y={obj.y}")
+        if obj.id not in obj_dic:
+            obj_dic[obj.id]=[]
+        obj_dic[obj.id].append(obj.y)
+    #print("----------------------------------------------------")
+        
+    
+def if_cutin(obj_y_list):
+    global Strg_Angle_Real_Value
+    global angular_velocity_z
+
+    #print(obj_y_list)
+    flag_cutin=False
+
+    for i, obj_y in enumerate(obj_y_list):
+        if abs(obj_y)>=1.1 and abs(angular_velocity_z)<=0.6:
+            for j in range(len(obj_y_list)-i-1):
+                if abs(obj_y_list[1+i+j])<=0.6 and abs(angular_velocity_z)<=0.6:
+                    flag_cutin=True
+                    #print('front car cut in')
+                    return flag_cutin
+    return flag_cutin
+
+
+def if_cutout(obj_y_list):
+    global Strg_Angle_Real_Value
+
+    global angular_velocity_z
+    #print(angular_velocity_z)
+    #print(obj_y_list)
+    #print('----------------------------------------------------------------------------')
+   
+    flag_cutout=False
+    for i, obj_y in enumerate(obj_y_list):
+        if abs(obj_y)<=0.6 and abs(angular_velocity_z)<=0.6:
+            for j in range(len(obj_y_list)-i-1):
+                if abs(obj_y_list[1+i+j])>=1.2 and abs(angular_velocity_z)<=0.6:
+                    flag_cutout=True
+                    #print('front car cut out')
+                    return flag_cutout
+    return flag_cutout           
+
+
+def if_difference(ego_Steering_real,ego_throttle_real,ego_Steering_cmd,ego_throttle_cmd): # 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
+    
+    difference_flag=False
+
+    statistic_Steering, p_value_Steering = stats.mannwhitneyu(ego_Steering_real, ego_Steering_cmd)
+    statistic_throttle, p_value_throttle = stats.mannwhitneyu(ego_throttle_real, ego_throttle_cmd)
+    if p_value_Steering < 0.05 or p_value_throttle < 0.05:
+        difference_flag=True
+
+    return difference_flag
+            
+
+def final_callback(event):
+    global obj_dic
+    global ego_Steering_real
+    global ego_throttle_real  
+    global ego_Steering_cmd
+    global ego_throttle_cmd
+    cutin_flag = False
+    cutout_flag = False
+    flag_difference = False
+    #print('ego_Steering_real=')
+    #print(ego_Steering_real)
+    #print('--------------------------------------------------------------')
+   
+    with obj_lock:
+        # 先判断前车是否切入
+        for obj_id, obj_value in obj_dic.items():           
+            if len(obj_value) <= 5:
+                continue
+            cutin_flag=if_cutin(obj_value)
+
+            
+        # 再判断前车是否切出    
+        for obj_id, obj_value in obj_dic.items():           
+            if len(obj_value) <= 5:
+                continue
+            cutout_flag=if_cutout(obj_value)
+
+        with difference_lock:    
+            if cutin_flag:
+                print('The cutting-in behavior of the front car was detected')
+                flag_difference=if_difference(ego_Steering_real,ego_throttle_real,ego_Steering_cmd,ego_throttle_cmd)
+                if flag_difference:
+                    event_label='cutin_difference'
+                    print('********cutin_difference********')
+               
+            if cutout_flag:
+                print('The cutting-out behavior of the front car was detected')
+                flag_difference=if_difference(ego_Steering_real,ego_throttle_real,ego_Steering_cmd,ego_throttle_cmd)
+                if flag_difference:
+                    event_label='cutout_difference'
+                    print('********cutout_difference********')
+                    '''
+                    print('ego_Steering_real=')
+                    print(ego_Steering_real)
+                    print('--------------------------------------------------------------')
+                    print('ego_Steering_cmd=')
+                    print(ego_Steering_cmd)
+                    print('--------------------------------------------------------------')
+                    print('ego_throttle_real=')
+                    print(ego_throttle_real)
+                    print('--------------------------------------------------------------')
+                    print('ego_throttle_cmd=')
+                    print(ego_throttle_cmd)
+                    print('--------------------------------------------------------------')
+                #print('----------------------------------------------------------------------------')
+                    '''
+            #ego_Steering_real.clear()
+            #ego_throttle_real.clear()
+            #ego_Steering_cmd.clear()
+            #ego_throttle_cmd.clear()
+            ego_Steering_real=[1]
+            ego_throttle_real=[1]
+            ego_Steering_cmd=[1]
+            ego_throttle_cmd=[1]
+           
+        obj_dic.clear()
+
+        
+def listener():
+    rospy.init_node('listener', anonymous=True)
+    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)
+    timer1 = rospy.Timer(rospy.Duration(3.5), final_callback)
+    rospy.spin()
+         
+
+if __name__ == '__main__':
+
+    listener()
+
+
+ 
+    
+    
+   
+
+
+        
+
+
+
+
+

trigger/jili/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.PjisuvParam) 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.PjisuvParam) 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.PjisuvParam) 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/jili/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.PjisuvParam) 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.PjisuvParam) 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.PjisuvParam) 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/jili/cicv_extend/datareturn2.py

@@ -0,0 +1,309 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from __future__ import print_function
+import rospy
+import sys
+import math
+from perception_msgs.msg import PerceptionLocalization
+from perception_msgs.msg import PerceptionObjects
+from perception_msgs.msg import Object
+from perception_msgs.msg import PolygonStamped
+from perception_msgs.msg import Polygonbus
+from perception_msgs.msg import Trajectory
+from perception_msgs.msg import TrajectoryInfo
+from perception_msgs.msg import TrajectoryPoint
+from control_msgs.msg import VehicleFdb
+from control_msgs.msg import VehicleCmd
+import numpy as np
+import math
+
+import rosbag
+import threading
+import time
+import subprocess
+from datetime import datetime
+from subprocess import Popen, PIPE
+import signal
+import os
+import datetime
+from threading import Thread
+from fnmatch import fnmatch
+from fnmatch import fnmatch, fnmatchcase 
+from rosbag import Bag
+#from scipy import stats
+
+
+
+obj_lock = threading.Lock()
+difference_lock = threading.Lock()
+
+#定义全局变量
+obj_dic={}
+obj_type_dic={}
+obj_speed_dic={}
+angular_velocity_z=0
+num_count_pji_Control_Command=0
+pre_Automode_stat_flag=0
+Ego_position_x=0
+Ego_position_y=0
+Ego_yaw=0
+last_curvatures=[]
+
+
+def callback_cicv_location(data):
+    global angular_velocity_z
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw 
+
+    Trajectory
+    Ego_position_x=data.position_x
+    Ego_position_y=data.position_y
+    Ego_yaw=data.yaw
+    #print(Ego_yaw)
+    angular_velocity_z=data.angular_velocity_z
+    #print(angular_velocity_z)
+    if abs(angular_velocity_z)>=27:
+        event_label='overswing' #横摆角速度过大
+        print(event_label)
+
+
+def callback_pj_vehicle_fdb_pub(data):
+    global pre_Automode_stat_flag
+    if data.Automode==0 and pre_Automode_stat_flag==1:
+        event_label='DriverTakeOver'    #人工接管
+        print(event_label)
+    pre_Automode_stat_flag=data.Automode
+        
+
+def callback_tpperception(data):
+    global obj_dic
+    global obj_type_dic
+    global obj_speed_dic
+    obj_list=data.objs
+    for obj in obj_list:
+
+        if  obj.x>=5  and abs(obj.y)<=10: # 根据这个条件过滤一遍目标物 ,  用于前车切入切出判别           
+            if obj.id not in obj_dic:
+                obj_dic[obj.id]=[]
+            obj_dic[obj.id].append(obj.y)
+        
+        if abs(obj.y)<=2 and obj.x>=6: # 根据这个条件过滤一遍目标物 ,  用于计算ＴＴＣ 
+            
+            TTC=-(obj.x-4)/(obj.vxrel+0.0001)
+            if 0<=TTC<=3:
+                event_label='TTC'
+                print(event_label)
+                break
+        
+        if obj_type_dic is not {}: #相邻两帧对于同一目标物感知类型不一致
+            if obj.id in obj_type_dic:
+                if obj.type != obj_type_dic[obj.id]:
+                    event_label='perceptiontypediffer'    
+
+                    print(event_label)
+                else:
+                    print(obj.type,"   ",obj_type_dic[obj.id])
+
+
+        if obj_speed_dic is not {}:  #相邻两帧对于同一目标物速度变化率超过设定阈值
+            if obj.id in obj_speed_dic:
+                 
+                objspeed=pow(pow(obj.vxabs,2)+pow(obj.vyabs,2),0.5)
+                diff_rate=abs(objspeed-obj_speed_dic[obj.id])/(obj_speed_dic[obj.id]+0.0001)
+                if diff_rate>0.1 and objspeed>0.5 and obj_speed_dic[obj.id]>0.5:
+                    event_label='perceptionspeeddiffer'    
+
+                    print(event_label)
+                    print(objspeed,"   ",obj_speed_dic[obj.id],"   ",diff_rate)
+                else:
+                    print(objspeed,"   ",obj_speed_dic[obj.id],"   ",diff_rate)                    
+                       
+    obj_type_dic={}
+    obj_speed_dic={}
+    for obj in obj_list:
+        obj_type_dic[obj.id]=obj.type
+        obj_speed_dic[obj.id]=pow(pow(obj.vxabs,2)+pow(obj.vyabs,2),0.5)
+                    
+
+        
+           
+
+def final_callback(event):
+    global obj_dic
+
+    cutin_flag = False
+    cutout_flag = False
+    flag_difference = False
+    def if_cutin(obj_y_list):# 判断前车是否切入
+        global angular_velocity_z
+        flag_cutin=False
+ 
+        for i, obj_y in enumerate(obj_y_list):
+            if abs(obj_y)>=1.1 and abs(angular_velocity_z)<=0.6:
+                for j in range(len(obj_y_list)-i-1):
+                    if abs(obj_y_list[1+i+j])<=0.6 and abs(angular_velocity_z)<=0.6:
+                        flag_cutin=True
+                        return flag_cutin
+        return flag_cutin
+ 
+    def if_cutout(obj_y_list): #判断前车是否切入
+        global angular_velocity_z       
+        flag_cutout=False
+        for i, obj_y in enumerate(obj_y_list):
+            if abs(obj_y)<=0.6 and abs(angular_velocity_z)<=0.6:
+                for j in range(len(obj_y_list)-i-1):
+                    if abs(obj_y_list[1+i+j])>=1.2 and abs(angular_velocity_z)<=0.6:
+                        flag_cutout=True
+                        return flag_cutout
+        return flag_cutout 
+   
+    with obj_lock:
+        # 先判断前车是否切入
+        for obj_id, obj_value in obj_dic.items():           
+            if len(obj_value) <= 5:
+                continue
+            cutin_flag=if_cutin(obj_value)
+            if cutin_flag :
+                event_label="cutin" # 前车切入
+                print(event_label)
+                continue                
+       
+        # 再判断前车是否切出    
+        for obj_id, obj_value in obj_dic.items():           
+            if len(obj_value) <= 5:
+                continue
+            cutout_flag=if_cutout(obj_value)
+            if cutout_flag:
+                event_label="cutiout"   #前车切出
+                print(event_label)
+                continue  
+        obj_dic.clear()
+       
+        
+def callback_map_polygon(data): #用于判断车辆是否冲出车道
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw 
+    
+    def get_vehicle_corners(x0, y0, D1, D2, D3, h0):
+        # 将航向角从度数转换为弧度
+        h0_rad = np.radians(h0)
+
+        # 车辆四个角点相对于后轴中心点的坐标（局部坐标系）
+        corners_local = np.array([
+            [D1, D3],  # 前左角
+            [D1, -D3],  # 前右角
+            [-D2, D3],  # 后左角
+            [-D2, -D3]  # 后右角
+        ])
+
+        # 旋转矩阵
+        rotation_matrix = np.array([
+            [np.cos(h0_rad), -np.sin(h0_rad)],
+            [np.sin(h0_rad), np.cos(h0_rad)]
+        ])
+
+        # 计算在全局坐标系中的角点坐标
+        corners_global = np.dot(corners_local, rotation_matrix.T) + np.array([x0, y0])
+
+        return corners_global 
+    
+    def ccw(A, B, C):
+        """ 判断点C相对于线段AB是否是逆时针方向 """
+        return (C[1] - A[1]) * (B[0] - A[0]) > (C[0] - A[0]) * (B[1] - A[1])
+
+    def line_intersect(A, B, C, D):
+        """ 判断线段AB和CD是否相交 """
+        return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)
+
+    def polygon_line_intersect(polygon, A, B):
+        """ 判断线段AB是否与多边形的任何边相交 """
+        num_vertices = len(polygon)
+        for i in range(num_vertices):
+            C = polygon[i]
+            D = polygon[(i + 1) % num_vertices]
+            if line_intersect(A, B, C, D):
+                return True
+        return False
+
+    x0 = Ego_position_x  # 后轴中心点 x 坐标
+    y0 = Ego_position_y  # 后轴中心点 y 坐标
+    D1 = 3.813  # 车辆前端到后轴中心点的距离
+    D2 = 0.952  # 车辆后端到后轴中心点的距离
+    D3 = 1.086  # 车辆左端和右端到后轴中心点的距离
+    h0 = Ego_yaw  # 航向角，度数
+    #print(Ego_yaw)
+    corners = get_vehicle_corners(x0, y0, D1, D2, D3, h0)
+    Points=data.polygon.points
+
+    for i in range(len(Points)-1):
+        
+        # 点A和点B的坐标
+    
+        x1, y1 =Points[i].x, Points[i].y  # 点A
+        x2, y2 = Points[i+1].x, Points[i+1].y  # 点B
+
+        A = np.array([x1, y1])
+        B = np.array([x2, y2])
+
+        # 检查线段AB是否与车辆的四边形相交
+        if polygon_line_intersect(corners, A, B):
+            event_label='outofroad'
+            print(event_label)
+            break
+def callback_pj_control_pub(data):  #控制下发的制动踏板开度大于９０，触发ＡＥＢ
+    if data.ICPV_Cmd_BrkPelPosActInv>=90:
+        event_label='aeb'
+        print(event_label)
+
+
+
+def callback_cicv_amr_trajectory(data):
+    global last_curvatures
+    
+    current_curvatures = [abs(point.curvature) for point in data.trajectoryinfo.trajectorypoints]
+    if last_curvatures !=[]:
+    
+        max_current = max(current_curvatures)
+        max_last = max(last_curvatures)
+        a=abs(max_current-max_last)
+        if a>=0.099:
+            event_label='controljump'
+            print(event_label)
+    last_curvatures=current_curvatures
+        
+  
+def listener():
+    rospy.init_node('listener', anonymous=True)
+    rospy.Subscriber("/cicv_location",PerceptionLocalization,callback_cicv_location)
+    rospy.Subscriber("/tpperception",PerceptionObjects,callback_tpperception)
+    rospy.Subscriber("/pj_control_pub",VehicleCmd,callback_pj_control_pub)
+    rospy.Subscriber("/pj_vehicle_fdb_pub",VehicleFdb,callback_pj_vehicle_fdb_pub)
+    rospy.Subscriber("/cicv_amr_trajectory",Trajectory,callback_cicv_amr_trajectory)
+    
+    #rospy.Subscriber("/data_read",Retrieval,callback_data_read)
+    rospy.Subscriber("/map_polygon",PolygonStamped,callback_map_polygon)
+    timer1 = rospy.Timer(rospy.Duration(3.5), final_callback)
+    
+    rospy.spin()
+         
+
+if __name__ == '__main__':
+
+    listener()
+
+
+ 
+    
+    
+   
+
+
+        
+
+
+
+
+

trigger/jili/cicv_location/AbnormalParking/main/AbnormalParking.go

@@ -0,0 +1,115 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	StartTime      int64
+	IsStopped      bool
+	IsEndPoint     bool
+	IsTrafficLight bool
+	count1         int64
+	//定义园区４个信号灯的坐标
+	point2     = Point{39.72975930689718, 116.48861102824081}
+	point3     = Point{39.7288805296616, 116.48812315228867}
+	point4     = Point{39.73061430369551, 116.49225103553502}
+	point5     = Point{39.73077491578002, 116.49060085035634}
+	EndPoint   = Point{0, 0}
+	pointlist1 = []Point{point2, point3, point4, point5}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "AbnormalParking"
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Longitude, _ := shareVars.Load("EndPointX")
+		Latitude, _ := shareVars.Load("EndPointY")
+		Automode = Automode.(int16)
+		EndPoint.Longitude = Longitude.(float64)
+		EndPoint.Latitude = Latitude.(float64)
+		pointlist2 := []Point{EndPoint}
+		IsTrafficLight = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
+		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
+		if Automode == 1 && !IsTrafficLight && !IsEndPoint && OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			if AbsSpeed.(float64) < 0.5 {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 5 {
+					if !IsStopped {
+						IsStopped = true
+						return Label()
+					}
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
+			}
+		} else {
+			StartTime = 0
+			IsStopped = false
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/AbnormalStopOnCurve/main/AbnormalStopOnCurve.go

@@ -0,0 +1,115 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	StartTime  int64
+	threshold  int64 = 3
+	IsStopped  bool
+	IsEndPoint bool
+	IsCurve    bool
+	count1     int64
+	pointcurve = Point{39.73004426154644, 116.49248639463602}
+	EndPoint   = Point{0, 0}
+	pointlist1 = []Point{pointcurve}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "AbnormalStopOnCurve"
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Longitude, ok1 := shareVars.Load("EndPointX")
+		if !ok1 {
+			Longitude = 0.0
+		}
+		Latitude, ok2 := shareVars.Load("EndPointY")
+		if !ok2 {
+			Latitude = 0.0
+		}
+		Automode = Automode.(int16)
+		EndPoint.Longitude = Longitude.(float64)
+		EndPoint.Latitude = Latitude.(float64)
+		pointlist2 := []Point{EndPoint}
+		IsCurve = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
+		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
+		if Automode == 1 && IsCurve && !IsEndPoint {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			if AbsSpeed.(float64) < 0.5 {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > threshold {
+					if !IsStopped {
+						IsStopped = true
+						return Label()
+					}
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
+			}
+		} else {
+			StartTime = 0
+			IsStopped = false
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/AbnormalStopOnJunction/main/AbnormalStopOnJunction.go

@@ -0,0 +1,117 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	StartTime  int64
+	threshold  int64 = 3
+	IsStopped  bool
+	IsJunction bool
+	IsEndPoint bool
+	count1     int64
+	EndPoint   = Point{0, 0}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "AbnormalStopOnJunction"
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+		Automode, ok3 := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Longitude, ok1 := shareVars.Load("EndPointX")
+		if !ok1 {
+			Longitude = 0.0
+		}
+		Latitude, ok2 := shareVars.Load("EndPointY")
+		if !ok2 {
+			Latitude = 0.0
+		}
+		Automode = Automode.(int16)
+		EndPoint.Longitude = Longitude.(float64)
+		EndPoint.Latitude = Latitude.(float64)
+		pointlist2 := []Point{EndPoint}
+		EnterJunctionFlag, ok := shareVars.Load("EnterJunctionFlag")
+		if ok && ok3 {
+
+			IsJunction = EnterJunctionFlag.(bool)
+			IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
+			if Automode == 1 && IsJunction && !IsEndPoint {
+				AbsSpeed, _ := shareVars.Load("AbsSpeed")
+				if AbsSpeed.(float64) < 0.5 {
+					// 如果之前没有记录开始时间，记录当前时间
+					if StartTime == 0 {
+						StartTime = time.Now().Unix()
+					}
+					// 判断是否持续超过 50s
+					if time.Now().Unix()-StartTime > threshold {
+						if !IsStopped {
+							IsStopped = true
+							return Label()
+						}
+					}
+				} else {
+					// 如果速度大于 0.1，重置开始时间和停止标志
+					StartTime = 0
+					IsStopped = false
+				}
+			} else {
+				StartTime = 0
+				IsStopped = false
+			}
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/AuLongStop/main/AuLongStop.go

@@ -0,0 +1,59 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+	"time"
+)
+
+var (
+	StartTime int64
+	IsStopped bool
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "AuLongStop"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	automodeOfPjVehicleFdbPub, ok := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	if ok {
+		if automodeOfPjVehicleFdbPub.(int16) == 1 {
+			if data.VelocityX < 0.5 {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 50 && OutsideWorkshopFlag == true {
+					if !IsStopped {
+						IsStopped = true
+						return Label()
+					}
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
+			}
+		} else {
+			StartTime = 0
+			IsStopped = false
+		}
+	}
+
+	return ""
+}

trigger/jili/cicv_location/Brake/main/Brake.go

@@ -0,0 +1,30 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "Brake"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	if data.AccelX*9.8 < -14.0 && OutsideWorkshopFlag == true {
+		return Label()
+	}
+	return ""
+}

trigger/jili/cicv_location/BrakeWithHighSpeed/main/BrakeWithHighSpeed.go

@@ -0,0 +1,35 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+)
+
+var (
+	threshold float64 = 5.4
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "BrakeWithHighSpeed"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	AbsSpeed, _ := shareVars.Load("AbsSpeed")
+	if data.AccelX*9.8 < -12.0 && OutsideWorkshopFlag == true && AbsSpeed.(float64) > threshold {
+		return Label()
+	}
+	return ""
+}

trigger/jili/cicv_location/CannotBypassObstacles/main/CannotBypassObstacles.go

@@ -0,0 +1,128 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	StartTime      int64
+	IsStopped      bool
+	IsEndPoint     bool
+	IsTrafficLight bool
+	count1         int64
+	//定义园区４个信号灯的坐标
+	point2     = Point{39.72975930689718, 116.48861102824081}
+	point3     = Point{39.7288805296616, 116.48812315228867}
+	point4     = Point{39.73061430369551, 116.49225103553502}
+	point5     = Point{39.73077491578002, 116.49060085035634}
+	EndPoint   = Point{0, 0}
+	pointlist1 = []Point{point2, point3, point4, point5}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CannotBypassObstacles"
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+
+	return d
+}
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] <= 13 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 6 {
+				return true
+
+			}
+		}
+	}
+	return false
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Longitude, _ := shareVars.Load("EndPointX")
+		Latitude, _ := shareVars.Load("EndPointY")
+
+		Automode = Automode.(int16)
+		EndPoint.Longitude = Longitude.(float64)
+		EndPoint.Latitude = Latitude.(float64)
+		pointlist2 := []Point{EndPoint}
+		IsTrafficLight = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
+		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
+		if Automode == 1 && !IsTrafficLight && !IsEndPoint && OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) < 0.5 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 5 {
+					if !IsStopped {
+						IsStopped = true
+						return Label()
+					}
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
+			}
+		} else {
+			StartTime = 0
+			IsStopped = false
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/CarFollowingTooCloseAtNight/main/CarFollowingTooCloseAtNight.go

@@ -0,0 +1,85 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	StartTime int64
+	count1    int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CarFollowingTooCloseAtNight"
+}
+
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] <= 15 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	return false
+}
+func Ifatnight() bool {
+	// 获取当前时间
+	now := time.Now()
+	later := now.Add(0 * time.Hour)
+	// 获取当前小时
+	hour := later.Hour()
+	// 判断当前时间是白天还是夜晚
+	if hour >= 0 && hour < 5 || hour >= 20 && hour <= 23 {
+		return true
+	} else {
+		return false
+	}
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%20 == 0 && Ifatnight() {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) > 1 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 3 {
+					count1 = 1
+					return Label()
+
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+
+			}
+		} else {
+			StartTime = 0
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/CarFollowingTooCloseHigh/main/CarFollowingTooCloseHigh.go

@@ -0,0 +1,72 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	StartTime int64
+	count1    int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CarFollowingTooCloseHigh"
+}
+
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] <= 15 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	return false
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%20 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) > 4 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 3 {
+					count1 = 1
+					return Label()
+
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+
+			}
+		} else {
+			StartTime = 0
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/CarFollowingTooCloseLow/main/CarFollowingTooCloseLow.go

@@ -0,0 +1,71 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	StartTime int64
+	count1    int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CarFollowingTooCloseLow"
+}
+
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] <= 12 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	return false
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%20 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) >= 1 && AbsSpeed.(float64) <= 3 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 3 {
+					count1 = 1
+					return Label()
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+
+			}
+		} else {
+			StartTime = 0
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/CurveOverspeed/main/CurveOverspeed.go

@@ -0,0 +1,85 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	threshold  float64 = 5
+	IsCurve    bool
+	count1     int64
+	pointcurve = Point{39.73004426154644, 116.49248639463602}
+	pointlist1 = []Point{pointcurve}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CurveOverspeed"
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Automode = Automode.(int16)
+		IsCurve = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
+
+		if Automode == 1 && IsCurve {
+			absspeed, ok := shareVars.Load("AbsSpeed")
+			if ok && absspeed.(float64) >= threshold {
+				eventLabel := "CurveOverspeed"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/EgoReversing/main/EgoReversing.go

@@ -0,0 +1,64 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+var (
+	count1 int = 0
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "EgoReversing"
+}
+func calculateDirectionAngle(speedX, speedY float64) float64 {
+	// 使用反正切函数计算方向角
+	angle := math.Atan2(speedY, speedX)
+
+	// 将角度转换为度数
+	angleDegree := angle * (180 / math.Pi)
+
+	// 将角度限定在 0~360 范围内
+	if angleDegree < 0 {
+		angleDegree += 360
+	} else if angleDegree > 360 {
+		angleDegree -= 360
+	}
+
+	return angleDegree
+}
+
+// 主进程的逻辑是先判断触发再缓存全局变量
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%200 == 0 {
+		OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
+		if ok3 {
+			directionAngle := calculateDirectionAngle(data.VelocityX, data.VelocityY)
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			diffAngle := math.Abs(float64(directionAngle - data.Yaw))
+			//fmt.Println(diffAngle)
+			if AbsSpeed.(float64) >= 1 && diffAngle >= 160 && diffAngle <= 200 && OutsideWorkshopFlag.(bool) {
+
+				eventLabel := "EgoReversing"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/EnterTjunction/main/EnterTjunction.go

@@ -0,0 +1,33 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "EnterTjunction"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	EnterJunctionFlag, ok := shareVars.Load("EnterJunctionFlag")
+	AbsSpeed, ok1 := shareVars.Load("AbsSpeed")
+
+	if ok && ok1 && EnterJunctionFlag.(bool) == true && AbsSpeed.(float64) >= 1 {
+		return Label()
+
+	}
+
+	return ""
+}

trigger/jili/cicv_location/ExcessiveSpeedWhenDownhill/main/ExcessiveSpeedWhenDownhill.go

@@ -0,0 +1,72 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+/*
+def callback_cicv_location(data):
+    global angular_velocity_z
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw
+
+
+    Ego_position_x=data.position_x
+    Ego_position_y=data.position_y
+    Ego_yaw=data.yaw
+    #print(Ego_yaw)
+    angular_velocity_z=data.angular_velocity_z
+    #print(angular_velocity_z)
+    if abs(angular_velocity_z)>=27:
+        event_label='overswing' #横摆角速度过大
+        print(event_label)
+
+*/
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "ExcessiveSpeedWhenDownhill"
+}
+
+var (
+	count     int = 0
+	threshold     = -0.05
+)
+
+func QuaternionToEuler(x, y, z, w float64) float64 {
+	// 归一化四元数
+	length := math.Sqrt(x*x + y*y + z*z + w*w)
+	x /= length
+	y /= length
+	z /= length
+	w /= length
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	return pitch
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count%10 == 0 {
+
+		OutsideWorkshopFlag, ok1 := shareVars.Load("OutsideWorkshopFlag")
+		if ok1 && OutsideWorkshopFlag.(bool) {
+			if math.Abs(data.AngularVelocityZ) >= 17.0 && QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw) <= threshold {
+				count = 1
+				return Label()
+			}
+		}
+	}
+	count++
+	return ""
+}

trigger/jili/cicv_location/ExcessiveSpeedWhenUphill/main/ExcessiveSpeedWhenUphill.go

@@ -0,0 +1,72 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+/*
+def callback_cicv_location(data):
+    global angular_velocity_z
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw
+
+
+    Ego_position_x=data.position_x
+    Ego_position_y=data.position_y
+    Ego_yaw=data.yaw
+    #print(Ego_yaw)
+    angular_velocity_z=data.angular_velocity_z
+    #print(angular_velocity_z)
+    if abs(angular_velocity_z)>=27:
+        event_label='overswing' #横摆角速度过大
+        print(event_label)
+
+*/
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "ExcessiveSpeedWhenUphill"
+}
+
+var (
+	count     int = 0
+	threshold     = 0.05
+)
+
+func QuaternionToEuler(x, y, z, w float64) float64 {
+	// 归一化四元数
+	length := math.Sqrt(x*x + y*y + z*z + w*w)
+	x /= length
+	y /= length
+	z /= length
+	w /= length
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	return pitch
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count%10 == 0 {
+
+		OutsideWorkshopFlag, ok1 := shareVars.Load("OutsideWorkshopFlag")
+		if ok1 && OutsideWorkshopFlag.(bool) {
+			if math.Abs(data.AngularVelocityZ) >= 17.0 && QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw) >= threshold {
+				count = 1
+				return Label()
+			}
+		}
+	}
+	count++
+	return ""
+}

trigger/jili/cicv_location/FindLongCurve/main/FindLongCurve.go

@@ -0,0 +1,77 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// todo 禁止存在下划线_
+func Label() string {
+	return "FindLongCurve"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int = 0
+
+	//定义弯道坐标
+	point = Point{39.73004426154644, 116.49248639463602}
+)
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+
+		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
+			//eventLabel := "FindTrafficLight"
+			//fmt.Println(eventLabel)
+			return "FindLongCurve"
+		}
+	}
+	count1++
+	return ""
+}
+
+func IfEnter(point Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	d := distance(point1, point)
+	if d <= radius {
+		return true
+	}
+
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

trigger/jili/cicv_location/FindTrafficLightP1/main/FindTrafficLightP1.go

@@ -0,0 +1,82 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// todo 禁止存在下划线_
+func Label() string {
+	return "FindTrafficLight"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int = 0
+
+	//定义园区４个信号灯的坐标
+	point = Point{39.72975930689718, 116.48861102824081}
+	//point3 = Point{39.7288805296616, 116.48812315228867}
+	//point4 = Point{39.73061430369551, 116.49225103553502}
+	//point5 = Point{39.73077491578002, 116.49060085035634}
+
+	//pointlist = []Point{point2, point3, point4, point5}
+)
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+
+		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
+			//eventLabel := "FindTrafficLight"
+			//fmt.Println(eventLabel)
+			return "FindTrafficLightP1"
+		}
+	}
+	count1++
+	return ""
+}
+
+func IfEnter(point Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	d := distance(point1, point)
+	if d <= radius {
+		return true
+	}
+
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

trigger/jili/cicv_location/FindTrafficLightP2/main/FindTrafficLightP2.go

@@ -0,0 +1,82 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// todo 禁止存在下划线_
+func Label() string {
+	return "FindTrafficLight"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int = 0
+
+	//定义园区４个信号灯的坐标
+	//point = Point{39.72975930689718, 116.48861102824081}
+	point = Point{39.7288805296616, 116.48812315228867}
+	//point4 = Point{39.73061430369551, 116.49225103553502}
+	//point5 = Point{39.73077491578002, 116.49060085035634}
+
+	//pointlist = []Point{point2, point3, point4, point5}
+)
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+
+		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
+			//eventLabel := "FindTrafficLight"
+			//fmt.Println(eventLabel)
+			return "FindTrafficLightP1"
+		}
+	}
+	count1++
+	return ""
+}
+
+func IfEnter(point Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	d := distance(point1, point)
+	if d <= radius {
+		return true
+	}
+
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

trigger/jili/cicv_location/FindTrafficLightP3/main/FindTrafficLightP3.go

@@ -0,0 +1,82 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// todo 禁止存在下划线_
+func Label() string {
+	return "FindTrafficLight"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int = 0
+
+	//定义园区４个信号灯的坐标
+	//point = Point{39.72975930689718, 116.48861102824081}
+	//point3 = Point{39.7288805296616, 116.48812315228867}
+	point = Point{39.73061430369551, 116.49225103553502}
+	//point5 = Point{39.73077491578002, 116.49060085035634}
+
+	//pointlist = []Point{point2, point3, point4, point5}
+)
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+
+		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
+			//eventLabel := "FindTrafficLight"
+			//fmt.Println(eventLabel)
+			return "FindTrafficLightP1"
+		}
+	}
+	count1++
+	return ""
+}
+
+func IfEnter(point Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	d := distance(point1, point)
+	if d <= radius {
+		return true
+	}
+
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

trigger/jili/cicv_location/FindTrafficLightP4/main/FindTrafficLightP4.go

@@ -0,0 +1,82 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// todo 禁止存在下划线_
+func Label() string {
+	return "FindTrafficLight"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int = 0
+
+	//定义园区４个信号灯的坐标
+	//point = Point{39.72975930689718, 116.48861102824081}
+	//point3 = Point{39.7288805296616, 116.48812315228867}
+	//point4 = Point{39.73061430369551, 116.49225103553502}
+	point = Point{39.73077491578002, 116.49060085035634}
+
+	//pointlist = []Point{point2, point3, point4, point5}
+)
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+
+		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
+			//eventLabel := "FindTrafficLight"
+			//fmt.Println(eventLabel)
+			return "FindTrafficLightP1"
+		}
+	}
+	count1++
+	return ""
+}
+
+func IfEnter(point Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	d := distance(point1, point)
+	if d <= radius {
+		return true
+	}
+
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

trigger/jili/cicv_location/JunctionOverspeed/main/JunctionOverspeed.go

@@ -0,0 +1,49 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	threshold float64 = 5
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "JunctionOverspeed"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+
+	Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+	Automode = Automode.(int16)
+	EnterJunctionFlag, ok1 := shareVars.Load("EnterJunctionFlag")
+
+	if ok1 && Automode == 1 && EnterJunctionFlag.(bool) == true {
+		absspeed, ok := shareVars.Load("AbsSpeed")
+		if ok && absspeed.(float64) >= threshold {
+			eventLabel := "JunctionOverspeed"
+			fmt.Println(eventLabel)
+
+			return Label()
+		}
+	}
+
+	return ""
+}

trigger/jili/cicv_location/LocationJump/main/LocationJump.go

@@ -0,0 +1,41 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "LocationJump"
+}
+
+// 主进程的逻辑是先判断触发再缓存全局变量
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	positionXOfCicvLocation, ok1 := shareVars.Load("PositionXOfCicvLocation")
+	positionYOfCicvLocation, ok2 := shareVars.Load("PositionYOfCicvLocation")
+	if ok1 && ok2 && ok3 {
+		v1 := positionXOfCicvLocation.(float64)
+		v2 := positionYOfCicvLocation.(float64)
+		if v1 != 0 && v2 != 0 {
+			d := math.Sqrt((v1-data.PositionX)*(v1-data.PositionX) + (v2-data.PositionY)*(v2-data.PositionY))
+			if d >= 2 && OutsideWorkshopFlag == true {
+				return Label()
+			}
+		}
+	}
+	return ""
+}

trigger/jili/cicv_location/LongDownhill/main/LongDownhill.go

@@ -0,0 +1,107 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+var (
+	threshold        = -0.05
+	count1           = 0
+	UphillPointSlice = [][]float64{{}, {}}
+	CountNoneUphill  = 0
+)
+
+type Point struct {
+	X float64
+	Y float64
+}
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "LongDownhill"
+}
+
+// QuaternionToEuler 将四元数转换为欧拉角
+func QuaternionToEuler(x, y, z, w float64) float64 {
+	// 归一化四元数
+	length := math.Sqrt(x*x + y*y + z*z + w*w)
+	x /= length
+	y /= length
+	z /= length
+	w /= length
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	if math.IsNaN(pitch) {
+		return 0.0
+	}
+	return pitch
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+
+	d := math.Sqrt((point2.X-point1.X)*(point2.X-point1.X) + (point2.Y-point1.Y)*(point2.Y-point1.Y))
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag1 := OutsideWorkshopFlag.(bool)
+	if OutsideWorkshopFlag1 == true {
+		if count1%10 == 0 {
+			pitch := QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw)
+			//fmt.Println(pitch)
+			if pitch < threshold {
+				UphillPointSlice[0] = append(UphillPointSlice[0], data.PositionX)
+				UphillPointSlice[1] = append(UphillPointSlice[1], data.PositionY)
+				if len(UphillPointSlice[0]) > 250 {
+
+					P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+					P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+					distance := distance(P1, P2)
+					if distance >= 100 {
+						event_lable := "LongUphill"
+						fmt.Println(event_lable)
+						CountNoneUphill = 0
+						UphillPointSlice = [][]float64{{}, {}}
+						return Label()
+					}
+
+				}
+			} else {
+				CountNoneUphill++
+				if CountNoneUphill == 3 {
+					if len(UphillPointSlice[0]) > 1 {
+						P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+						P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+						distance := distance(P1, P2)
+						if distance >= 100 {
+							event_lable := "LongUphill"
+							fmt.Println(event_lable)
+							return Label()
+						}
+						UphillPointSlice = [][]float64{{}, {}}
+					}
+					CountNoneUphill = 0
+				}
+			}
+			count1 = 1
+		}
+		count1++
+	}
+
+	return ""
+}

trigger/jili/cicv_location/LongUphill/main/LongUphill.go

@@ -0,0 +1,107 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+var (
+	threshold        = 0.05
+	count1           = 0
+	UphillPointSlice = [][]float64{{}, {}}
+	CountNoneUphill  = 0
+)
+
+type Point struct {
+	X float64
+	Y float64
+}
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "LongUphill"
+}
+
+// QuaternionToEuler 将四元数转换为欧拉角
+func QuaternionToEuler(x, y, z, w float64) float64 {
+	// 归一化四元数
+	length := math.Sqrt(x*x + y*y + z*z + w*w)
+	x /= length
+	y /= length
+	z /= length
+	w /= length
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	if math.IsNaN(pitch) {
+		return 0.0
+	}
+	return pitch
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+
+	d := math.Sqrt((point2.X-point1.X)*(point2.X-point1.X) + (point2.Y-point1.Y)*(point2.Y-point1.Y))
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	if OutsideWorkshopFlag == true {
+		if count1%10 == 0 {
+			pitch := QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw)
+			//fmt.Println(pitch)
+			if pitch >= threshold {
+				UphillPointSlice[0] = append(UphillPointSlice[0], data.PositionX)
+				UphillPointSlice[1] = append(UphillPointSlice[1], data.PositionY)
+				if len(UphillPointSlice[0]) > 250 {
+
+					P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+					P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+					distance := distance(P1, P2)
+					if distance >= 100 {
+						event_lable := "LongUphill"
+						fmt.Println(event_lable)
+						CountNoneUphill = 0
+						UphillPointSlice = [][]float64{{}, {}}
+						return Label()
+					}
+				}
+			} else {
+				CountNoneUphill++
+				if CountNoneUphill == 3 {
+					if len(UphillPointSlice[0]) > 1 {
+						P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+						P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+						distance := distance(P1, P2)
+						if distance >= 100 {
+							event_lable := "LongUphill"
+							fmt.Println(event_lable)
+							return Label()
+						}
+
+						UphillPointSlice = [][]float64{{}, {}}
+					}
+					CountNoneUphill = 0
+				}
+			}
+			count1 = 1
+		}
+		count1++
+	}
+
+	return ""
+}

trigger/jili/cicv_location/OverSpeed/main/OverSpeed.go

@@ -0,0 +1,33 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "OverSpeed"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	//threshold := 65.0
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	threshold := 10.0
+	if math.Pow(math.Pow(data.VelocityX, 2)+math.Pow(data.VelocityY, 2), 0.5) >= threshold && OutsideWorkshopFlag == true {
+		return "OverSpeed"
+	} else {
+		return ""
+	}
+}

trigger/jili/cicv_location/OverSwing/main/OverSwing.go

@@ -0,0 +1,53 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+/*
+def callback_cicv_location(data):
+    global angular_velocity_z
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw
+
+
+    Ego_position_x=data.position_x
+    Ego_position_y=data.position_y
+    Ego_yaw=data.yaw
+    #print(Ego_yaw)
+    angular_velocity_z=data.angular_velocity_z
+    #print(angular_velocity_z)
+    if abs(angular_velocity_z)>=27:
+        event_label='overswing' #横摆角速度过大
+        print(event_label)
+
+*/
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "OverSwing"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	automodeOfPjVehicleFdbPub, ok := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+	OutsideWorkshopFlag, ok1 := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	if ok && ok1 {
+		if math.Abs(data.AngularVelocityZ) >= 27.0 && automodeOfPjVehicleFdbPub.(int16) == 1 && OutsideWorkshopFlag == true {
+			return Label()
+		}
+	}
+	return ""
+}

trigger/jili/cicv_location/OverspeedAtNight/main/OverspeedAtNight.go

@@ -0,0 +1,60 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+	"time"
+)
+
+var (
+	threshold float64 = 25 / 3.6
+	count1    int     = 0
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "OverspeedAtNight"
+}
+
+func Ifatnight() bool {
+	// 获取当前时间
+	now := time.Now()
+	later := now.Add(0 * time.Hour)
+	// 获取当前小时
+	hour := later.Hour()
+	// 判断当前时间是白天还是夜晚
+	if hour >= 0 && hour < 5 || hour >= 20 && hour <= 23 {
+		return true
+	} else {
+		return false
+	}
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%100 == 0 {
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		Automode = Automode.(int16)
+		if OutsideWorkshopFlag.(bool) && Automode == 1 && Ifatnight() {
+			absspeed, ok := shareVars.Load("AbsSpeed")
+			if ok && absspeed.(float64) >= threshold {
+				eventLabel := "OverspeedAtNight"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+		count1 = 1
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/RapidAccel/main/RapidAccel.go

@@ -0,0 +1,30 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "RapidAccel"
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	if data.AccelX*9.8 > 5.0 && OutsideWorkshopFlag == true {
+		return Label()
+	} else {
+		return ""
+	}
+}

trigger/jili/cicv_location/RearVehicleFollowingTooCloseHigh/main/RearVehicleFollowingTooCloseHigh.go

@@ -0,0 +1,71 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	StartTime int64
+	count1    int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "RearVehicleFollowingTooCloseHigh"
+}
+
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] >= -12 && obj[0][len(obj[0])-1] <= -1 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	return false
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%20 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) >= 4 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 3 {
+					count1 = 1
+					return Label()
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+
+			}
+		} else {
+			StartTime = 0
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/RearVehicleFollowingTooCloseLow/main/RearVehicleFollowingTooCloseLow.go

@@ -0,0 +1,71 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	StartTime int64
+	count1    int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "RearVehicleFollowingTooCloseLow"
+}
+
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] >= -12 && obj[0][len(obj[0])-1] <= -1 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	return false
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%20 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) >= 1 && AbsSpeed.(float64) <= 3 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 3 {
+					count1 = 1
+					return Label()
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+
+			}
+		} else {
+			StartTime = 0
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/ReverseAndOverspeed/main/ReverseAndOverspeed.go

@@ -0,0 +1,65 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+var (
+	threshold float64 = 15 / 3.6
+	count1    int     = 0
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "ReverseAndOverspeed"
+}
+func calculateDirectionAngle(speedX, speedY float64) float64 {
+	// 使用反正切函数计算方向角
+	angle := math.Atan2(speedY, speedX)
+
+	// 将角度转换为度数
+	angleDegree := angle * (180 / math.Pi)
+
+	// 将角度限定在 0~360 范围内
+	if angleDegree < 0 {
+		angleDegree += 360
+	} else if angleDegree > 360 {
+		angleDegree -= 360
+	}
+
+	return angleDegree
+}
+
+// 主进程的逻辑是先判断触发再缓存全局变量
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%200 == 0 {
+		OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
+		if ok3 {
+			directionAngle := calculateDirectionAngle(data.VelocityX, data.VelocityY)
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			diffAngle := math.Abs(float64(directionAngle - data.Yaw))
+			//fmt.Println(diffAngle)
+			if AbsSpeed.(float64) >= threshold && diffAngle >= 160 && diffAngle <= 200 && OutsideWorkshopFlag.(bool) {
+
+				eventLabel := "ReverseAndOverspeed"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/SpeedBumpOverspeed/main/SpeedBumpOverspeed.go

@@ -0,0 +1,90 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	threshold   float64 = 4.0
+	IsSpeedBump bool
+	count1      int64
+	//定义园减速带经纬度
+	point3     = Point{39.73002669559404, 116.49020088105833}
+	point4     = Point{39.728688903362254, 116.4896802037327}
+	point5     = Point{39.728406077854494, 116.48901074348075}
+	point6     = Point{39.7287051534828, 116.48822784823321}
+	point7     = Point{39.72925799042968, 116.49074368869654}
+	pointlist1 = []Point{point3, point4, point5, point6, point7}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "SpeedBumpOverspeed"
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Automode = Automode.(int16)
+		IsSpeedBump = IfEnter(pointlist1, 7.0, data.Latitude, data.Longitude)
+
+		if Automode == 1 && IsSpeedBump {
+			absspeed, ok := shareVars.Load("AbsSpeed")
+			if ok && absspeed.(float64) >= threshold {
+				eventLabel := "SpeedBumpOverspeed"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/TargetCarBehindWhenReversing/main/TargetCarBehindWhenReversing.go

@@ -0,0 +1,83 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "TargetCarBehindWhenReversing"
+}
+
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] >= -15 && obj[0][len(obj[0])-1] <= -1 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 6 {
+				return true
+
+			}
+		}
+
+	}
+	return false
+}
+func calculateDirectionAngle(speedX, speedY float64) float64 {
+	// 使用反正切函数计算方向角
+	angle := math.Atan2(speedY, speedX)
+
+	// 将角度转换为度数
+	angleDegree := angle * (180 / math.Pi)
+
+	// 将角度限定在 0~360 范围内
+	if angleDegree < 0 {
+		angleDegree += 360
+	} else if angleDegree > 360 {
+		angleDegree -= 360
+	}
+
+	return angleDegree
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%100 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		directionAngle := calculateDirectionAngle(data.VelocityX, data.VelocityY)
+		diffAngle := math.Abs(float64(directionAngle - data.Yaw))
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if flag && AbsSpeed.(float64) >= 1 && diffAngle >= 160 && diffAngle <= 200 {
+				eventLabel := "TargetCarBehindWhenReversing"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+
+		}
+	}
+	count1++
+	return ""
+}

trigger/jili/cicv_location/TurnAround/main/TurnAround.go

@@ -0,0 +1,55 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+// 记得在produce_window.go中将Yowslice的注释解除！！！
+var (
+	count1 int = 0
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "TurnAround"
+}
+func IfTurnAround(Yowslice []float64) bool {
+	diffangle := math.Abs(Yowslice[0] - Yowslice[len(Yowslice)-1])
+	//fmt.Println(diffangle)
+	if diffangle >= 110 {
+		return true
+	}
+	return false
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%200 == 0 {
+		OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
+		Yowslice, ok := shareVars.Load("Yowslice")
+		if ok3 && ok && OutsideWorkshopFlag.(bool) {
+			if IfTurnAround(Yowslice.([]float64)) {
+				eventLabel := "TurnAround"
+				fmt.Println(eventLabel)
+				count1 = 1
+				Yowslice = make([]float64, 0)
+				shareVars.Store("Yowslice", Yowslice)
+
+				return Label()
+			}
+		}
+		count1 = 1
+	}
+	count1++
+	return ""
+}