diff --git a/script/game_ado_vehicle_filter.py b/script/game_ado_vehicle_filter.py index d175411..ec0f30c 100755 --- a/script/game_ado_vehicle_filter.py +++ b/script/game_ado_vehicle_filter.py @@ -165,8 +165,7 @@ def relevant_filter(pos_x, pos_y, yaw, object): ) car_yaw = tf.transformations.euler_from_quaternion(quat)[2] ori_car = [np.cos(car_yaw), np.sin(car_yaw)] - ori_circle = [-car_pos[0], -car_pos[1]] - ori_circle = ori_circle/np.linalg.norm(ori_circle) + ori_circle = np.negative(car_pos)/np.linalg.norm(car_pos) rel_ang = np.dot(ori_circle, ori_car) rospy.logdebug("Position of ado: {}".format(car_pos))