From 06083812340ecbbdc3902ef2a566ef253330fb50 Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Mon, 20 Jan 2025 12:04:51 -0500 Subject: [PATCH] Speed up 2d lidar by ignoring faraway objects. --- rotorpy/sensors/range_sensors.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rotorpy/sensors/range_sensors.py b/rotorpy/sensors/range_sensors.py index 5ee21d9..d19a2eb 100644 --- a/rotorpy/sensors/range_sensors.py +++ b/rotorpy/sensors/range_sensors.py @@ -124,6 +124,17 @@ class TwoDRangeSensor(): distances = np.ones((self.N_rays,))*1e5 # Begin with for object_edges in self.world_edges: # Object edges is a list of tuples, each tuple describing one of the four edges + + # Check if the object is within the field of view of the sensor + xmin = object_edges[0][0] + xmax = object_edges[1][0] + ymin = object_edges[0][1] + ymax = object_edges[2][1] + + if (xmin > x + self.Dmax) or (xmax < x - self.Dmax) or (ymin > y + self.Dmax) or (ymax < y - self.Dmax): + # Don't check if the object is outside the field of view + continue + for edge in object_edges: # First collect the coordinates for point 1 and 2 corresponding to this edge x1 = edge[0]