Check if a target object is within a certain distance in front of a reference object. :param target_transform: location of the target object :param current_transform: location of the reference object :param orientation: orientation of the reference object :param max_distance: m
(target_transform, current_transform, max_distance)
| 40 | return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) |
| 41 | |
| 42 | def is_within_distance_ahead(target_transform, current_transform, max_distance): |
| 43 | """ |
| 44 | Check if a target object is within a certain distance in front of a reference object. |
| 45 | |
| 46 | :param target_transform: location of the target object |
| 47 | :param current_transform: location of the reference object |
| 48 | :param orientation: orientation of the reference object |
| 49 | :param max_distance: maximum allowed distance |
| 50 | :return: True if target object is within max_distance ahead of the reference object |
| 51 | """ |
| 52 | target_vector = np.array([target_transform.location.x - current_transform.location.x, target_transform.location.y - current_transform.location.y]) |
| 53 | norm_target = np.linalg.norm(target_vector) |
| 54 | |
| 55 | # If the vector is too short, we can simply stop here |
| 56 | if norm_target < 0.001: |
| 57 | return True |
| 58 | |
| 59 | if norm_target > max_distance: |
| 60 | return False |
| 61 | |
| 62 | fwd = current_transform.get_forward_vector() |
| 63 | forward_vector = np.array([fwd.x, fwd.y]) |
| 64 | d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) |
| 65 | |
| 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 | """ |
no test coverage detected