Check if a target object is within a certain distance from a reference object. A vehicle in front would be something around 0 deg, while one behind around 180 deg. :param target_location: location of the target object :param current_location: location of the reference objec
(target_location, current_location, orientation, max_distance, d_angle_th_up, d_angle_th_low=0)
| 66 | return d_angle < 90.0 |
| 67 | |
| 68 | def is_within_distance(target_location, current_location, orientation, max_distance, d_angle_th_up, d_angle_th_low=0): |
| 69 | """ |
| 70 | Check if a target object is within a certain distance from a reference object. |
| 71 | A vehicle in front would be something around 0 deg, while one behind around 180 deg. |
| 72 | |
| 73 | :param target_location: location of the target object |
| 74 | :param current_location: location of the reference object |
| 75 | :param orientation: orientation of the reference object |
| 76 | :param max_distance: maximum allowed distance |
| 77 | :param d_angle_th_up: upper thereshold for angle |
| 78 | :param d_angle_th_low: low thereshold for angle (optional, default is 0) |
| 79 | :return: True if target object is within max_distance ahead of the reference object |
| 80 | """ |
| 81 | target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) |
| 82 | norm_target = np.linalg.norm(target_vector) |
| 83 | |
| 84 | # If the vector is too short, we can simply stop here |
| 85 | if norm_target < 0.001: |
| 86 | return True |
| 87 | |
| 88 | if norm_target > max_distance: |
| 89 | return False |
| 90 | |
| 91 | forward_vector = np.array( |
| 92 | [math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) |
| 93 | d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) |
| 94 | |
| 95 | return d_angle_th_low < d_angle < d_angle_th_up |
| 96 | |
| 97 | |
| 98 | def compute_magnitude_angle(target_location, current_location, orientation): |
no test coverage detected