From 44987f1446e466c2693d7f209801b943919a13dc Mon Sep 17 00:00:00 2001 From: Lennart Luttkus Date: Tue, 13 Feb 2024 16:04:05 +0100 Subject: [PATCH 1/7] rename a few variables --- robot_sf/sensor/range_sensor.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/robot_sf/sensor/range_sensor.py b/robot_sf/sensor/range_sensor.py index b031ad3..8d6e62a 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,7 +132,7 @@ 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 From 277c897e10f5e29ea9c22bd657df83b923e77e31 Mon Sep 17 00:00:00 2001 From: Lennart Luttkus Date: Tue, 13 Feb 2024 16:04:37 +0100 Subject: [PATCH 2/7] fix quadratic solution --- robot_sf/sensor/range_sensor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_sf/sensor/range_sensor.py b/robot_sf/sensor/range_sensor.py index 8d6e62a..e54c898 100644 --- a/robot_sf/sensor/range_sensor.py +++ b/robot_sf/sensor/range_sensor.py @@ -134,8 +134,8 @@ def circle_line_intersection_distance( # 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 From bccdf2ab478415c2d39ea0f20ed0f47b1c0ed41d Mon Sep 17 00:00:00 2001 From: Lennart Luttkus Date: Tue, 13 Feb 2024 16:05:45 +0100 Subject: [PATCH 3/7] Add unit tests for range sensor functions --- tests/test_range_sensor.py | 50 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 tests/test_range_sensor.py diff --git a/tests/test_range_sensor.py b/tests/test_range_sensor.py new file mode 100644 index 0000000..f5f8054 --- /dev/null +++ b/tests/test_range_sensor.py @@ -0,0 +1,50 @@ +from robot_sf.sensor.range_sensor import ( + circle_line_intersection_distance, + euclid_dist +) + +# 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) \ No newline at end of file From 7b681d68b2461bb9a4c5656402662b0e8d5db2c0 Mon Sep 17 00:00:00 2001 From: Lennart Luttkus Date: Tue, 13 Feb 2024 16:05:59 +0100 Subject: [PATCH 4/7] track .vscode --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3a37c69..65abc2b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ -.vscode __pycache__/ .scannerwork .pytest_cache From a11d195300d7e42a8f22c90469589821e8d32016 Mon Sep 17 00:00:00 2001 From: Lennart Luttkus Date: Tue, 13 Feb 2024 16:16:45 +0100 Subject: [PATCH 5/7] Add docs for raycasting function to detect pedestrians within scanner's range --- robot_sf/sensor/range_sensor.py | 36 +++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/robot_sf/sensor/range_sensor.py b/robot_sf/sensor/range_sensor.py index e54c898..40cf0c7 100644 --- a/robot_sf/sensor/range_sensor.py +++ b/robot_sf/sensor/range_sensor.py @@ -179,30 +179,66 @@ def __post_init__(self): 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): + """ + 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. + 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 + ------- + None + """ + # 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( From d2e4f2d134d5de7b25e9c7f3c4d13c8c789084c0 Mon Sep 17 00:00:00 2001 From: Lennart Luttkus Date: Tue, 13 Feb 2024 16:17:24 +0100 Subject: [PATCH 6/7] Refactor raycast_pedestrians function signature --- robot_sf/sensor/range_sensor.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/robot_sf/sensor/range_sensor.py b/robot_sf/sensor/range_sensor.py index 40cf0c7..3519a68 100644 --- a/robot_sf/sensor/range_sensor.py +++ b/robot_sf/sensor/range_sensor.py @@ -177,8 +177,13 @@ 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. From d37cb1602bbf883934d5e851a8e4b459fc892ca3 Mon Sep 17 00:00:00 2001 From: Lennart Luttkus Date: Tue, 13 Feb 2024 17:05:15 +0100 Subject: [PATCH 7/7] Add raycast_pedestrians function to range_sensor.py and write tests --- robot_sf/sensor/range_sensor.py | 3 +- tests/test_range_sensor.py | 66 ++++++++++++++++++++++++++++++++- 2 files changed, 66 insertions(+), 3 deletions(-) diff --git a/robot_sf/sensor/range_sensor.py b/robot_sf/sensor/range_sensor.py index 3519a68..a6a81a4 100644 --- a/robot_sf/sensor/range_sensor.py +++ b/robot_sf/sensor/range_sensor.py @@ -191,6 +191,7 @@ def raycast_pedestrians( ---------- 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 @@ -204,7 +205,7 @@ def raycast_pedestrians( Returns ------- - None + output_ranges is modified in place. """ # Check if pedestrian positions array is empty or not 2D diff --git a/tests/test_range_sensor.py b/tests/test_range_sensor.py index f5f8054..d520685 100644 --- a/tests/test_range_sensor.py +++ b/tests/test_range_sensor.py @@ -1,6 +1,8 @@ +import numpy as np from robot_sf.sensor.range_sensor import ( circle_line_intersection_distance, - euclid_dist + euclid_dist, + raycast_pedestrians ) # Circle-line intersection tests @@ -28,6 +30,7 @@ def test_negative_ray_direction(): 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) @@ -47,4 +50,63 @@ def test_negative_coordinates(): 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) \ No newline at end of file + 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