Ejemplo n.º 1
0
    def __init__(self):
        '''
        Takes no args.  Maybe we could specify colors, lidar params, etc.
        '''

        # Create the frame
        tk.Frame.__init__(self, borderwidth=4, relief='sunken')
        self.master.geometry(
            str(DISPLAY_CANVAS_SIZE_PIXELS) + "x" +
            str(DISPLAY_CANVAS_SIZE_PIXELS))
        self.master.title('XV Lidar [ESC to quit]')
        self.grid()
        self.master.rowconfigure(0, weight=1)
        self.master.columnconfigure(0, weight=1)
        self.grid(sticky=tk.W + tk.E + tk.N + tk.S)
        self.background = DISPLAY_CANVAS_COLOR

        # Add a canvas for drawing
        self.canvas =  tk.Canvas(self, \
            width = DISPLAY_CANVAS_SIZE_PIXELS, \
            height = DISPLAY_CANVAS_SIZE_PIXELS,\
            background = DISPLAY_CANVAS_COLOR)
        self.canvas.grid(row = 0, column = 0,\
                    rowspan = 1, columnspan = 1,\
                    sticky = tk.W+tk.E+tk.N+tk.S)

        # Set up a key event for exit on ESC
        self.bind('<Key>', self._key)

        # This call gives the frame focus so that it receives input
        self.focus_set()

        # No scanlines initially
        self.lines = []

        # Connect to the XVLidar
        self.lidar = XVLidar(COM_PORT)

        # No scan data to start
        self.scandata = []

        # Pre-compute some values useful for plotting

        scan_angle_rad = [radians(-XVLIDAR_DETECTION_DEG/2 + (float(k)/XVLIDAR_SCAN_SIZE) * \
                                   XVLIDAR_DETECTION_DEG) for k in range(XVLIDAR_SCAN_SIZE)]

        self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2
        scale = self.half_canvas_pix / float(XVLIDAR_MAX_SCAN_DIST_MM)

        self.cos = [-cos(angle) * scale for angle in scan_angle_rad]
        self.sin = [sin(angle) * scale for angle in scan_angle_rad]

        # Add scan lines to canvas, to be modified later
        self.lines = [self.canvas.create_line(\
                         self.half_canvas_pix, \
                         self.half_canvas_pix, \
                         self.half_canvas_pix + self.sin[k] * XVLIDAR_MAX_SCAN_DIST_MM,\
                         self.half_canvas_pix + self.cos[k] * XVLIDAR_MAX_SCAN_DIST_MM)
                         for k in range(XVLIDAR_SCAN_SIZE)]

        [
            self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR)
            for line in self.lines
        ]

        # Start a new thread and set a flag to let it know when we stop running
        thread.start_new_thread(self.grab_scan, ())
        self.running = True
Ejemplo n.º 2
0
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10
LIDAR_DEVICE = '/dev/ttyACM0'

from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import XVLidar as LaserModel

from xvlidar import XVLidar as Lidar

from roboviz import MapVisualizer

if __name__ == '__main__':

    # Connect to Lidar unit
    lidar = Lidar(LIDAR_DEVICE)

    # Create an RMHC SLAM object with a laser model and optional robot model
    slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)

    # Set up a SLAM display
    display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')

    # Initialize an empty trajectory
    trajectory = []

    # Initialize empty map
    mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)

    while True:
Ejemplo n.º 3
0
class XVLidarPlotter(tk.Frame):
    '''
    XVLidarPlotter extends tk.Frame to plot Lidar scans.
    '''
    def __init__(self):
        '''
        Takes no args.  Maybe we could specify colors, lidar params, etc.
        '''

        # Create the frame
        tk.Frame.__init__(self, borderwidth=4, relief='sunken')
        self.master.geometry(
            str(DISPLAY_CANVAS_SIZE_PIXELS) + "x" +
            str(DISPLAY_CANVAS_SIZE_PIXELS))
        self.master.title('XV Lidar [ESC to quit]')
        self.grid()
        self.master.rowconfigure(0, weight=1)
        self.master.columnconfigure(0, weight=1)
        self.grid(sticky=tk.W + tk.E + tk.N + tk.S)
        self.background = DISPLAY_CANVAS_COLOR

        # Add a canvas for drawing
        self.canvas =  tk.Canvas(self, \
            width = DISPLAY_CANVAS_SIZE_PIXELS, \
            height = DISPLAY_CANVAS_SIZE_PIXELS,\
            background = DISPLAY_CANVAS_COLOR)
        self.canvas.grid(row = 0, column = 0,\
                    rowspan = 1, columnspan = 1,\
                    sticky = tk.W+tk.E+tk.N+tk.S)

        # Set up a key event for exit on ESC
        self.bind('<Key>', self._key)

        # This call gives the frame focus so that it receives input
        self.focus_set()

        # No scanlines initially
        self.lines = []

        # Connect to the XVLidar
        self.lidar = XVLidar(COM_PORT)

        # No scan data to start
        self.scandata = []

        # Pre-compute some values useful for plotting

        scan_angle_rad = [radians(-XVLIDAR_DETECTION_DEG/2 + (float(k)/XVLIDAR_SCAN_SIZE) * \
                                   XVLIDAR_DETECTION_DEG) for k in range(XVLIDAR_SCAN_SIZE)]

        self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2
        scale = self.half_canvas_pix / float(XVLIDAR_MAX_SCAN_DIST_MM)

        self.cos = [-cos(angle) * scale for angle in scan_angle_rad]
        self.sin = [sin(angle) * scale for angle in scan_angle_rad]

        # Add scan lines to canvas, to be modified later
        self.lines = [self.canvas.create_line(\
                         self.half_canvas_pix, \
                         self.half_canvas_pix, \
                         self.half_canvas_pix + self.sin[k] * XVLIDAR_MAX_SCAN_DIST_MM,\
                         self.half_canvas_pix + self.cos[k] * XVLIDAR_MAX_SCAN_DIST_MM)
                         for k in range(XVLIDAR_SCAN_SIZE)]

        [
            self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR)
            for line in self.lines
        ]

        # Start a new thread and set a flag to let it know when we stop running
        thread.start_new_thread(self.grab_scan, ())
        self.running = True

    # Runs on its own thread
    def grab_scan(self):

        while True:

            # Lidar sends 360 (distance, quality) pairs, which may be empty on start
            self.scandata = [pair[0] for pair in self.lidar.getScan()]

            self.count += 1

            if not self.running:
                break

            # yield to plotting thread
            sleep(.0001)

    def run(self):
        '''
        Call this when you're ready to run.
        '''

        # Record start time and initiate a count of scans for testing
        self.count = 0
        self.start_sec = time()
        self.showcount = 0

        # Start the recursive timer-task
        plotter._task()

        # Start the GUI
        plotter.mainloop()

    def destroy(self):
        '''
        Called automagically when user clicks X to close window.
        '''

        self._quit()

    def _quit(self):

        self.running = False
        elapsed_sec = time() - self.start_sec

        del self.lidar

        exit(0)

    def _key(self, event):

        # Make sure the frame is receiving input!
        self.focus_force()
        if event.keysym == 'Escape':
            self._quit()

    def _task(self):

        # Modify the displayed lines according to the current scan
        [self.canvas.coords(self.lines[k],
            self.half_canvas_pix, \
                    self.half_canvas_pix, \
                    self.half_canvas_pix + self.sin[k] * self.scandata[k],\
                    self.half_canvas_pix + self.cos[k] * self.scandata[k]) \
                    for k in range(len(self.scandata))]

        # Reschedule this task immediately
        self.after(1, self._task)

        # Record another display for reporting performance
        self.showcount += 1
Ejemplo n.º 4
0
MAP_SIZE_PIXELS         = 500
MAP_SIZE_METERS         = 3
LIDAR_DEVICE            = '/dev/ttyACM0'

from breezyslam.algorithms import VLP_SLAM
from breezyslam.components import XVLidar as LaserModel

from xvlidar import XVLidar as Lidar

from cvslamshow import SlamShow

if __name__ == '__main__':

    # Connect to Lidar unit
    lidar = Lidar(LIDAR_DEVICE)

    # Create an RMHC SLAM object with a laser model and optional robot model
    slam = VLP_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)

    # Set up a SLAM display
    display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')

    # Initialize an empty trajectory
    trajectory = []

    # Initialize empty map
    mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
    
    #wait for positioning to be ready
    ready = "0"
Ejemplo n.º 5
0
    def signal_handler(signa, frame):
        robot.stop()
        sys.exit(0)

    signal.signal(signal.SIGINT, signal_handler)

    try:
        print('Initializing..')
        robot = NexusRobot(ROBOT_DEVICE)
        print('Initialized')
        t = Thread(target=telem, args=(robot, ))
        t.daemon = True
        t.start()
        odomRobot = NexusRobotOd()
        # Connect to Lidar unit
        lidar = Lidar(LIDAR_DEVICE)

        # Create an RMHC SLAM object with a laser model and optional robot model
        slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)

        # Set up a SLAM display
        display = SlamShow(MAP_SIZE_PIXELS,
                           MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, 'SLAM')

        # Initialize an empty trajectory
        trajectory = []

        # Initialize empty map
        mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)

        while True:
Ejemplo n.º 6
0
    def __init__(self):
        '''
        Takes no args.  Maybe we could specify colors, lidar params, etc.
        '''
        
        # Create the frame        
        tk.Frame.__init__(self, borderwidth = 4, relief = 'sunken')
        self.master.geometry(str(DISPLAY_CANVAS_SIZE_PIXELS)+ "x" + str(DISPLAY_CANVAS_SIZE_PIXELS))
        self.master.title('XV Lidar [ESC to quit]')
        self.grid()
        self.master.rowconfigure(0, weight = 1)
        self.master.columnconfigure(0, weight = 1)
        self.grid(sticky = tk.W+tk.E+tk.N+tk.S)
        self.background = DISPLAY_CANVAS_COLOR
        
        # Add a canvas for drawing
        self.canvas =  tk.Canvas(self, \
            width = DISPLAY_CANVAS_SIZE_PIXELS, \
            height = DISPLAY_CANVAS_SIZE_PIXELS,\
            background = DISPLAY_CANVAS_COLOR)
        self.canvas.grid(row = 0, column = 0,\
                    rowspan = 1, columnspan = 1,\
                    sticky = tk.W+tk.E+tk.N+tk.S)

        # Set up a key event for exit on ESC
        self.bind('<Key>', self._key)

        # This call gives the frame focus so that it receives input
        self.focus_set()

        # No scanlines initially                             
        self.lines = []
        
        # Connect to the XVLidar
        self.lidar = XVLidar(COM_PORT)
        
        # No scan data to start
        self.scandata = []
        
        # Pre-compute some values useful for plotting
        
        scan_angle_rad = [radians(-XVLIDAR_DETECTION_DEG/2 + (float(k)/XVLIDAR_SCAN_SIZE) * \
                                   XVLIDAR_DETECTION_DEG) for k in range(XVLIDAR_SCAN_SIZE)]
                                   
        self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2
        scale = self.half_canvas_pix / float(XVLIDAR_MAX_SCAN_DIST_MM)

        self.cos = [-cos(angle) * scale for angle in scan_angle_rad]
        self.sin = [ sin(angle) * scale for angle in scan_angle_rad]
        
        # Add scan lines to canvas, to be modified later        
        self.lines = [self.canvas.create_line(\
                         self.half_canvas_pix, \
                         self.half_canvas_pix, \
                         self.half_canvas_pix + self.sin[k] * XVLIDAR_MAX_SCAN_DIST_MM,\
                         self.half_canvas_pix + self.cos[k] * XVLIDAR_MAX_SCAN_DIST_MM)
                         for k in range(XVLIDAR_SCAN_SIZE)]
                         
        [self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR) for line in self.lines]
        
        # Start a new thread and set a flag to let it know when we stop running
        thread.start_new_thread( self.grab_scan, () )       
        self.running = True      
Ejemplo n.º 7
0
class XVLidarPlotter(tk.Frame):
    '''
    XVLidarPlotter extends tk.Frame to plot Lidar scans.
    '''
    
    def __init__(self):
        '''
        Takes no args.  Maybe we could specify colors, lidar params, etc.
        '''
        
        # Create the frame        
        tk.Frame.__init__(self, borderwidth = 4, relief = 'sunken')
        self.master.geometry(str(DISPLAY_CANVAS_SIZE_PIXELS)+ "x" + str(DISPLAY_CANVAS_SIZE_PIXELS))
        self.master.title('XV Lidar [ESC to quit]')
        self.grid()
        self.master.rowconfigure(0, weight = 1)
        self.master.columnconfigure(0, weight = 1)
        self.grid(sticky = tk.W+tk.E+tk.N+tk.S)
        self.background = DISPLAY_CANVAS_COLOR
        
        # Add a canvas for drawing
        self.canvas =  tk.Canvas(self, \
            width = DISPLAY_CANVAS_SIZE_PIXELS, \
            height = DISPLAY_CANVAS_SIZE_PIXELS,\
            background = DISPLAY_CANVAS_COLOR)
        self.canvas.grid(row = 0, column = 0,\
                    rowspan = 1, columnspan = 1,\
                    sticky = tk.W+tk.E+tk.N+tk.S)

        # Set up a key event for exit on ESC
        self.bind('<Key>', self._key)

        # This call gives the frame focus so that it receives input
        self.focus_set()

        # No scanlines initially                             
        self.lines = []
        
        # Connect to the XVLidar
        self.lidar = XVLidar(COM_PORT)
        
        # No scan data to start
        self.scandata = []
        
        # Pre-compute some values useful for plotting
        
        scan_angle_rad = [radians(-XVLIDAR_DETECTION_DEG/2 + (float(k)/XVLIDAR_SCAN_SIZE) * \
                                   XVLIDAR_DETECTION_DEG) for k in range(XVLIDAR_SCAN_SIZE)]
                                   
        self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2
        scale = self.half_canvas_pix / float(XVLIDAR_MAX_SCAN_DIST_MM)

        self.cos = [-cos(angle) * scale for angle in scan_angle_rad]
        self.sin = [ sin(angle) * scale for angle in scan_angle_rad]
        
        # Add scan lines to canvas, to be modified later        
        self.lines = [self.canvas.create_line(\
                         self.half_canvas_pix, \
                         self.half_canvas_pix, \
                         self.half_canvas_pix + self.sin[k] * XVLIDAR_MAX_SCAN_DIST_MM,\
                         self.half_canvas_pix + self.cos[k] * XVLIDAR_MAX_SCAN_DIST_MM)
                         for k in range(XVLIDAR_SCAN_SIZE)]
                         
        [self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR) for line in self.lines]
        
        # Start a new thread and set a flag to let it know when we stop running
        thread.start_new_thread( self.grab_scan, () )       
        self.running = True      

    # Runs on its own thread
    def grab_scan(self):

        while True:
            
            # Lidar sends 360 (distance, quality) pairs, which may be empty on start
            self.scandata = [pair[0] for pair in self.lidar.getScan()]

            self.count += 1

            if not self.running:
                break

            # yield to plotting thread
            sleep(.0001)

    def run(self):
        '''
        Call this when you're ready to run.
        '''

        # Record start time and initiate a count of scans for testing
        self.count = 0
        self.start_sec = time()
        self.showcount = 0

        # Start the recursive timer-task
        plotter._task() 

        # Start the GUI
        plotter.mainloop()


    def destroy(self):
        '''
        Called automagically when user clicks X to close window.
        '''  

        self._quit()

    def _quit(self):

        self.running = False
        elapsed_sec = time() - self.start_sec

        del self.lidar

        exit(0)

    def _key(self, event):

        # Make sure the frame is receiving input!
        self.focus_force()
        if event.keysym == 'Escape':
            self._quit()

    def _task(self):

        # Modify the displayed lines according to the current scan
        [self.canvas.coords(self.lines[k], 
            self.half_canvas_pix, \
                    self.half_canvas_pix, \
                    self.half_canvas_pix + self.sin[k] * self.scandata[k],\
                    self.half_canvas_pix + self.cos[k] * self.scandata[k]) \
                    for k in range(len(self.scandata))]    

        # Reschedule this task immediately
        self.after(1, self._task)

        # Record another display for reporting performance
        self.showcount += 1