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

Works with simplified MapVisualizer API

Browse Source
This commit is contained in:
simondlevy
2019-01-12 23:08:39 -05:00
parent c5ac00142e
commit 0aadd46aaf
5 changed files with 19 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/logmovie.py
View File

@@ -111,7 +111,7 @@ def main():
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display, named by dataset # Set up a SLAM display, named by dataset
display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, dataset) viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, dataset)
# Pose will be modified in our threaded code # Pose will be modified in our threaded code
pose = [0,0,0] pose = [0,0,0]
@@ -124,12 +124,8 @@ def main():
# Loop forever,displaying current map and pose # Loop forever,displaying current map and pose
while True: while True:
# Display map and robot pose # Display map and robot pose, exiting gracefully if user closes it
display.displayMap(mapbytes) if not viz.display(pose[0]/1000., pose[1]/1000., pose[2], mapbytes):
display.setPose(pose[0]/1000., pose[1]/1000., pose[2])
# Refresh the display, exiting gracefully if user closes it
if not display.refresh():
exit(0) exit(0)
main() main()

16
examples/rpslam.py
View File

@@ -43,7 +43,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display # Set up a SLAM display
display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory # Initialize an empty trajectory
trajectory = [] trajectory = []
@@ -86,16 +86,10 @@ if __name__ == '__main__':
# Get current map bytes as grayscale # Get current map bytes as grayscale
slam.getmap(mapbytes) slam.getmap(mapbytes)
# Display the map # Display map and robot pose, exiting gracefully if user closes it
display.displayMap(mapbytes) if not viz.display(x/1000., y/1000., theta, mapbytes):
exit(0)
# Display the robot's pose in the map
display.setPose(x/1000., y/1000., theta)
# Break on window close
if not display.refresh():
break
# Shut down the lidar connection # Shut down the lidar connection
lidar.stop() lidar.stop()
lidar.disconnect() lidar.disconnect()

16
examples/rpslam_scipy.py
View File

@@ -47,7 +47,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display # Set up a SLAM display
display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory # Initialize an empty trajectory
trajectory = [] trajectory = []
@@ -98,16 +98,10 @@ if __name__ == '__main__':
# Get current map bytes as grayscale # Get current map bytes as grayscale
slam.getmap(mapbytes) slam.getmap(mapbytes)
# Display map # Display map and robot pose, exiting gracefully if user closes it
display.displayMap(mapbytes) if not viz.display(x/1000., y/1000., theta, mapbytes):
exit(0)
# Display pose after converting from mm to meters
display.setPose(x/1000., y/1000., theta)
# Break on window close
if not display.refresh():
break
# Shut down the lidar connection # Shut down the lidar connection
lidar.stop() lidar.stop()
lidar.disconnect() lidar.disconnect()

13
examples/urgslam.py
View File

@@ -39,7 +39,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display # Set up a SLAM display
display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map # Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
@@ -55,13 +55,6 @@ if __name__ == '__main__':
# Get current map bytes as grayscale # Get current map bytes as grayscale
slam.getmap(mapbytes) slam.getmap(mapbytes)
# Display map # Display map and robot pose, exiting gracefully if user closes it
display.displayMap(mapbytes) if not viz.display(x/1000., y/1000., theta, mapbytes):
# Display pose after converting from mm to meters
display.setPose(x/1000., y/1000., theta)
# Exit on window close
if not display.refresh():
exit(0) exit(0)

13
examples/xvslam.py
View File

@@ -39,7 +39,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display # Set up a SLAM display
display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory # Initialize an empty trajectory
trajectory = [] trajectory = []
@@ -58,13 +58,6 @@ if __name__ == '__main__':
# Get current map bytes as grayscale # Get current map bytes as grayscale
slam.getmap(mapbytes) slam.getmap(mapbytes)
# Display map # Display map and robot pose, exiting gracefully if user closes it
display.displayMap(mapbytes) if not viz.display(x/1000., y/1000., theta, mapbytes):
# Display pose after converting from mm to meters
display.setPose(x/1000., y/1000., theta)
# Exit on window close
if not display.refresh():
exit(0) exit(0)