commit

trigger/pjisuv/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()
+
+
+ 
+    
+    
+   
+
+
+        
+
+
+
+
+