Added maps.py

src/cs4313_utilities/maps.py

+#!/usr/bin/env python
+
+# Contains classes that will be used to maintain a map for use in robot mapping
+
+import random
+
+import cs4313_utilities.geometry as geometry
+import cs4313_utilities.robot_math as rm
+
+
+class Map(object):
+    ''' Abstract class for defining a map for robot navigation use.  Class
+    methods should be implemented by inheriting classes to achieve the desired
+    functionality.
+
+    Abstract public methods:
+      set_goal: resets the goal position and the sampling bias
+      sample: returns a random free-space location
+      is_free_space: Boolean method that returns True if a test state is in free space
+      is_free_space_path: Boolean method that returns True if a straight test path is in free space
+      nearest_obstacle: computes the distance between a point and the nearest obstacle
+      distance: provides a scalar direct-path distance between two map states
+    '''
+
+    def __init__(self):
+        ''' Class initializer
+        '''
+        pass
+
+
+    def set_goal(self, goal, goal_bias=0.0):
+        ''' Resets the goal position and sampling bias
+        @param new goal position
+        @param new probabilistic bias to the goal for sampling
+        '''
+        return None
+
+
+    def sample(self):
+        ''' Inheriting classes should override this method stub to generate
+        a random sample of the object's navagable free space in the form of
+        a Cartesian coordinate (or a form compatible with the planner).
+        @return a free-space location within the modeled world
+        '''
+        return None
+
+
+    def is_free_space(self, state):
+        ''' Inheriting classes should override this method stub to compute a
+        Boolean value based on whether or not the test value is in free space
+        @param state: state that is being tested
+        @return True if the state is in free space
+        '''
+        return True
+
+
+    def is_free_space_path(self, state1, state2):
+        ''' Computes a Boolean value based on whether or not a straight
+        line test path defined by its endpoints is completely in free space
+        @param state1: beginning of the path--(x, y) tuple expected
+        @param state2: end of the path--(x, y) tuple expected
+        @returns True if the path from state1 to state2 is in free space
+        '''
+        return True
+
+
+    def nearest_obstacle(self, point):
+        ''' Computes distance from a test point to the nearest mapped obstacle
+        @param point: Cartesian coordiantes of the test point
+        @return: tuple containing the closest obstacle point and the distance
+        '''
+        return (None, 0.0)
+
+
+    def distance(self, state1, state2):
+        ''' Computes a scalar "distance" between two states in the map 
+        @param state1: First state definition
+        @param state2: Second state definition
+        @return direct-path distance between the states
+        '''
+        return math.hypot((state2[0] - state1[0]), (state2[1] - state1[1]))
+
+
+class GeometryMap(Map):
+    ''' Encodes a rectangular 2 dimensional feature map.  Obastacles (non-free
+    space) are defined by a set of Geometry objects.  This class can be used to
+    implement  biased or unbiased sampling towards a specified "goal" position
+    specified Cartesian coordinates
+
+    Protected member variables:
+      _sw_corner: Cartesian coordinates of the map's southwest corner
+      _ne_corner: Cartesian coordinates of the map's northeast corner
+      _goal: Cartesian coordinates of a specified goal position
+      _goal_bias: probabilistic sampling bias towards the goal
+      _geometries: set of Geometry objects representing non-free space
+
+    Implementations of parent class abstract methods:
+      set_goal: resets the goal position and the sampling bias
+      sample: returns a random free-space location
+      is_free_space: Boolean method that returns True if a test state is in free space
+      is_free_space_path: Boolean method that returns True if a straight test path is in free space
+      distance: provides a scalar direct-path distance between two map states
+
+    Public methods:
+      set_geometries: resets the object's geometries set to new values (erases old)
+      add_geometries: adds additional geometries to the object's polygon set
+    '''
+
+    def __init__(self, sw_corner, ne_corner, geometries, goal=None, goal_bias=0.0):
+        ''' Initializes rectangle corners and geometries.  Corners are specified
+        using Cartesian coordinates.  Polygon objects must be of type
+        geometry.Geometry with coordinates specified in the same coordinate
+        frame as the map corners
+        @param sw_corner: Cartesian coordinates of the map's southwest corner
+        @param ne_corner: Cartesian coordinates of the map's northeast corner
+        @param geometries: iterable object containing geometries
+        @param goal: Cartesian coordinates of the goal position (default None)
+        @param goal_bias: probabilistic sampling bias towards the goal (default 0.0)
+        '''
+        self._sw_corner = ( sw_corner[0], sw_corner[1])
+        self._ne_corner = ( ne_corner[0], ne_corner[1])
+        if goal:
+            self._goal = ( goal[0], goal[1] )
+        else:
+            self._goal = None
+        if self._goal:
+            self._goal_bias = rm.saturate(goal_bias, 0.0, 1.0)
+        else:
+            self._goal_bias = 0.0
+        self._geometries = set(geometries)
+
+
+    #--------------------------------
+    # Abstract method implementations
+    #--------------------------------
+
+    def set_goal(self, goal, goal_bias=0.0):
+        ''' Resets the goal position and sampling bias
+        @param Cartesian coordinates of the new goal position
+        @param new probabilistic bias to the goal for sampling
+        '''
+        if goal:
+            self._goal = ( goal[0], goal[1] )
+        else:
+            self._goal = None
+        if self._goal:
+            self._goal_bias = math_utils.saturate(goal_bias, 0.0, 1.0)
+        else:
+            self._goal_bias = 0.0
+
+
+    def sample(self):
+        ''' Generates a random sample of the object's navagable free space in
+        the form of a latitude/longitude.
+        @return a random free-space location, (lat, lon) tuple
+        '''
+        if random.random() < self._goal_bias:
+            return self._goal
+        random_x = random.uniform(self._sw_corner[0], self._ne_corner[0])
+        random_y = random.uniform(self._sw_corner[1], self._ne_corner[1])
+        while not self.__is_free_cartesian_space(random_x, random_y):
+            random_x = random.uniform(self._sw_corner[0], self._ne_corner[0])
+            random_y = random.uniform(self._sw_corner[1], self._ne_corner[1])
+        return (random_x, random_y)
+
+
+    def is_free_space(self, state):
+        ''' Computes a Boolean value based on whether or not the test value
+        is in free space
+        @param state: state that is being tested--(x, y) tuple expected
+        @return True if the state is in free space
+        '''
+        if (state[0] < self._sw_corner[0]) or \
+           (state[1] < self._sw_corner[1]) or \
+           (state[0] > self._ne_corner[0]) or \
+           (state[1] > self._ne_corner[1]):
+            return False
+
+        for geometry in self._geometries:
+            if geometry.contains(state):
+                return False
+        return True
+
+
+    def is_free_space_path(self, state1, state2):
+        ''' Computes a Boolean value based on whether or not a straight
+        line test path defined by its endpoints is completely in free space
+        @param state1: beginning of the path--(x, y) tuple expected
+        @param state2: end of the path--(x, y) tuple expected
+        @returns True if the path from state1 to state2 is in free space
+        '''
+        if not (self.is_free_space(state1) and self.is_free_space(state2)):
+            return False
+
+        for geometry in self._geometries:
+            if geometry.intersects(state1, state2):
+                return False
+        return True
+
+
+    def nearest_obstacle(self, point):
+        ''' Computes distance from a test point to the nearest mapped obstacle
+        @param point: Cartesian coordiantes of the test point
+        @return: tuple containing the closest obstacle point and the distance
+        '''
+        closest_point = None
+        min_d = 0.0
+        for geometry in self._geometries:
+            pt = geometry.closest_point(point)
+            d = self.distance(point, pt)
+            if (closest_point == None) or (d < min_d):
+                closest_point = pt
+                min_d = d
+        return (closest_point, min_d)
+
+
+    def distance(self, state1, state2):
+        ''' Computes a scalar "distance" between two states in the map 
+        @param state1: Cartesian coordinates of the first state
+        @param state2: Cartesian coordinates of the second state
+        @return straight-line distance between the states
+        '''
+        return math.hypot((state2[0] - state1[0]), (state2[1] - state1[1]))
+
+
+    #------------------------
+    # Locally defined methods
+    #------------------------
+
+    def set_geometries(self, geometries):
+        ''' Clears the current set of geometries representing non-free space
+        and replaces them with new ones.  New geometries must be in the same
+        Cartesian space as the previous ones using a x-north/y-east
+        orientation and origin at the area's southwest corner
+        @param geometries: iterable object containing geometries
+        '''
+        self._geometries = set(geometries)
+        
+
+    def add_geometries(self, geometries):
+        ''' Adds new geometries to the current set of geometries representing
+        non-free space.  New geometries must be in the same Cartesian space
+        as the previous ones using a x-north/y-east orientation and origin
+        at the area's southwest corner
+        @param geometries: iterable object containing geometries
+        '''
+        self._geometries.update(geometries)
+