cicv_data_closed_loop/resource/cut_trigger_jinlong.py

#!/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()