Merge pull request #4 from ll7/fix_range_sensor

Fix range sensor

.gitignore

@@ -1,4 +1,3 @@
-.vscode
 __pycache__/
 .scannerwork
 .pytest_cache

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(

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)