Add comments and restructure reference path class.

Add update_bounds method.

reference_path.py

@@ -24,7 +24,20 @@ class Waypoint:
         self.psi = psi
         self.kappa = kappa
 
+        # information about drivable area at waypoint
+        # upper and lower bound of drivable area orthogonal to
+        # waypoint orientation
+        self.lb = None
+        self.ub = None
+        self.border_cells = None
+
     def __sub__(self, other):
+        """
+        Overload subtract operator. Difference of two waypoints is equal to
+        their euclidean distance.
+        :param other: subtrahend
+        :return: euclidean distance between two waypoints
+        """
         return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5
 
 
@@ -34,35 +47,46 @@ class Waypoint:
 
 
 class ReferencePath:
-    def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, width_info=False):
+    def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, max_width):
         """
         Reference Path object. Create a reference trajectory from specified
         corner points with given resolution. Smoothing around corners can be
-        applied.
+        applied. Waypoints represent center-line of the path with specified
+        maximum width to both sides.
+        :param map: map object on which path will be placed
+        :param wp_x: x coordinates of corner points in global coordinates
+        :param wp_y: y coordinates of corner points in global coordinates
+        :param resolution: resolution of the path in m/wp
+        :param smoothing_distance: number of waypoints used for smoothing the
+        path by averaging neighborhood of waypoints
+        :param max_width: maximum width of path to both sides in m
         """
-        # precision
+
+        # Precision
         self.eps = 1e-12
 
-        # map
+        # Map
         self.map = map
 
-        # resolution of the path
+        # Resolution of the path
         self.resolution = resolution
 
-        # look ahead distance for path averaging
+        # Look ahead distance for path averaging
         self.smoothing_distance = smoothing_distance
 
-        # waypoints with x, y, psi, k
+        # List of waypoint objects
-        self.waypoints = self.construct_path(wp_x, wp_y)
+        self.waypoints = self._construct_path(wp_x, wp_y)
 
-        # path width
+        # Compute path width (attribute of each waypoint)
-        self.get_width_info = width_info
+        self._compute_width(max_width=max_width)
-        if self.get_width_info:
-            self.width_info = self.compute_width()
-            self.min_width = (np.min(self.width_info[0, :]),
-                                np.min(self.width_info[3, :]))
 
-    def construct_path(self, wp_x, wp_y):
+    def _construct_path(self, wp_x, wp_y):
+        """
+        Construct path from given waypoints.
+        :param wp_x: x coordinates of waypoints in global coordinates
+        :param wp_y: y coordinates of waypoints in global coordinates
+        :return: list of waypoint objects
+        """
 
         # Number of waypoints
         n_wp = [int(np.sqrt((wp_x[i + 1] - wp_x[i]) ** 2 +
@@ -78,7 +102,7 @@ class ReferencePath:
                     tolist() for i in range(len(wp_y) - 1)]
         wp_y = [wp for segment in wp_y for wp in segment] + [gp_y]
 
-        # smooth path
+        # Smooth path
         wp_xs = []
         wp_ys = []
         for wp_id in range(self.smoothing_distance, len(wp_x) -
@@ -88,147 +112,282 @@ class ReferencePath:
             wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id
                                             + self.smoothing_distance + 1]))
 
+        # Construct list of waypoint objects
         waypoints = list(zip(wp_xs, wp_ys))
-        waypoints = self.spatial_reformulation(waypoints)
+        waypoints = self._construct_waypoints(waypoints)
+
         return waypoints
 
-    def spatial_reformulation(self, waypoints):
+    def _construct_waypoints(self, waypoint_coordinates):
         """
         Reformulate conventional waypoints (x, y) coordinates into waypoint
-        objects containing (x, y, psi, kappa)
+        objects containing (x, y, psi, kappa, ub, lb)
+        :param waypoint_coordinates: list of (x, y) coordinates of waypoints in
+        global coordinates
         :return: list of waypoint objects for entire reference path
         """
 
-        waypoints_spatial = []
+        # List containing waypoint objects
-        for wp_id in range(len(waypoints) - 1):
+        waypoints = []
 
-            # get start and goal waypoints
+        # Iterate over all waypoints
-            current_wp = np.array(waypoints[wp_id])
+        for wp_id in range(len(waypoint_coordinates) - 1):
-            next_wp = np.array(waypoints[wp_id + 1])
 
-            # difference vector
+            # Get start and goal waypoints
+            current_wp = np.array(waypoint_coordinates[wp_id])
+            next_wp = np.array(waypoint_coordinates[wp_id + 1])
+
+            # Difference vector
             dif_ahead = next_wp - current_wp
 
-            # angle ahead
+            # Angle ahead
             psi = np.arctan2(dif_ahead[1], dif_ahead[0])
 
-            # distance to next waypoint
+            # Distance to next waypoint
             dist_ahead = np.linalg.norm(dif_ahead, 2)
 
-            # get x and y coordinates of current waypoint
+            # Get x and y coordinates of current waypoint
-            x = current_wp[0]
+            x, y = current_wp[0], current_wp[1]
-            y = current_wp[1]
 
+            # Compute local curvature at waypoint
             # first waypoint
             if wp_id == 0:
                 kappa = 0
             else:
-                prev_wp = np.array(waypoints[wp_id - 1])
+                prev_wp = np.array(waypoint_coordinates[wp_id - 1])
                 dif_behind = current_wp - prev_wp
                 angle_behind = np.arctan2(dif_behind[1], dif_behind[0])
                 angle_dif = np.mod(psi - angle_behind + math.pi, 2 * math.pi) \
                             - math.pi
-                kappa = np.abs(angle_dif / (dist_ahead + self.eps))
+                kappa = angle_dif / (dist_ahead + self.eps)
 
-            waypoints_spatial.append(Waypoint(x, y, psi, kappa))
+            waypoints.append(Waypoint(x, y, psi, kappa))
 
-        return waypoints_spatial
+        return waypoints
 
-    def compute_width(self, max_dist=2.0):
+    def _compute_width(self, max_width):
-        max_dist = max_dist  # m
+        """
-        width_info = np.zeros((6, len(self.waypoints)))
+        Compute the width of the path by checking the maximum free space to
+        the left and right of the center-line.
+        :param max_width: maximum width of the path.
+        """
+
+        # Iterate over all waypoints
         for wp_id, wp in enumerate(self.waypoints):
+            # List containing information for current waypoint
+            width_info = []
+            # Check width left and right of the center-line
             for i, dir in enumerate(['left', 'right']):
-                # get pixel coordinates of waypoint
+                # Get angle orthogonal to path in current direction
-                wp_x, wp_y = self.map.w2m(wp.x, wp.y)
-                # get angle orthogonal to path in current direction
                 if dir == 'left':
                     angle = np.mod(wp.psi + math.pi / 2 + math.pi,
                                  2 * math.pi) - math.pi
                 else:
                     angle = np.mod(wp.psi - math.pi / 2 + math.pi,
                                    2 * math.pi) - math.pi
-                # get closest cell to orthogonal vector
+                # Get closest cell to orthogonal vector
-                t_x, t_y = self.map.w2m(wp.x + max_dist * np.cos(angle), wp.y + max_dist * np.sin(angle))
+                t_x, t_y = self.map.w2m(wp.x + max_width * np.cos(angle), wp.y
-                # compute path between cells
+                                        + max_width * np.sin(angle))
-                width_info[3*i:3*(i+1), wp_id] = self.get_min_dist(wp_x, wp_y, t_x, t_y, max_dist)
+                # Compute distance to orthogonal cell on path border
-        return width_info
+                b_value, b_cell = self._get_min_width(wp, t_x, t_y, max_width)
+                # Add information to list for current waypoint
+                width_info.append(b_value)
+                width_info.append(b_cell)
 
-    def get_min_dist(self, wp_x, wp_y, t_x, t_y, max_dist):
+            # Set waypoint attributes with width to the left and right
-        # get neighboring cells (account for discretization)
+            wp.ub = width_info[0]
-        neighbors_x, neighbors_y = [], []
+            wp.lb = -1 * width_info[2]  # minus can be assumed as waypoints
+            # represent center-line of the path
+            # Set border cells of waypoint
+            wp.border_cells = (width_info[1], width_info[3])
+
+    def _get_min_width(self, wp, t_x, t_y, max_width):
+        """
+        Compute the minimum distance between the current waypoint and the
+        orthogonal cell on the border of the path
+        :param wp: current waypoint
+        :param t_x: x coordinate of border cell in map coordinates
+        :param t_y: y coordinate of border cell in map coordinates
+        :param max_width: maximum path width in m
+        :return: min_width to border and corresponding cell
+        """
+
+        # Get neighboring cells of orthogonal cell (account for
+        # discretization inaccuracy)
+        tn_x, tn_y = [], []
         for i in range(-1, 2, 1):
             for j in range(-1, 2, 1):
-                neighbors_x.append(t_x + i)
+                tn_x.append(t_x+i)
-                neighbors_y.append(t_y + j)
+                tn_y.append(t_y+j)
 
-        # get bresenham paths to all neighboring cells
+        # Get pixel coordinates of waypoint
+        wp_x, wp_y = self.map.w2m(wp.x, wp.y)
+
+        # Get Bresenham paths to all possible cells
         paths = []
-        for t_x, t_y in zip(neighbors_x, neighbors_y):
+        for t_x, t_y in zip(tn_x, tn_y):
             path = list(bresenham(wp_x, wp_y, t_x, t_y))
             paths.append(path)
 
-        min_dist = max_dist
+        # Compute minimum distance to border cell
+        min_width = max_width
+        # map inspected cell to world coordinates
         min_cell = self.map.m2w(t_x, t_y)
         for path in paths:
             for cell in path:
-                t_x = cell[0]
+                t_x, t_y = cell[0], cell[1]
-                t_y = cell[1]
+                # If path goes through occupied cell
-                # if path goes through occupied cell
                 if self.map.data[t_y, t_x] == 0:
-                    # get world coordinates
+                    # Get world coordinates
-                    x, y = self.map.m2w(wp_x, wp_y)
                     c_x, c_y = self.map.m2w(t_x, t_y)
-                    cell_dist = np.sqrt((x - c_x) ** 2 + (y - c_y) ** 2)
+                    cell_dist = np.sqrt((wp.x - c_x) ** 2 + (wp.y - c_y) ** 2)
-                    if cell_dist < min_dist:
+                    if cell_dist < min_width:
-                        min_dist = cell_dist
+                        min_width = cell_dist
                         min_cell = (c_x, c_y)
-        dist_info = np.array([min_dist, min_cell[0], min_cell[1]])
-        return dist_info
 
-    def show(self):
+        # decrease min_width by radius of circle around cell
+        min_width -= 1 / np.sqrt(2) * self.map.resolution
 
-        # plot map
+        return min_width, min_cell
+
+    def update_bounds(self, wp_id):
+        """
+        Compute upper and lower bounds of the drivable area orthogonal to
+        the given waypoint.
+        :param wp_id: ID of reference waypoint
+        """
+
+        # Get reference waypoint
+        wp = self.waypoints[wp_id]
+
+        # Get waypoint's border cells in map coordinates
+        ub_p = self.map.w2m(wp.border_cells[0][0], wp.border_cells[0][1])
+        lb_p = self.map.w2m(wp.border_cells[1][0], wp.border_cells[1][1])
+
+        # Compute path from left border cell to right border cell
+        path = list(bresenham(ub_p[0], ub_p[1], lb_p[0], lb_p[1]))
+
+        # Initialize upper and lower bound of drivable area to
+        # upper bound of path
+        ub_o, lb_o = ub_p, ub_p
+
+        # Initialize upper and lower bound of best segment to upper bound of
+        # path
+        ub_ls, lb_ls = ub_p, ub_p
+
+        # Iterate over path from left border to right border
+        for x, y in path:
+            # If cell is free, update lower bound
+            if self.map.data[y, x] == 1:
+                lb_o = (x, y)
+            # If cell is occupied, end segment. Update best segment if current
+            # segment is larger than previous best segment. Then, reset upper
+            # and lower bound to current cell
+            elif self.map.data[y, x] == 0:
+                if np.sqrt((ub_o[0]-lb_o[0])**2+(ub_o[1]-lb_o[1])**2) > \
+                    np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2):
+                    ub_ls = ub_o
+                    lb_ls = lb_o
+                # Start new segment
+                ub_o = (x, y)
+                lb_o = (x, y)
+
+        # If no segment was set (no obstacle between left and right border),
+        # return original bounds of path
+        if ub_ls == ub_p and lb_ls == ub_p:
+            return wp.lb, wp.ub
+
+        # Transform upper and lower bound cells to world coordinates
+        ub_ls = self.map.m2w(ub_ls[0], ub_ls[1])
+        lb_ls = self.map.m2w(lb_ls[0], lb_ls[1])
+        # Check sign of upper and lower bound
+        angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x)
+                          - wp.psi + math.pi, 2*math.pi) - math.pi
+        angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x)
+                          - wp.psi + math.pi, 2*math.pi) - math.pi
+        sign_ub = np.sign(angle_ub)
+        sign_lb = np.sign(angle_lb)
+        # Compute upper and lower bound of largest drivable area
+        ub = sign_ub * np.sqrt((ub_ls[0]-wp.x)**2+(ub_ls[1]-wp.y)**2)
+        lb = sign_lb * np.sqrt((lb_ls[0]-wp.x)**2+(lb_ls[1]-wp.y)**2)
+
+        # Update member variables of waypoint
+        wp.ub = ub
+        wp.lb = lb
+        wp.border_cells = (ub_ls, lb_ls)
+
+        return lb, ub
+
+    def show(self, display_drivable_area=True):
+        """
+        Display path object on current figure.
+        :param display_drivable_area: If True, display arrows indicating width
+        of drivable area
+        """
+
+        # Clear figure
         plt.clf()
-        plt.imshow(np.flipud(self.map.data),cmap='gray',
+
+        # Plot map in gray-scale and set extent to match world coordinates
+        plt.imshow(np.flipud(self.map.data), cmap='gray',
                    extent=[self.map.origin[0], self.map.origin[0] +
                            self.map.width * self.map.resolution,
                            self.map.origin[1], self.map.origin[1] +
                            self.map.height * self.map.resolution])
-        # plot reference path
+
+        # Get x and y coordinates for all waypoints
         wp_x = np.array([wp.x for wp in self.waypoints])
         wp_y = np.array([wp.y for wp in self.waypoints])
-        plt.scatter(wp_x, wp_y, color='k', s=5)
 
-        if self.get_width_info:
+        # Get x and y locations of border cells for upper and lower bound
-            print('Min Width Left: {:f} m'.format(self.min_width[0]))
+        wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints])
-            print('Min Width Right: {:f} m'.format(self.min_width[1]))
+        wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints])
-            plt.quiver(wp_x, wp_y, self.width_info[1, :] - wp_x,
+        wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints])
-                       self.width_info[2, :] - wp_y, scale=1, units='xy',
+        wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints])
-                       width=0.05, color='#D4AC0D')
+
-            plt.quiver(wp_x, wp_y, self.width_info[4, :] - wp_x,
+        # Plot waypoints
-                       self.width_info[5, :] - wp_y, scale=1, units='xy',
+        plt.scatter(wp_x, wp_y, color='#99A3A4', s=3)
-                       width=0.05, color='#BA4A00')
+
+        # Plot arrows indicating drivable area
+        if display_drivable_area:
+            plt.quiver(wp_x, wp_y, wp_ub_x - wp_x, wp_ub_y - wp_y, scale=1,
+                   units='xy', width=0.2*self.resolution, color='#2ECC71',
+                   headwidth=1, headlength=2)
+            plt.quiver(wp_x, wp_y, wp_lb_x - wp_x, wp_lb_y - wp_y, scale=1,
+                   units='xy', width=0.2*self.resolution, color='#2ECC71',
+                   headwidth=1, headlength=2)
 
 
 if __name__ == '__main__':
 
-    # Create Map
+    # Select Path | 'Race' or 'Q'
-    map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
+    path = 'Race'
 
+    # Create Map
+    if path == 'Race':
+        map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
         # Specify waypoints
-    # Floor 2
+        wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
-    # wp_x = [-9.169, -2.7, 11.9, 7.3, -6.95]
+                1.25, -0.75, -0.75, -0.25]
-    # wp_y = [-15.678, -7.12, 10.9, 14.5, -3.31]
+        wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0,
-    # Race Track
+                -1.5, -1.5]
-    wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, 1.25, -0.75, -0.75, -0.25]
-    wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, -1.5, -1.5]
         # Specify path resolution
         path_resolution = 0.05  # m / wp
-
-    # Smooth Path
         reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
-                                          smoothing_distance=5)
+                                       smoothing_distance=5, max_width=0.22)
+    elif path == 'Q':
+        map = Map(file_path='map_floor2.png')
+        wp_x = [-9.169, 11.9, 7.3, -6.95]
+        wp_y = [-15.678, 10.9, 14.5, -3.31]
+        # Specify path resolution
+        path_resolution = 0.20  # m / wp
+        reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
+                                       smoothing_distance=5, max_width=1.5)
+    else:
+        reference_path = None
+        print('Invalid path!')
+        exit(1)
+
     reference_path.show()
     plt.show()