def __init__(self, linear=None): """init the coordinate attr. Args: linear: a tuple describes the linear in (x, y, z) or (x, y) """ if linear != None: try: assert (len(linear) == 3 or len(linear) == 2) \ and type(linear) == tuple for attr in linear: assert type(attr) == float except AssertionError: logger.warning('Pls set linear into (x, y, z) or (x, y) with float, but I ' + 'get %s' % (str(linear))) # print('Warning: pls set linear into (x, y, z) or (x, y) with float!'+ # ' But i get %s' % (str(linear))) else: if len(linear) == 3: self.x = linear[0] self.y = linear[1] self.z = linear[1] self.attr = linear else: self.x = linear[0] self.y = linear[1] self.z = 0. ## default setting ## self.attr = self.attr = (self.x, self.y, self.z)
def __init__(self, position=None): """init the coordinate attr. Args: position: a tuple describes (x, y, z) or (x, y) """ if position != None: try: assert (len(position) == 3 or len(position) == 2) \ and type(position) == tuple for attr in position: assert type(attr) == float except AssertionError: logger.warning('Pls set position into (x, y, z) or (x, y) with float, but I ' + 'get %s' % (str(position))) # print('Warning: pls set position into (x, y, z) or (x, y) with float!'+ # ' But i get %s' % (str(position))) else: if len(position) == 3: self.x = position[0] self.y = position[1] self.z = position[1] self.attr = position else: self.x = position[0] self.y = position[1] self.z = 0. ## default setting ## self.attr = self.attr = (self.x, self.y, self.z)
def set_time_stamp(self, time): """set time stamp Args: time: must be a float, seted by time.time(), in sec. """ try: assert type(time) == float except AssertionError: logger.warning('Pls set time through time.time()') self.time_stamp = time
def set(self, size): """set geometry size. Args: size: a tuple describes (length, width, height) """ try: assert len(size) == 3 and type(size) == tuple for attr in size: assert type(attr) == float except AssertionError: logger.warning('Pls set size into (length, width, height) with float, but I ' + 'get %s' % (str(size))) else: self.height = size[0] self.width = size[1] self.depth = size[2] self.attr = size
def __init__(self, size=None): """init the gemometry size. Args: size: a tuple describes (height, width, depth) """ if size != None: try: assert (len(size) == 3 and type(size) == tuple) for attr in size: assert type(attr) == float except AssertionError: logger.warning('Pls set size into (length, width, height) with float, but I ' + 'get %s' % (str(size))) #print('Warning: pls set size into (length, width, height) with float!') else: self.height = size[0] self.width = size[1] self.depth = size[2] self.attr = size
def set(self, orientation): """set orientation attr Args: orientation: a tuple describes (x, y, z, w) """ try: assert (len(orientation) == 4 and type(orientation) == tuple) for attr in orientation: assert type(attr) == float except AssertionError: logger.warning( 'Pls set orientation into (x, y, z, w) with float, but I ' + 'get %s' % (str(orientation))) else: self.x = orientation[0] self.y = orientation[1] self.z = orientation[2] self.w = orientation[3] self.attr = orientation
def __init__(self, orientation=None): """init the orientation attr. Args: orientation: a tuple describes quaternion (x, y, z, w) """ if orientation != None: try: assert (len(orientation) == 4 and type(orientation) == tuple) for attr in orientation: assert type(attr) == float except AssertionError: logger.warning( 'Pls set orientation into (x, y, z, w) with float, but I ' + 'get %s' % (str(orientation))) # print('Warning: pls set orientation into (x, y, z, w) with float!'+ # ' But i get %s' % (str(orientation))) else: self.x = orientation[0] self.y = orientation[1] self.z = orientation[2] self.w = orientation[3] self.attr = orientation
def set(self, angular): """set angular rate Args: angular: a tuple describes (x, y, z) or (z,) or a float z, represents angular rate in each axis. """ try: if type(angular) == tuple: assert (len(angular) == 3 or len(angular) == 1) if type(angular) != float: for attr in angular: assert type(attr) == float else: assert type(angular) == float except AssertionError: logger.warning( 'Pls set angular into (x, y, z), (z,) or z with float, but I ' + 'get %s' % (str(angular))) # print('Warning: Pls set angular into (x, y, z), (z,) or z with float!'+ # ' But i get %s'%(str(angular))) else: if type(angular) == tuple: if len(angular) == 3: self.x = angular[0] self.y = angular[1] self.z = angular[2] self.attr = angular elif len(angular) == 1: self.x = 0. self.y = 0. self.z = angular[0] ## default setting ## self.attr = (self.x, self.y, self.z) else: self.x = 0. self.y = 0. self.z = angular ## default setting ## self.attr = (self.x, self.y, self.z)
def set(self, linear): """set linear attr Args: linear: a tuple describes (x, y, z) or (x, y) """ try: assert (len(linear) == 3 or len(linear) == 2) \ and type(linear) == tuple for attr in linear: assert type(attr) == float except AssertionError: logger.warning('Pls set linear into (x, y, z) or (x, y) with float, but I ' + 'get %s' % (str(linear))) else: if len(linear) == 3: self.x = linear[0] self.y = linear[1] self.z = linear[2] self.attr = linear else: self.x = linear[0] self.y = linear[1] self.z = 0. ## default setting ## self.attr = (self.x, self.y, self.z)
def set(self, position): """set position attr Args: position: a tuple describes (x, y, z) or (x, y) """ try: assert (len(position) == 3 or len(position) == 2) \ and type(position) == tuple for attr in position: assert type(attr) == float except AssertionError: logger.warning('Pls set position into (x, y, z) or (x, y) with float, but I ' + 'get %s' % (str(position))) else: if len(position) == 3: self.x = position[0] self.y = position[1] self.z = position[2] self.attr = position else: self.x = position[0] self.y = position[1] self.z = 0. ## default setting ## self.attr = (self.x, self.y, self.z)
def __init__(self, angular=None): """init the coordinate attr. Args: angular: a tuple describes (x, y, z) or (z,) or a float z """ if angular != None: try: if type(angular) == tuple: assert (len(angular) == 3 or len(angular) == 1) if type(angular) != float: for attr in angular: assert type(attr) == float else: assert type(angular) == float except AssertionError: logger.warning( 'Pls set angular into (x, y, z), (z,) or z with float, but I ' + 'get %s' % (str(angular))) #print('Warning: pls set angular into (x, y, z), (z,) or z with float!') else: if type(angular) == tuple: if len(angular) == 3: self.x = angular[0] self.y = angular[1] self.z = angular[2] self.attr = angular elif len(angular) == 1: self.x = 0 ## default setting ## self.y = 0 ## default setting ## self.z = angular[0] self.attr = (self.x, self.y, self.z) else: self.x = 0 ## default setting ## self.y = 0 ## default setting ## self.z = angular self.attr = (self.x, self.y, self.z)
def _assess_one_obj_threat_score(ego_vehicle, road_obj, weather_type=scene_m.weather.clear, road_state=scene_m.road_state.normal): """assess the road object safety degree for ego vehicle Args: ego_vehicle: a class in obj_state.ego_vehicel road_obj: a class in obj_state.road_obj weather_type: when in unclear weather, it is more likely to be dangerous road_state: when road is wetness, snow or leaves, it is more likely to be dangerous Return: [Dangerous score, attentive score, safe score] """ def judge_relation_state(ego_pos, ego_size, other_pos, other_size): """jugde the relation between ego vehicle and other obj Args: ego_pos: a tuple describe ego position, (x, y, z) ego_size: a tuple describe ego geometry size, (h, w, d) other_pos: a tuple describe other vehicle position, (x, y, z) other_size: a tuple describe other vehicle geometry size, (h, w, d) Return: a relation_state """ ego_vehicle_radius = math.sqrt((ego_size[1]/2)**2 + (ego_size[2]/2)**2) other_obj_radius = math.sqrt((other_size[1]/2)**2 + (other_size[2]/2)**2) length = ego_vehicle_radius + other_obj_radius m_distance_x = math.fabs(ego_pos[0] - other_pos[0]) m_distance_y = math.fabs(ego_pos[1] - other_pos[1]) if m_distance_x <= length or m_distance_y <= length: if m_distance_x <= length and m_distance_y <= length: return relation_state.all_overlap elif m_distance_x <= length: return relation_state.overlap_in_x else: return relation_state.overlap_in_y else: return relation_state.none_overlap def time_to_collision_2d(ego_pos, ego_linear, ego_size, other_pos, other_linear, other_size, dim='xy'): """calculate TTC in x,y dimention, TTC is calculated as: TTC = (x[i-1] - x[i] - L) / (v[i] - v[i-1]), where i indicates the vehicle position which has the bigger velocity. When TTC > 0, it indicates that the remaining time in which two vehicles would collide. When TTC < 0, in indicates that it's impossible for two cars to collide. Args: ego_pos: a tuple describe ego position, (x, y, z) ego_linear: a tuple describe ego linear velocity in (x, y, z) dimention ego_size: a tuple describe ego geometry size, (h, w, d) other_pos: a tuple describe other vehicle position, (x, y, z) other_linear: a tuple describe ego linear velocity in (x, y, z) dimention other_size: a tuple describe other vehicle geometry size, (h, w, d) dim: a string indicates calculate TTC in which dimention, must be 'xy', 'y' or 'x'. Return: ttc_in_x: the remaining time in which two vehicles would collide in x dimention. ttc_in_y: the remaining time in which two vehicles would collide in y dimention. """ ## assert ## assert dim in ['xy', 'x', 'y'] ## length ## ego_vehicle_radius = math.sqrt((ego_size[1] / 2) ** 2 + (ego_size[2] / 2) ** 2) other_obj_radius = math.sqrt((other_size[1] / 2) ** 2 + (other_size[2] / 2) ** 2) length = ego_vehicle_radius + other_obj_radius ## 1e-5 is to avoid zero-divide error ## if ego_linear[0] >= other_linear[0]: ttc_in_x = (other_pos[0] - ego_pos[0] - length) / (ego_linear[0] - other_linear[0] + 1e-5) else: ttc_in_x = (ego_pos[0] - other_pos[0] - length) / (other_linear[0] - ego_linear[0] + 1e-5) if ego_linear[1] >= other_linear[1]: ttc_in_y = (other_pos[1] - ego_pos[1] - length) / (ego_linear[1] - other_linear[1] + 1e-5) else: ttc_in_y = (ego_pos[1] - other_pos[1] - length) / (other_linear[1] - ego_linear[1] + 1e-5) if dim == 'xy': return ttc_in_x, ttc_in_y elif dim == 'x': return ttc_in_x elif dim == 'y': return ttc_in_y else: raise ValueError("Imposible get here!!! Something must wrong!!!") def much_bigger(val, bigger_than): """ normally, the val should be a abs of subtraction value Return: if val >= bigger_than: True else: False """ if val >= bigger_than: return True else: return False def time_to_escape_2d(ego_pos, ego_linear, ego_size, other_pos, other_linear, other_size, dim='xy'): """When the relation between ego vehicle and other obj is overlap, we need this indicator to indicate the remaining time in which the ego(or other obj) can escape the overlap area. Args: ego_pos: a tuple describe ego position, (x, y, z) ego_linear: a tuple describe ego linear velocity in (x, y, z) dimention ego_size: a tuple describe ego geometry size, (h, w, d) other_pos: a tuple describe other vehicle position, (x, y, z) other_linear: a tuple describe ego linear velocity in (x, y, z) dimention other_size: a tuple describe other vehicle geometry size, (h, w, d) dim: a string indicates calculate TTE in which dimention, must be 'xy', 'y' or 'x'. Return: TTE in specific dimention """ ## length ## ego_vehicle_radius = math.sqrt((ego_size[1] / 2) ** 2 + (ego_size[2] / 2) ** 2) other_obj_radius = math.sqrt((other_size[1] / 2) ** 2 + (other_size[2] / 2) ** 2) length = ego_vehicle_radius + other_obj_radius ## 1e-8 is to avoid zero-divide error ## if ego_linear[0] >= other_linear[0]: tte_in_x = (other_pos[0] - ego_pos[0] + length) / (ego_linear[0] - other_linear[0] + 1e-8) else: tte_in_x = (ego_pos[0] - other_pos[0] + length) / (other_linear[0] - ego_linear[0] + 1e-8) if ego_linear[1] >= other_linear[1]: tte_in_y = (other_pos[1] - ego_pos[1] + length) / (ego_linear[1] - other_linear[1] + 1e-8) else: tte_in_y = (ego_pos[1] - other_pos[1] + length) / (other_linear[1] - ego_linear[1] + 1e-8) if dim == 'xy': return tte_in_x, tte_in_y elif dim == 'x': return tte_in_x elif dim == 'y': return tte_in_y else: raise ValueError("Imposible get here!!! Something must wrong!!!") def softmax(x): return np.exp(x) / np.expand_dims(np.sum(np.exp(x), axis=-1),axis=-1) def cos_vec(vec1, vec2): """caculate the cos val between two vec Args: vec1: ndarray with a shape [n_dim] vec2: ndarray with a shape [n_dim] """ cos = np.sum(vec1*vec2) / (np.sqrt(np.sum(np.square(vec1)))*np.sqrt(np.sum(np.square(vec2))) +1e-6) return cos ## assert ## assert type(ego_vehicle) == ego_v.ego_vehicle assert type(road_obj) == road_o.road_obj assert weather_type in list(scene_m.weather.__members__.values()) assert road_state in list(scene_m.road_state.__members__.values()) ## init safety degree score ## safety_score = np.zeros(shape=[len(list(safety_degree.__members__.values()))]) ## ego_pos = ego_vehicle.get_position() ego_linear = ego_vehicle.get_linear() ego_size = ego_vehicle.get_size() other_pos = road_obj.get_position() other_size = road_obj.get_size() other_linear = road_obj.get_linear() r_state = judge_relation_state(ego_pos, ego_size, other_pos, other_size) # print('r_state:', r_state) if r_state is relation_state.none_overlap: ttc_in_x, ttc_in_y = time_to_collision_2d(ego_pos, ego_linear, ego_size, other_pos, other_linear, other_size) if ttc_in_x < 0. or ttc_in_y < 0.: ## safe ## return np.array([0., 0., 1.]), -1. else: vel_m_s = math.sqrt(ego_linear[0] ** 2 + ego_linear[1] ** 2) ## m/s vel_m_s_x = math.fabs(ego_linear[0]) vel_m_s_y = math.fabs(ego_linear[1]) c_t_thresh = (vel_m_s/COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[weather_type] c_t_thresh_x = (vel_m_s_x / COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type] c_t_thresh_y = (vel_m_s_y / COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type] ## maybe unsafe ## if much_bigger(math.fabs(ttc_in_y - ttc_in_x), c_t_thresh): ## safe ## return np.array([0., 0., 1.]), -1. elif much_bigger(ttc_in_x, c_t_thresh_x) and much_bigger(ttc_in_y, c_t_thresh_y): ## safe ## return np.array([0., 0., 1.]), -1. else: ## safe, attentive or dagerous ## ## we use the speedofometer to calculate the threshold of different safety degree # vel_km_h = vel_m_s / .27777 ## km/h # thresh_w = np.array(safe_distance_config[weather_type])*vel_km_h/vel_m_s ##add safe_distance factor and weatehr factor # thresh_ratio_r = road_state_ratio[road_state] ##add road # thresh = thresh_w*thresh_ratio_r ##add thresh = [(vel_m_s/(MAX_DECELERATION*road_state_ratio[road_state])*weather_effect_ratio[weather_type]), (vel_m_s/(AVG_DECELERATION*road_state_ratio[road_state])*weather_effect_ratio[weather_type]), (vel_m_s/(COMFORTABLE_DECELERATION*road_state_ratio[road_state])*weather_effect_ratio[weather_type])] dangerous_score = gaussian_1d(x=max(ttc_in_y, ttc_in_x), mean=thresh[0], for_what=safety_degree.dangerous, std=(vel_m_s*3.6)/20, max=(vel_m_s*3.6)/2) attentive_score = gaussian_1d(x=max(ttc_in_y, ttc_in_x), mean=thresh[1], for_what=safety_degree.attentive, std=(vel_m_s*3.6)/20, max=(vel_m_s*3.6)/2) safe_score = gaussian_1d(x=max(ttc_in_y, ttc_in_x), mean=thresh[2], for_what=safety_degree.safe, std=(vel_m_s*3.6)/20, max=(vel_m_s*3.6)/2) return softmax(np.array([dangerous_score, attentive_score, safe_score])), -1. elif r_state is relation_state.overlap_in_x: tte_in_x = time_to_escape_2d(ego_pos, ego_linear, ego_size, other_pos, other_linear, other_size, dim='x') ttc_in_y = time_to_collision_2d(ego_pos, ego_linear, ego_size, other_pos, other_linear, other_size, dim='y') try: assert tte_in_x >= 0. except AssertionError: logger.warning('Get a negative tte val(%f) in x, something may be wrong!'%(tte_in_x)) if ttc_in_y < 0.: ## safe ## return np.array([0., 0., 1.]), -1. else: vel_m_s = math.sqrt(ego_linear[0] ** 2 + ego_linear[1] ** 2) ## m/s vel_m_s_x = math.fabs(ego_linear[0] - other_linear[1]) vel_m_s_y = math.fabs(ego_linear[1] - other_linear[1]) # if much_bigger(ttc_in_y - tte_in_x, # vel_m_s/(COMFORTABLE_DECELERATION*road_state_ratio[road_state]*weather_effect_ratio[weather_type])): # ## safe ## # return np.array([0., 0., 1.]) if tte_in_x >= ttc_in_y: if much_bigger(ttc_in_y, (vel_m_s_y / ((COMFORTABLE_DECELERATION * road_state_ratio[road_state])) \ * weather_effect_ratio[weather_type])): ## safe ## return np.array([0., 0., 1.]), ttc_in_y else: ## safe, attentive, dangerous ## thresh = [ (vel_m_s_y / (MAX_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[weather_type]), (vel_m_s_y / (AVG_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[weather_type]), (vel_m_s_y / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type])] dangerous_score = gaussian_1d(x=ttc_in_y, mean=thresh[0], for_what=safety_degree.dangerous, std=(vel_m_s_y * 3.6) / 20, max=(vel_m_s_y * 3.6) / 2) attentive_score = gaussian_1d(x=ttc_in_y, mean=thresh[1], for_what=safety_degree.attentive, std=(vel_m_s_y * 3.6) / 20, max=(vel_m_s_y * 3.6) / 2) safe_score = gaussian_1d(x=ttc_in_y, mean=thresh[2], for_what=safety_degree.safe, std=(vel_m_s_y * 3.6) / 20, max=(vel_m_s_y * 3.6) / 2) return softmax(np.array([dangerous_score, attentive_score, safe_score])), ttc_in_y else: if much_bigger(ttc_in_y, (vel_m_s_y / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) \ * weather_effect_ratio[weather_type])): ## safe ## return np.array([0., 0., 1.]), ttc_in_y else: thresh = [(vel_m_s / ( MAX_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[weather_type]), (vel_m_s / (AVG_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type]), (vel_m_s / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type])] ## safe attentive dangerous ## dangerous_score = gaussian_1d(x=math.fabs(ttc_in_y - tte_in_x), mean=thresh[0], for_what=safety_degree.dangerous, std=(vel_m_s * 3.6) / 20, max=(vel_m_s * 3.6) / 2) attentive_score = gaussian_1d(x=math.fabs(ttc_in_y - tte_in_x), mean=thresh[1], for_what=safety_degree.attentive, std=(vel_m_s * 3.6) / 20, max=(vel_m_s * 3.6) / 2) safe_score = gaussian_1d(x=math.fabs(ttc_in_y - tte_in_x), mean=thresh[2], for_what=safety_degree.safe, std=(vel_m_s * 3.6) / 20, max=(vel_m_s * 3.6) / 2) return softmax(np.array([dangerous_score, attentive_score, safe_score])), ttc_in_y elif r_state is relation_state.overlap_in_y: tte_in_y = time_to_escape_2d(ego_pos, ego_linear, ego_size, other_pos, other_linear, other_size, dim='y') ttc_in_x = time_to_collision_2d(ego_pos, ego_linear, ego_size, other_pos, other_linear, other_size, dim='x') try: assert tte_in_y >= 0. except AssertionError: logger.warning('Get a negative tte val(%f) in y, something may be wrong!' % (tte_in_y)) if ttc_in_x < 0.: ## safe ## return np.array([0., 0., 1.]), ttc_in_x else: vel_m_s = math.sqrt(ego_linear[0] ** 2 + ego_linear[1] ** 2) ## m/s vel_m_s_x = math.fabs(ego_linear[0] - other_linear[0]) vel_m_s_y = math.fabs(ego_linear[1] - other_linear[0]) # if much_bigger(ttc_in_y - tte_in_x, # vel_m_s/(COMFORTABLE_DECELERATION*road_state_ratio[road_state]*weather_effect_ratio[weather_type])): # ## safe ## # return np.array([0., 0., 1.]) if tte_in_y >= ttc_in_x: if much_bigger(ttc_in_x, (vel_m_s_x / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) \ * weather_effect_ratio[weather_type])): ## safe ## return np.array([0., 0., 1.]), ttc_in_x else: ## safe, attentive, dangerous ## thresh = [ (vel_m_s_x / (MAX_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type]), (vel_m_s_x / (AVG_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type]), (vel_m_s_x / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type])] dangerous_score = gaussian_1d(x=ttc_in_x, mean=thresh[0], for_what=safety_degree.dangerous, std=(vel_m_s_x * 3.6) / 20, max=(vel_m_s_x * 3.6) / 2) attentive_score = gaussian_1d(x=ttc_in_x, mean=thresh[1], for_what=safety_degree.attentive, std=(vel_m_s_x * 3.6) / 20, max=(vel_m_s_x * 3.6) / 2) safe_score = gaussian_1d(x=ttc_in_x, mean=thresh[2], for_what=safety_degree.safe, std=(vel_m_s_x * 3.6) / 20, max=(vel_m_s_x * 3.6) / 2) return softmax(np.array([dangerous_score, attentive_score, safe_score])), ttc_in_x else: if much_bigger(ttc_in_x, (vel_m_s_x / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) \ * weather_effect_ratio[weather_type])): ## safe ## return np.array([0., 0., 1.]),ttc_in_x else: thresh = [(vel_m_s / ( MAX_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[weather_type]), (vel_m_s / (AVG_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type]), (vel_m_s / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type])] ## safe attentive dangerous ## dangerous_score = gaussian_1d(x=math.fabs(ttc_in_x - tte_in_y), mean=thresh[0], for_what=safety_degree.dangerous, std=(vel_m_s * 3.6) / 20, max=(vel_m_s * 3.6) / 2) attentive_score = gaussian_1d(x=math.fabs(ttc_in_x - tte_in_y), mean=thresh[1], for_what=safety_degree.attentive, std=(vel_m_s * 3.6) / 20, max=(vel_m_s * 3.6) / 2) safe_score = gaussian_1d(x=math.fabs(ttc_in_x - tte_in_y), mean=thresh[2], for_what=safety_degree.safe, std=(vel_m_s * 3.6) / 20, max=(vel_m_s * 3.6) / 2) return softmax(np.array([dangerous_score, attentive_score, safe_score])), ttc_in_x elif r_state is relation_state.all_overlap: ## length ## ego_vehicle_radius = math.sqrt((ego_size[1] / 2) ** 2 + (ego_size[2] / 2) ** 2) other_obj_radius = math.sqrt((other_size[1] / 2) ** 2 + (other_size[2] / 2) ** 2) length = ego_vehicle_radius + other_obj_radius ## pos_vec ## ego_pos_vec = np.array([ego_pos[0], ego_pos[1]]) other_pos_vec = np.array([other_pos[0], other_pos[1]]) pos_vec = ego_pos_vec - other_pos_vec pos_scalar = np.sqrt(np.sum(np.square(pos_vec))) ## vel_vec ## ego_vel_vec = np.array([ego_linear[0], ego_linear[1]]) other_vel_vec = np.array([other_linear[0], other_linear[1]]) vel_vex = other_vel_vec - ego_vel_vec vel_scalar = np.sqrt(np.sum(np.square(vel_vex))) ego_vel_scalar = np.sqrt(np.sum(np.square(ego_vel_vec))) ## m/s cos = cos_vec(pos_vec, vel_vex) if cos <= 0.: ##means the distance between ego and other obj would increase. return np.array([0., 0., 1.]), -1 else: ##means the distance between ego and other obj would decrease. vel_rel = vel_scalar * cos ttc = (pos_scalar - length) / vel_rel ## safe, attentive, dangerous ## thresh = [ (ego_vel_scalar / (MAX_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type]), (ego_vel_scalar / (AVG_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type]), (ego_vel_scalar / (COMFORTABLE_DECELERATION * road_state_ratio[road_state]) * weather_effect_ratio[ weather_type])] dangerous_score = gaussian_1d(x=ttc, mean=thresh[0], for_what=safety_degree.dangerous, std=(ego_vel_scalar * 3.6) / 20, max=(ego_vel_scalar * 3.6) / 2) attentive_score = gaussian_1d(x=ttc, mean=thresh[1], for_what=safety_degree.attentive, std=(ego_vel_scalar * 3.6) / 20, max=(ego_vel_scalar * 3.6) / 2) safe_score = gaussian_1d(x=ttc, mean=thresh[2], for_what=safety_degree.safe, std=(ego_vel_scalar * 3.6) / 20, max=(ego_vel_scalar * 3.6) / 2) return softmax(np.array([dangerous_score, attentive_score, safe_score])), ttc else: raise ValueError("Imposible get here!!! Something must wrong!!!")