swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Speed up 2d lidar by ignoring faraway objects.

Browse Source
This commit is contained in:
spencerfolk
2025-01-20 12:04:51 -05:00
parent d355a11e08
commit 0608381234
1 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
rotorpy/sensors/range_sensors.py
View File

@@ -124,6 +124,17 @@ class TwoDRangeSensor():
distances = np.ones((self.N_rays,))*1e5 # Begin with distances = np.ones((self.N_rays,))*1e5 # Begin with
for object_edges in self.world_edges: for object_edges in self.world_edges:
# Object edges is a list of tuples, each tuple describing one of the four 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: for edge in object_edges:
# First collect the coordinates for point 1 and 2 corresponding to this edge # First collect the coordinates for point 1 and 2 corresponding to this edge
x1 = edge[0] x1 = edge[0]