diff --git a/.gitignore b/.gitignore index 3a37c69..65abc2b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ -.vscode __pycache__/ .scannerwork .pytest_cache diff --git a/robot_sf/sensor/range_sensor.py b/robot_sf/sensor/range_sensor.py index b031ad3..a6a81a4 100644 --- a/robot_sf/sensor/range_sensor.py +++ b/robot_sf/sensor/range_sensor.py @@ -108,17 +108,19 @@ def circle_line_intersection_distance( intersection. """ # Unpack circle center and radius, and ray vector - (c_x, c_y), r = circle + (circle_x, circle_y), radius = circle ray_x, ray_y = ray_vec # Shift circle's center to the origin (0, 0) - (p1_x, p1_y) = origin[0] - c_x, origin[1] - c_y + p1_x = origin[0] - circle_x + p1_y = origin[1] - circle_y # Calculate squared radius and norm of p1 - r_sq = r**2 + r_sq = radius**2 norm_p1 = p1_x**2 + p1_y**2 # Coefficients a, b, c of the quadratic solution formula + # ax^2+bx+c=0 s_x, s_y = ray_x, ray_y t_x, t_y = p1_x, p1_y a = s_x**2 + s_y**2 @@ -130,10 +132,10 @@ def circle_line_intersection_distance( if disc < 0 or (b > 0 and b**2 > disc): return np.inf - # compute quadratic solutions + # Compute quadratic solutions disc_root = disc**0.5 - mu_1 = (-b - disc_root) / 2 * a - mu_2 = (-b + disc_root) / 2 * a + mu_1 = (-b - disc_root) / (2 * a) + mu_2 = (-b + disc_root) / (2 * a) # Compute cross points S1, S2 and distances to the origin s1_x, s1_y = mu_1 * s_x + t_x, mu_1 * s_y + t_y @@ -175,32 +177,74 @@ def __post_init__(self): @numba.njit(fastmath=True) def raycast_pedestrians( - out_ranges: np.ndarray, scanner_pos: Vec2D, max_scan_range: float, - ped_positions: np.ndarray, ped_radius: float, ray_angles: np.ndarray): + out_ranges: np.ndarray, + scanner_pos: Vec2D, + max_scan_range: float, + ped_positions: np.ndarray, + ped_radius: float, + ray_angles: np.ndarray + ): + """ + Perform raycasting to detect pedestrians within the scanner's range. + + Parameters + ---------- + out_ranges : np.ndarray + The output array to store the detected range for each ray. + ! This array is modified in place. + scanner_pos : Vec2D + The position of the scanner. + max_scan_range : float + The maximum range of the scanner. + ped_positions : np.ndarray + The positions of the pedestrians. + ped_radius : float + The radius of the pedestrians. + ray_angles : np.ndarray + The angles of the rays. + + Returns + ------- + output_ranges is modified in place. + """ + # Check if pedestrian positions array is empty or not 2D if len(ped_positions.shape) != 2 or ped_positions.shape[0] == 0 \ or ped_positions.shape[1] != 2: return + # Convert scanner position to numpy array scanner_pos_np = np.array([scanner_pos[0], scanner_pos[1]]) + + # Calculate square of maximum scan range threshold_sq = max_scan_range**2 + + # Calculate relative positions of pedestrians and their squared distances relative_ped_pos = ped_positions - scanner_pos_np dist_sq = np.sum(relative_ped_pos**2, axis=1) + + # Find pedestrians within scanner's range ped_dist_mask = np.where(dist_sq <= threshold_sq)[0] close_ped_pos = relative_ped_pos[ped_dist_mask] + # If no pedestrians are within range, return if len(ped_dist_mask) == 0: return + # For each ray angle, calculate cosine similarities and find pedestrians + # in the direction of the ray for i, angle in enumerate(ray_angles): unit_vec = cos(angle), sin(angle) cos_sims = close_ped_pos[:, 0] * unit_vec[0] \ + close_ped_pos[:, 1] * unit_vec[1] + # Find pedestrians in the direction of the ray ped_dir_mask = np.where(cos_sims >= 0)[0] joined_mask = ped_dist_mask[ped_dir_mask] relevant_ped_pos = relative_ped_pos[joined_mask] + # For each pedestrian in the direction of the ray, calculate the + # distance to the pedestrian's edge and update the output range for pos in relevant_ped_pos: ped_circle = ((pos[0], pos[1]), ped_radius) coll_dist = circle_line_intersection_distance( diff --git a/tests/test_range_sensor.py b/tests/test_range_sensor.py new file mode 100644 index 0000000..d520685 --- /dev/null +++ b/tests/test_range_sensor.py @@ -0,0 +1,112 @@ +import numpy as np +from robot_sf.sensor.range_sensor import ( + circle_line_intersection_distance, + euclid_dist, + raycast_pedestrians +) + +# Circle-line intersection tests +def test_intersection_at_origin(): + circle = ((0.0, 0.0), 1.0) # Circle centered at origin with radius 1 + origin = (0.0, 0.0) # Ray starts at origin + ray_vec = (1.0, 0.0) # Ray points along the x-axis + assert circle_line_intersection_distance(circle, origin, ray_vec) == 1.0 + +def test_no_intersection(): + circle = ((0.0, 0.0), 1.0) # Circle centered at origin with radius 1 + origin = (2.0, 2.0) # Ray starts outside the circle + ray_vec = (1.0, 0.0) # Ray points along the x-axis + assert circle_line_intersection_distance(circle, origin, ray_vec) == float('inf') + +def test_intersection_at_circle_perimeter(): + circle = ((0.0, 0.0), 1.0) # Circle centered at origin with radius 1 + origin = (0.0, 0.0) # Ray starts at origin + ray_vec = (1.0, 1.0) # Ray points diagonally + assert circle_line_intersection_distance(circle, origin, ray_vec) == 1.0 + +def test_negative_ray_direction(): + circle = ((0.0, 0.0), 1.0) # Circle centered at origin with radius 1 + origin = (1.0, 0.0) # Ray starts at x=1 + ray_vec = (-1.0, 0.0) # Ray points along the negative x-axis + assert circle_line_intersection_distance(circle, origin, ray_vec) == 0.0 + +################################################################################ +# Euclidean distance tests +def test_same_point(): + vec_1 = (0.0, 0.0) + vec_2 = (0.0, 0.0) + assert euclid_dist(vec_1, vec_2) == 0.0 + +def test_unit_distance(): + vec_1 = (0.0, 0.0) + vec_2 = (1.0, 0.0) + assert euclid_dist(vec_1, vec_2) == 1.0 + +def test_negative_coordinates(): + vec_1 = (0.0, 0.0) + vec_2 = (-1.0, -1.0) + assert euclid_dist(vec_1, vec_2) == (2**0.5) + +def test_non_integer_distance(): + vec_1 = (0.0, 0.0) + vec_2 = (1.0, 1.0) + assert euclid_dist(vec_1, vec_2) == (2**0.5) + +################################################################################ +# Raycasting pedestrians tests +# def test_no_pedestrians(): +# out_ranges = np.array([10.0, 10.0]) +# scanner_pos = (0.0, 0.0) +# max_scan_range = 10.0 +# ped_positions = np.array([], dtype=np.float64) +# ped_radius = 1.0 +# ray_angles = np.array([0.0, np.pi / 2]) + +# raycast_pedestrians( +# out_ranges, +# scanner_pos, +# max_scan_range, +# ped_positions, +# ped_radius, +# ray_angles +# ) + +# assert np.all(out_ranges == 10.0) +# TODO testing with no pedestrians did not work + +def test_pedestrian_in_range(): + out_ranges = np.array([10.0, 10.0]) + scanner_pos = (0.0, 0.0) + max_scan_range = 10.0 + ped_positions = np.array([[5.0, 0.0]]) + ped_radius = 1.0 + ray_angles = np.array([0.0, np.pi / 2]) + + raycast_pedestrians( + out_ranges, + scanner_pos, + max_scan_range, + ped_positions, + ped_radius, + ray_angles) + + assert out_ranges[0] == 4.0 + assert out_ranges[1] == 10.0 + +def test_pedestrian_out_of_range(): + out_ranges = np.array([10.0, 10.0]) + scanner_pos = (0.0, 0.0) + max_scan_range = 10.0 + ped_positions = np.array([[15.0, 0.0]]) + ped_radius = 1.0 + ray_angles = np.array([0.0, np.pi / 2]) + + raycast_pedestrians( + out_ranges, + scanner_pos, + max_scan_range, + ped_positions, + ped_radius, + ray_angles) + + assert np.all(out_ranges == 10.0) \ No newline at end of file