diff --git a/examples/logmovie.py b/examples/logmovie.py index 577b4b1..c0d1991 100755 --- a/examples/logmovie.py +++ b/examples/logmovie.py @@ -45,7 +45,6 @@ from roboviz import Visualizer from sys import argv, exit from time import sleep from threading import Thread -import pickle def threadfunc(robot, slam, timestamps, lidars, odometries, mapbytes, pose): ''' @@ -112,7 +111,7 @@ def main(): else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display, named by dataset - display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, dataset) + display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, dataset) # Pose will be modified in our threaded code pose = [0,0,0] @@ -127,7 +126,7 @@ def main(): # Display map and robot pose display.displayMap(mapbytes) - display.setPose(*pose) + display.setPose(pose[0]/1000., pose[1]/1000., pose[2]) # Refresh the display, exiting gracefully if user closes it if not display.refresh(): diff --git a/examples/mines.py b/examples/mines.py index 592eecd..0c6b69c 100644 --- a/examples/mines.py +++ b/examples/mines.py @@ -33,7 +33,6 @@ along with this code. If not, see . from breezyslam.vehicles import WheeledVehicle from breezyslam.sensors import URG04LX -import math # Method to load all from file ------------------------------------------------ diff --git a/examples/rpslam.py b/examples/rpslam.py index cc096a1..393a099 100755 --- a/examples/rpslam.py +++ b/examples/rpslam.py @@ -43,7 +43,7 @@ if __name__ == '__main__': slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display - display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM') + display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') # Initialize an empty trajectory trajectory = [] @@ -90,7 +90,7 @@ if __name__ == '__main__': display.displayMap(mapbytes) # Display the robot's pose in the map - display.setPose(x, y, theta) + display.setPose(x/1000., y/1000., theta) # Break on window close if not display.refresh(): diff --git a/examples/rpslam_scipy.py b/examples/rpslam_scipy.py index 5eea4ce..cf66c45 100755 --- a/examples/rpslam_scipy.py +++ b/examples/rpslam_scipy.py @@ -47,7 +47,7 @@ if __name__ == '__main__': slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display - display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM') + display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') # Initialize an empty trajectory trajectory = [] @@ -98,9 +98,11 @@ if __name__ == '__main__': # Get current map bytes as grayscale slam.getmap(mapbytes) + # Display map display.displayMap(mapbytes) - display.setPose(x, y, theta) + # Display pose after converting from mm to meters + display.setPose(x/1000., y/1000., theta) # Break on window close if not display.refresh(): diff --git a/examples/urgslam.py b/examples/urgslam.py index fe02410..7525dc3 100755 --- a/examples/urgslam.py +++ b/examples/urgslam.py @@ -39,7 +39,7 @@ if __name__ == '__main__': slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display - display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM') + display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') # Initialize empty map mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) @@ -55,9 +55,11 @@ if __name__ == '__main__': # Get current map bytes as grayscale slam.getmap(mapbytes) + # Display map display.displayMap(mapbytes) - display.setPose(x, y, theta) + # Display pose after converting from mm to meters + display.setPose(x/1000., y/1000., theta) # Exit on window close if not display.refresh(): diff --git a/examples/xvslam.py b/examples/xvslam.py index e8a633e..c3b08ea 100755 --- a/examples/xvslam.py +++ b/examples/xvslam.py @@ -30,8 +30,6 @@ from xvlidar import XVLidar as Lidar from roboviz import Visualizer -from sys import stdout - if __name__ == '__main__': # Connect to Lidar unit @@ -41,7 +39,7 @@ if __name__ == '__main__': slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display - display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM') + display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') # Initialize an empty trajectory trajectory = [] @@ -60,9 +58,11 @@ if __name__ == '__main__': # Get current map bytes as grayscale slam.getmap(mapbytes) + # Display map display.displayMap(mapbytes) - display.setPose(x, y, theta) + # Display pose after converting from mm to meters + display.setPose(x/1000., y/1000., theta) # Exit on window close if not display.refresh():