Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- # find the closest mid-lane point to the agent centroid
- dists_to_mid_points = np.linalg.norm(centroid - lanes_mid_points, axis=1)
- i_min_dist_to_mid = dists_to_mid_points.argmin()
- mid_point = lanes_mid_points[i_min_dist_to_mid]
- # disqualify the point if there is any left-lane or right-lane point closer to mid_point than the centroid
- min_dist_to_left = np.min(np.linalg.norm(mid_point - lanes_left_points, axis=1))
- min_dist_to_right = np.min(np.linalg.norm(mid_point - lanes_right_points, axis=1))
- dist_to_centroid = np.linalg.norm(mid_point - centroid)
- if dist_to_centroid > min_dist_to_left or dist_to_centroid > min_dist_to_right:
- if verbose:
- print(f'Agent {agent_name} discarded, dist_to_centroid: {dist_to_centroid},'
- f' min_dist_to_left: {min_dist_to_left}, min_dist_to_right: {min_dist_to_right}')
- return False
- if verbose:
- print(f'Agent {agent_name} is OK')
- return True
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement