add grid map lib and code clean up

AtsushiSakai · AtsushiSakai · commit 9d81a98b5778 · 2019-06-29T21:11:39.000+09:00
diff --git a/Mapping/grid_map_lib/__init__.py b/Mapping/grid_map_lib/__init__.py
diff --git a/Mapping/grid_map_lib/grid_map_lib.py b/Mapping/grid_map_lib/grid_map_lib.py
@@ -0,0 +1,238 @@
+"""
+
+Grid map library in python
+
+author: Atsushi Sakai
+
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+class GridMap:
+    """
+    GridMap class
+    """
+
+    def __init__(self, width, height, resolution,
+                 center_x, center_y, init_val=0.0):
+        """__init__
+
+        :param width: number of grid for width
+        :param height: number of grid for heigt
+        :param resolution: grid resolution [m]
+        :param center_x: center x position  [m]
+        :param center_y: center y position [m]
+        :param init_val: initial value for all grid
+        """
+        self.width = width
+        self.height = height
+        self.resolution = resolution
+        self.center_x = center_x
+        self.center_y = center_y
+
+        self.left_lower_x = self.center_x - \
+                            (self.width / 2.0) * self.resolution
+        self.left_lower_y = self.center_y - \
+                            (self.height / 2.0) * self.resolution
+
+        self.ndata = self.width * self.height
+        self.data = [init_val] * self.ndata
+
+    def get_value_from_xy_index(self, x_ind, y_ind):
+        """get_value_from_xy_index
+
+        when the index is out of grid map area, return None
+
+        :param x_ind: x index
+        :param y_ind: y index
+        """
+
+        grid_ind = self.calc_grid_index_from_xy_index(x_ind, y_ind)
+
+        if 0 <= grid_ind <= self.ndata:
+            return self.data[grid_ind]
+        else:
+            return None
+
+    def get_xy_index_from_xy_pos(self, x_pos, y_pos):
+        """get_xy_index_from_xy_pos
+
+        :param x_pos: x position [m]
+        :param y_pos: y position [m]
+        """
+        x_ind = self.calc_xy_index_from_position(
+            x_pos, self.left_lower_x, self.width)
+        y_ind = self.calc_xy_index_from_position(
+            y_pos, self.left_lower_y, self.height)
+
+        return x_ind, y_ind
+
+    def set_value_from_xy_pos(self, x_pos, y_pos, val):
+        """set_value_from_xy_pos
+
+        return bool flag, which means setting value is succeeded or not
+
+        :param x_pos: x position [m]
+        :param y_pos: y position [m]
+        :param val: grid value
+        """
+
+        x_ind, y_ind = self.get_xy_index_from_xy_pos(x_pos, y_pos)
+
+        if (not x_ind) or (not y_ind):
+            return False  # NG
+
+        flag = self.set_value_from_xy_index(x_ind, y_ind, val)
+
+        return flag
+
+    def set_value_from_xy_index(self, x_ind, y_ind, val):
+        """set_value_from_xy_index
+
+        return bool flag, which means setting value is succeeded or not
+
+        :param x_ind: x index
+        :param y_ind: y index
+        :param val: grid value
+        """
+
+        grid_ind = int(y_ind * self.width + x_ind)
+
+        if 0 <= grid_ind <= self.ndata:
+            self.data[grid_ind] = val
+            return True  # OK
+        else:
+            return False  # NG
+
+    def set_value_from_polygon(self, pol_x, pol_y, val, inside=True):
+        """set_value_from_polygon
+
+        Setting value inside or outside polygon
+
+        :param pol_x: x position list for a polygon
+        :param pol_y: y position list for a polygon
+        :param val: grid value
+        :param inside: setting data inside or outside
+        """
+
+        # making ring polygon
+        if (pol_x[0] != pol_x[-1]) or (pol_y[0] != pol_y[-1]):
+            pol_x.append(pol_x[0])
+            pol_y.append(pol_y[0])
+
+        # setting value for all grid
+        for x_ind in range(self.width):
+            for y_ind in range(self.height):
+                x_pos, y_pos = self.calc_grid_central_xy_position_from_xy_index(
+                    x_ind, y_ind)
+
+                flag = self.check_inside_polygon(x_pos, y_pos, pol_x, pol_y)
+
+                if flag is inside:
+                    self.set_value_from_xy_index(x_ind, y_ind, val)
+
+    def calc_grid_index_from_xy_index(self, x_ind, y_ind):
+        grid_ind = int(y_ind * self.width + x_ind)
+        return grid_ind
+
+    def calc_grid_central_xy_position_from_xy_index(self, x_ind, y_ind):
+        x_pos = self.calc_grid_central_xy_position_from_index(
+            x_ind, self.left_lower_x)
+        y_pos = self.calc_grid_central_xy_position_from_index(
+            y_ind, self.left_lower_y)
+
+        return x_pos, y_pos
+
+    def calc_grid_central_xy_position_from_index(self, index, lower_pos):
+        return lower_pos + index * self.resolution + self.resolution / 2.0
+
+    def calc_xy_index_from_position(self, pos, lower_pos, max_index):
+        ind = int(np.floor((pos - lower_pos) / self.resolution))
+        if 0 <= ind <= max_index:
+            return ind
+        else:
+            return None
+
+    def check_occupied_from_xy_index(self, xind, yind, occupied_val=1.0):
+
+        val = self.get_value_from_xy_index(xind, yind)
+
+        if val >= occupied_val:
+            return True
+        else:
+            return False
+
+    @staticmethod
+    def check_inside_polygon(iox, ioy, x, y):
+
+        npoint = len(x) - 1
+        inside = False
+        for i1 in range(npoint):
+            i2 = (i1 + 1) % (npoint + 1)
+
+            if x[i1] >= x[i2]:
+                min_x, max_x = x[i2], x[i1]
+            else:
+                min_x, max_x = x[i1], x[i2]
+            if not min_x < iox < max_x:
+                continue
+
+            if (y[i1] + (y[i2] - y[i1]) / (x[i2] - x[i1])
+                * (iox - x[i1]) - ioy) > 0.0:
+                inside = not inside
+
+        return inside
+
+    def plot_grid_map(self):
+
+        grid_data = np.reshape(np.array(self.data), (self.height, self.width))
+        fig, ax = plt.subplots()
+        heat_map = ax.pcolor(grid_data, cmap="Blues", vmin=0.0, vmax=1.0)
+        plt.axis("equal")
+        plt.show()
+
+        return heat_map
+
+
+def test_polygon_set():
+    ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
+    oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
+
+    grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
+
+    grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
+
+    grid_map.plot_grid_map()
+
+    plt.axis("equal")
+    plt.grid(True)
+
+
+def test_position_set():
+    grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
+
+    grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
+    grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
+    grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
+    grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
+    grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
+    grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
+
+    grid_map.plot_grid_map()
+
+
+def main():
+    print("start!!")
+
+    test_position_set()
+    test_polygon_set()
+
+    plt.show()
+
+    print("done!!")
+
+
+if __name__ == '__main__':
+    main()
diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
@@ -1,17 +1,19 @@
-import cubic_spline_planner
-import scipy.linalg as la
-import matplotlib.pyplot as plt
-import math
-import numpy as np
 """
 
 Path tracking simulation with LQR speed and steering control
 
 author Atsushi Sakai (@Atsushi_twi)
 
 """
+import math
 import sys
+
+import matplotlib.pyplot as plt
+import numpy as np
+import scipy.linalg as la
+
 sys.path.append("../../PathPlanning/CubicSpline/")
+import cubic_spline_planner
 
 
 # LQR parameter
@@ -59,6 +61,7 @@ def solve_DARE(A, B, Q, R):
     solve a discrete time_Algebraic Riccati equation (DARE)
     """
     X = Q
+    Xn = Q
     maxiter = 150
     eps = 0.01
 
@@ -85,9 +88,9 @@ def dlqr(A, B, Q, R):
     # compute the LQR gain
     K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
 
-    eigVals, eigVecs = la.eig(A - B @ K)
+    eigs = la.eig(A - B @ K)
 
-    return K, X, eigVals
+    return K, X, eigs[0]
 
 
 def lqr_steering_control(state, cx, cy, cyaw, ck, pe, pth_e, sp):
diff --git a/tests/test_rrt_star.py b/tests/test_rrt_star.py
@@ -1,6 +1,7 @@
-from unittest import TestCase
-import sys
 import os
+import sys
+from unittest import TestCase
+
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../PathPlanning/RRTstar/")
 
@@ -23,4 +24,3 @@ def test1(self):
 if __name__ == '__main__':  # pragma: no cover
     test = Test()
     test.test1()
-    print(aa)
