vato007/breezyslam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

RPSPLAM example working without scipy

Browse Source
This commit is contained in:
simondlevy
2018-07-04 17:19:11 -04:00
parent fc368c4cb3
commit b2e4098491
2 changed files with 14 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
c/coreslam.c
View File

@@ -119,10 +119,7 @@ static void interpolate_scan(scan_t * scan, float * lidar_angles_deg, int * lida
for (k=0; k<scan->size; ++k) for (k=0; k<scan->size; ++k)
{ {
lidar_distances_mm[k] = (int)interpolate(interp->angles, interp->distances, scan_size, (float)k); lidar_distances_mm[k] = (int)interpolate(interp->angles, interp->distances, scan_size, (float)k);
printf("%3d: %d\n", k, lidar_distances_mm[k]);
} }
printf("\n");
} }
/* Local helpers--------------------------------------------------- */ /* Local helpers--------------------------------------------------- */
@@ -541,7 +538,6 @@ scan_update(
interpolate_scan(scan, lidar_angles_deg, lidar_distances_mm, scan_size); interpolate_scan(scan, lidar_angles_deg, lidar_distances_mm, scan_size);
} }
/* Take velocity into account */ /* Take velocity into account */
int degrees_per_second = (int)(scan->rate_hz * 360); int degrees_per_second = (int)(scan->rate_hz * 360);
double horz_mm = velocities_dxy_mm / degrees_per_second; double horz_mm = velocities_dxy_mm / degrees_per_second;

22
examples/rpslam.py
View File

@@ -52,8 +52,6 @@ if __name__ == '__main__':
while True: while True:
try:
# Extract (quality, angle, distance) triples from current scan # Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)] items = [item for item in next(iterator)]
@@ -61,32 +59,26 @@ if __name__ == '__main__':
distances = [item[2] for item in items] distances = [item[2] for item in items]
angles = [item[1] for item in items] angles = [item[1] for item in items]
print(len(distances), len(angles))
# Update SLAM with current Lidar scan and scan angles # Update SLAM with current Lidar scan and scan angles
slam.update(distances, scan_angles_degrees=angles) slam.update(distances, scan_angles_degrees=angles)
except KeyboardInterrupt:
break
# Get current robot position # Get current robot position
#x, y, theta = slam.getpos() x, y, theta = slam.getpos()
# Get current map bytes as grayscale # Get current map bytes as grayscale
#slam.getmap(mapbytes) slam.getmap(mapbytes)
# Display the map # Display the map
#display.displayMap(mapbytes) display.displayMap(mapbytes)
# Display the robot's pose in the map # Display the robot's pose in the map
#display.setPose(x, y, theta) display.setPose(x, y, theta)
# Break on window close # Break on window close
#if not display.refresh(): if not display.refresh():
# break break
# Shut down the lidar connection # Shut down the lidar connection
from time import sleep from time import sleep
lidar.stop() lidar.stop()
lidar.disconnect() lidar.disconnect()