diff --git a/pyrobolearn/utils/data_structures/__init__.py b/pyrobolearn/utils/data_structures/__init__.py
index d1d8dbe..3c07c2b 100644
--- a/pyrobolearn/utils/data_structures/__init__.py
+++ b/pyrobolearn/utils/data_structures/__init__.py
@@ -4,5 +4,8 @@
 # Ordered sets
 from .orderedset import *
 
+# Queues
+from .queues import *
+
 # Graph
-from .graph import *
\ No newline at end of file
+from .graph import *
diff --git a/pyrobolearn/utils/data_structures/orderedset.py b/pyrobolearn/utils/data_structures/orderedset.py
index 039269a..94ecc61 100644
--- a/pyrobolearn/utils/data_structures/orderedset.py
+++ b/pyrobolearn/utils/data_structures/orderedset.py
@@ -1,6 +1,19 @@
+#!/usr/bin/env python
+"""Define the OrderedSet data structure class.
+"""
 
 import collections
 
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
 class OrderedSet(collections.MutableSet):
     r"""Ordered Set
 
@@ -59,6 +72,10 @@ class OrderedSet(collections.MutableSet):
             for item in iterator:
                 self.add(item)
 
+    ###########
+    # Methods #
+    ###########
+
     def add(self, item):
         """
         Add/Append an item to the ordered set.
@@ -86,15 +103,14 @@ class OrderedSet(collections.MutableSet):
         Time complexity: O(N)
         """
         # check idx
-        idx = self._checkIndex(idx)
+        idx = self._check_index(idx)
         if item in self._set:
             # move the item at the specified location
-            self.move(idx, item) # O(N)
+            self.move(idx, item)  # O(N)
         else:
             # add it
-            self._list.insert(idx, item) # O(N)
-            self._set.add(item) # O(1)
-
+            self._list.insert(idx, item)  # O(N)
+            self._set.add(item)  # O(1)
 
     def move(self, idx, item):
         """
@@ -102,11 +118,10 @@ class OrderedSet(collections.MutableSet):
         Time complexity: O(N)
         """
         # remove the item from the list/set
-        self.remove(item) # O(N)
+        self.remove(item)  # O(N)
 
         # insert item
-        self.insert(idx, item) # O(N)
-
+        self.insert(idx, item)  # O(N)
 
     def discard(self, item):
         """
@@ -115,7 +130,7 @@ class OrderedSet(collections.MutableSet):
         """
         if item in self._set:
             self._list.remove(item)  # O(N)
-            self._set.remove(item) # O(1)
+            self._set.remove(item)  # O(1)
 
     def remove(self, item):
         """
@@ -134,9 +149,9 @@ class OrderedSet(collections.MutableSet):
             index: index in the ordered set.
         """
         if index is None: index = len(self._list)
-        index = self._checkIndex(index) # to be sure the index is valid
-        self._set.remove(self._list[index]) # O(1)
-        item = self._list.pop(index) # O(1) if last, O(N) if first
+        index = self._check_index(index)  # to be sure the index is valid
+        self._set.remove(self._list[index])  # O(1)
+        item = self._list.pop(index)  # O(1) if last, O(N) if first
         return item
 
     def copy(self):
@@ -146,7 +161,7 @@ class OrderedSet(collections.MutableSet):
         """
         return self.__class__(self)
 
-    def _checkIndex(self, idx):
+    def _check_index(self, idx):
         """
         Check the given index; if it is in the range of the ordered set, and if it is negative return the
         corresponding positive index.
@@ -274,7 +289,12 @@ class OrderedSet(collections.MutableSet):
         """
         return other.issuperset(self, order=order)
 
+    #############
+    # Operators #
+    #############
+
     def __repr__(self):
+        """Return a representation string."""
         return '%s(%r)' % (self.__class__.__name__, list(self))
 
     def __contains__(self, item):
@@ -336,14 +356,14 @@ class OrderedSet(collections.MutableSet):
             else:
                 self._list[idx] = item
                 self._set.add(item)
-        elif isinstance(idx, slice): # slice
+        elif isinstance(idx, slice):  # slice
             # replace in list
-            items_to_remove = self._list[idx] # O(K)
-            self._list[idx] = item # O(K+N)
+            items_to_remove = self._list[idx]  # O(K)
+            self._list[idx] = item  # O(K+N)
 
             # remove previous items from the set
             for elem in items_to_remove:
-                self._set.remove(elem) # O(1)
+                self._set.remove(elem)  # O(1)
 
             # add new items in the set
             for elem in item:
@@ -455,6 +475,10 @@ class OrderedSet2(collections.MutableSet):
             for item in iterator:
                 self.add(item)
 
+    ###########
+    # Methods #
+    ###########
+
     def add(self, item):
         """
         Add/Append an item to the ordered set.
@@ -491,7 +515,7 @@ class OrderedSet2(collections.MutableSet):
         Time complexity: O(N)
         """
         # check idx
-        idx = self._checkIndex(idx)
+        idx = self._check_index(idx)
 
         # if the set is initially empty or index is the size of the set, just add the item (at the end)
         if len(self._map) == 0 or idx == len(self._map):
@@ -503,10 +527,10 @@ class OrderedSet2(collections.MutableSet):
                 # get current item at the specified index, update the items nearby, and insert the new item
                 curr = self[idx]
                 prev_item, next_item = self._map[curr]
-                if prev_item == self.NonePtr: # beginning of the ordered set (idx=0)
+                if prev_item == self.NonePtr:  # beginning of the ordered set (idx=0)
                     self._map[curr][0] = item
                     self._map[item] = [self.NonePtr, curr]
-                else: # somewhere between the start and the end (not included)
+                else:  # somewhere between the start and the end (not included)
                     self._map[item] = [prev_item, next_item]
                     self._map[prev_item][1] = item
                     self._map[next_item][0] = item
@@ -525,7 +549,6 @@ class OrderedSet2(collections.MutableSet):
         # insert item
         self.insert(idx, item)
 
-
     def discard(self, item):
         """
         Remove an item from the ordered set if it is a member. If the item is not a member do nothing.
@@ -579,7 +602,7 @@ class OrderedSet2(collections.MutableSet):
         """
         return self.__class__(self)
 
-    def _checkIndex(self, idx):
+    def _check_index(self, idx):
         """
         Check the given index; if it is in the range of the ordered set, and if it is negative return the
         corresponding positive index.
@@ -709,7 +732,12 @@ class OrderedSet2(collections.MutableSet):
         """
         return other.issuperset(self, order=order)
 
+    #############
+    # Operators #
+    #############
+
     def __repr__(self):
+        """Return a representation string."""
         return '%s(%r)' % (self.__class__.__name__, list(self))
 
     def __contains__(self, item):
@@ -763,7 +791,7 @@ class OrderedSet2(collections.MutableSet):
         if isinstance(idx, int): # index is an integer
 
             # check index
-            idx = self._checkIndex(idx)
+            idx = self._check_index(idx)
 
             # traverse the set in a specific order based on how close the index is wrt the start/end of the set
             curr = self.NonePtr
@@ -818,7 +846,7 @@ class OrderedSet2(collections.MutableSet):
         Time complexity: O(N)
         """
         # check idx
-        idx = self._checkIndex(idx)
+        idx = self._check_index(idx)
 
         # check if item already in the set
         if item in self._map:
@@ -880,7 +908,7 @@ class OrderedSet2(collections.MutableSet):
         self &= other
 
 
-#OrderedSet = OrderedSet2
+# OrderedSet = OrderedSet2
 
 
 # Tests
@@ -954,4 +982,4 @@ if __name__ == '__main__':
     print("s3 - s1 = {}".format(s3 - s1))
     print("s3.difference(s1) = {}".format(s3.difference(s1)))
     print("s1 - s2 = {}".format(s1 - s2))
-    print("s1 - s0 = {}".format(s1 - s0))
\ No newline at end of file
+    print("s1 - s0 = {}".format(s1 - s0))
diff --git a/pyrobolearn/utils/data_structures/queues.py b/pyrobolearn/utils/data_structures/queues.py
new file mode 100644
index 0000000..b6a07e6
--- /dev/null
+++ b/pyrobolearn/utils/data_structures/queues.py
@@ -0,0 +1,347 @@
+#!/usr/bin/env python
+"""Define the queue data structures.
+
+These inherit from the various queues in the `queue` Python library, to which we provide few more functionalities.
+"""
+
+import queue
+import heapq
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+# # add functionalities to the original queue.Queue (from which queue.PriorityQueue and LifoQueue inherit from)
+#
+# def append(self, item):
+#     """
+#     Append an item in at the end of the FIFO queue. If the queue is already full it will remove the first item.
+#     """
+#     # if full remove the first item
+#     if self.full():
+#         self.get()
+#
+#     # add the new item
+#     self.put(item)
+#
+#
+# def tolist(self):
+#     """
+#     Return a list of the queue.
+#     """
+#     return list(self.queue)
+#
+#
+# def __repr__(self):
+#     """Return a representation string."""
+#     return str(self.queue)
+#
+#
+# def __len__(self):
+#     """Return the current length of the FIFO queue."""
+#     return self.qsize()
+#
+#
+# def __getitem__(self, idx):
+#     """Return the item corresponding to the given index.
+#
+#     Args:
+#         idx (int): index.
+#     """
+#     return self.queue[idx]
+#
+#
+# def __setitem__(self, idx, item):
+#     """Set the given item to the given index."""
+#     self.queue[idx] = item
+#
+#
+# def __iter__(self):
+#     """Return an iterator over the queue."""
+#     return iter(self.queue)
+#
+#
+# # add functionalities
+# queue.Queue.append = append
+# queue.Queue.tolist = tolist
+# queue.Queue.__repr__ = __repr__
+# queue.Queue.__len__ = __len__
+# queue.Queue.__getitem__ = __getitem__
+# queue.Queue.__setitem__ = __setitem__
+# queue.Queue.__iter__ = __iter__
+#
+#
+# # provide aliases
+# FIFOQueue = queue.Queue
+# LIFOQueue = queue.LifoQueue
+
+
+class FIFOQueue(queue.Queue):
+    r"""FIFO Queue
+
+    We provide few more functionalities on top of the original `queue.Queue` class.
+    """
+
+    def __init__(self, maxsize=0):
+        """
+        Initialize the FIFO queue.
+
+        Args:
+            maxsize (int): maximum size of the queue.
+        """
+        queue.Queue.__init__(self, maxsize)
+
+    def append(self, item):
+        """
+        Append an item in at the end of the FIFO queue. If the queue is already full it will remove the first item.
+        """
+        # if full remove the first item
+        if self.full():
+            self.get()
+
+        # add the new item
+        self.put(item)
+
+    def tolist(self):
+        """
+        Return a list of the queue.
+        """
+        return list(self.queue)
+
+    def __repr__(self):
+        """Return a representation string."""
+        return str(self.queue)
+
+    def __len__(self):
+        """Return the current length of the FIFO queue."""
+        return self.qsize()
+
+    def __getitem__(self, idx):
+        """Return the item corresponding to the given index.
+
+        Args:
+            idx (int): index.
+        """
+        return self.queue[idx]
+
+    def __setitem__(self, idx, item):
+        """Set the given item to the given index."""
+        self.queue[idx] = item
+
+    def __iter__(self):
+        """Return an iterator over the queue."""
+        return iter(self.queue)
+
+
+class LIFOQueue(queue.LifoQueue):
+    r"""LIFO Queue.
+
+    We provide few more functionalities on top of the original `queue.LifoQueue` class.
+    """
+
+    def __init__(self, maxsize=0):
+        """
+        Initialize the FIFO queue.
+
+        Args:
+            maxsize (int): maximum size of the queue.
+        """
+        queue.Queue.__init__(self, maxsize)
+
+    def append(self, item):
+        """
+        Append an item in at the end of the FIFO queue. If the queue is already full it will remove the first item.
+        """
+        # if full remove the first item
+        if self.full():
+            self.get()
+
+        # add the new item
+        self.put(item)
+
+    def tolist(self):
+        """
+        Return a list of the queue.
+        """
+        return list(self.queue)
+
+    def __repr__(self):
+        """Return a representation string."""
+        return str(self.queue)
+
+    def __len__(self):
+        """Return the current length of the FIFO queue."""
+        return self.qsize()
+
+    def __getitem__(self, idx):
+        """Return the item corresponding to the given index.
+
+        Args:
+            idx (int): index.
+        """
+        return self.queue[idx]
+
+    def __setitem__(self, idx, item):
+        """Set the given item to the given index."""
+        self.queue[idx] = item
+
+    def __iter__(self):
+        """Return an iterator over the queue."""
+        return reversed(self.queue)
+
+
+class PriorityQueue(queue.PriorityQueue):
+    r"""Priority Queue.
+
+    We provide few more functionalities on top of the original `queue.PriorityQueue` class.
+    """
+
+    def __init__(self, maxsize=0, ascending=True):
+        """
+        Initialize the FIFO queue.
+
+        Args:
+            maxsize (int): maximum size of the queue.
+            ascending (bool): if True, the item with the lowest priority will be the first one to be retrieved.
+        """
+        queue.Queue.__init__(self, maxsize)
+        self.ascending = bool(ascending)
+
+    def append(self, item):
+        """
+        Append an item in at the end of the FIFO queue. If the queue is already full it will remove the first item.
+        """
+        # if full remove the first item
+        if self.full():
+            self.get()
+
+        # add the new item
+        self.put(item)
+
+    def tolist(self):
+        """
+        Return a list of the queue.
+        """
+        return list(self.queue)
+
+    def _put(self, item, heappush=heapq.heappush):
+        if not self.ascending:
+            item = (-item[0], item[1])
+        queue.PriorityQueue._put(self, item, heappush=heappush)
+
+    def _get(self, heappop=heapq.heappop):
+        item = queue.PriorityQueue._get(self, heappop=heappop)
+        if not self.ascending:
+            item = (-item[0], item[1])
+        return item
+
+    def get_lowest(self):
+        """
+        Get the lowest priority item.
+        """
+        if len(self) == 0:
+            raise ValueError("The priority queue is empty.")
+
+        if self.ascending:
+            return self[0][1]
+        else:
+            return self[-1][1]
+
+    def get_highest(self):
+        """
+        Get the highest priority item.
+        """
+        if len(self) == 0:
+            raise ValueError("The priority queue is empty.")
+        if self.ascending:
+            return self[-1][1]
+        else:
+            return self[0][1]
+
+    def __repr__(self):
+        """Return a representation string."""
+        return str(self.queue)
+
+    def __len__(self):
+        """Return the current length of the FIFO queue."""
+        return self.qsize()
+
+    def __getitem__(self, idx):
+        """Return the item corresponding to the given index.
+
+        Args:
+            idx (int): index.
+        """
+        return self.queue[idx]
+
+    def __setitem__(self, idx, item):
+        """Set the given item to the given index."""
+        self.queue[idx] = item
+
+    def __iter__(self):
+        """Return an iterator over the queue."""
+        if self.ascending:
+            return iter(self.queue)
+        else:
+            return reversed(self.queue)
+
+
+# Tests
+if __name__ == '__main__':
+    # create queue
+    q = FIFOQueue(2)
+    print("Initial queue (maxsize={}): {}".format(q.maxsize, q))
+
+    # add two elements in the queue
+    q.append(1)
+    q.append(2)
+    print("After adding '1' and '2' in queue: {}".format(q))
+
+    # add third element in the queue
+    q.append(3)
+    print("After adding '3' in queue: {}".format(q))
+
+    # iterate over queue
+    for i, item in enumerate(q):
+        print("Item {}: {}".format(i, item))
+
+    # create stack
+    stack = LIFOQueue(maxsize=2)
+    print("\nInitial stack (maxsize={}): {}".format(stack.maxsize, stack))
+
+    # add two elements in the stack
+    stack.append(1)
+    stack.append(2)
+    print("After adding '1' and '2' in the stack: {}".format(stack))
+
+    # add third element
+    stack.append(3)
+    print("After adding '3' in queue: {}".format(stack))
+
+    # iterate over stack
+    for i, item in enumerate(stack):
+        print("Item {}: {}".format(i, item))
+
+    # create priority queue
+    pq = PriorityQueue(maxsize=2, ascending=True)
+    print("\nInitial priority queue (maxsize={}): {}".format(pq.maxsize, pq))
+
+    # add two elements in the queue
+    pq.append((1, 'hello'))
+    pq.append((2, 'world'))
+    print("After adding (1, 'hello') and (2, 'world') in priority queue: {}".format(pq))
+    print("Lowest priority item: {}".format(pq.get_lowest()))
+    print("Highest priority item: {}".format(pq.get_highest()))
+
+    print("Remove the 1st item: {} --> resulting priority queue: {}".format(pq.get(), pq))
+
+    # add two elements in the queue
+    pq.append((3, 'hello'))
+    pq.append((4, 'sir'))
+    print("After adding (3, 'hello') and (4, 'sir') in priority queue: {}".format(pq))