replace math.radians to np.deg2rad

Author: AtsushiSakai

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -11,12 +11,12 @@
 import matplotlib.pyplot as plt
 
 # Estimation parameter of EKF
-Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])**2
-R = np.diag([1.0, math.radians(40.0)])**2
+Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
+R = np.diag([1.0, np.deg2rad(40.0)])**2
 
 #  Simulation parameter
 Qsim = np.diag([0.5, 0.5])**2
-Rsim = np.diag([1.0, math.radians(30.0)])**2
+Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]

Localization/particle_filter/particle_filter.py

@@ -12,11 +12,11 @@
 
 # Estimation parameter of PF
 Q = np.diag([0.1])**2  # range error
-R = np.diag([1.0, math.radians(40.0)])**2  # input error
+R = np.diag([1.0, np.deg2rad(40.0)])**2  # input error
 
 #  Simulation parameter
 Qsim = np.diag([0.2])**2
-Rsim = np.diag([1.0, math.radians(30.0)])**2
+Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]
@@ -170,13 +170,17 @@ def plot_covariance_ellipse(xEst, PEst):
 
     t = np.arange(0, 2 * math.pi + 0.1, 0.1)
 
-    #eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
-    #close to 0 (~10^-20), catch these cases and set the respective variable to 0
-    try: a = math.sqrt(eigval[bigind])
-    except ValueError: a = 0
-
-    try: b = math.sqrt(eigval[smallind])
-    except ValueError: b = 0
+    # eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
+    # close to 0 (~10^-20), catch these cases and set the respective variable to 0
+    try:
+        a = math.sqrt(eigval[bigind])
+    except ValueError:
+        a = 0
+
+    try:
+        b = math.sqrt(eigval[smallind])
+    except ValueError:
+        b = 0
 
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]

Localization/unscented_kalman_filter/unscented_kalman_filter.py

@@ -12,12 +12,12 @@
 import matplotlib.pyplot as plt
 
 # Estimation parameter of UKF
-Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])**2
-R = np.diag([1.0, math.radians(40.0)])**2
+Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
+R = np.diag([1.0, np.deg2rad(40.0)])**2
 
 #  Simulation parameter
 Qsim = np.diag([0.5, 0.5])**2
-Rsim = np.diag([1.0, math.radians(30.0)])**2
+Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]

Mapping/circle_fitting/circle_fitting.py

@@ -93,8 +93,8 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
 def plot_circle(x, y, size, color="-b"):
     deg = list(range(0, 360, 5))
     deg.append(0)
-    xl = [x + size * math.cos(math.radians(d)) for d in deg]
-    yl = [y + size * math.sin(math.radians(d)) for d in deg]
+    xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
+    yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
     plt.plot(xl, yl, color)
 
 
@@ -107,8 +107,8 @@ def main():
     cx = -2.0  # initial x position of obstacle
     cy = -8.0  # initial y position of obstacle
     cr = 1.0  # obstacle radious
-    theta = math.radians(30.0)  # obstacle moving direction
-    angle_reso = math.radians(3.0)  # sensor angle resolution
+    theta = np.deg2rad(30.0)  # obstacle moving direction
+    angle_reso = np.deg2rad(3.0)  # sensor angle resolution
 
     time = 0.0
     while time <= simtime:

Mapping/raycasting_grid_map/raycasting_grid_map.py

@@ -114,7 +114,7 @@ def main():
     print(__file__ + " start!!")
 
     xyreso = 0.25  # x-y grid resolution [m]
-    yawreso = math.radians(10.0)  # yaw angle resolution [rad]
+    yawreso = np.deg2rad(10.0)  # yaw angle resolution [rad]
 
     for i in range(5):
         ox = (np.random.rand(4) - 0.5) * 10.0

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -93,8 +93,8 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
 def plot_circle(x, y, size, color="-b"):
     deg = list(range(0, 360, 5))
     deg.append(0)
-    xl = [x + size * math.cos(math.radians(d)) for d in deg]
-    yl = [y + size * math.sin(math.radians(d)) for d in deg]
+    xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
+    yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
     plt.plot(xl, yl, color)
 
 
@@ -107,8 +107,8 @@ def main():
     cx = -2.0  # initial x position of obstacle
     cy = -8.0  # initial y position of obstacle
     cr = 1.0  # obstacle radious
-    theta = math.radians(30.0)  # obstacle moving direction
-    angle_reso = math.radians(3.0)  # sensor angle resolution
+    theta = np.deg2rad(30.0)  # obstacle moving direction
+    angle_reso = np.deg2rad(3.0)  # sensor angle resolution
 
     time = 0.0
     while time <= simtime:

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -221,10 +221,8 @@ def choose_parent(self, newNode, nearinds):
 
         return newNode
 
-
     def pi_2_pi(self, angle):
-        return (angle + math.pi) % (2*math.pi) - math.pi
-
+        return (angle + math.pi) % (2 * math.pi) - math.pi
 
     def steer(self, rnd, nind):
         #  print(rnd)
@@ -265,7 +263,7 @@ def get_random_point(self):
     def get_best_last_indexs(self):
         #  print("get_best_last_index")
 
-        YAWTH = math.radians(1.0)
+        YAWTH = np.deg2rad(1.0)
         XYTH = 0.5
 
         goalinds = []
@@ -420,8 +418,8 @@ def main():
     ]  # [x,y,size(radius)]
 
     # Set Initial parameters
-    start = [0.0, 0.0, math.radians(0.0)]
-    goal = [6.0, 7.0, math.radians(90.0)]
+    start = [0.0, 0.0, np.deg2rad(0.0)]
+    goal = [6.0, 7.0, np.deg2rad(90.0)]
 
     rrt = RRT(start, goal, randArea=[-2.0, 20.0], obstacleList=obstacleList)
     flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation)

PathPlanning/ClosedLoopRRTStar/pure_pursuit.py

@@ -236,9 +236,9 @@ def main():
     #  state = unicycle_model.State(x=-1.0, y=-5.0, yaw=0.0, v=-30.0 / 3.6)
     #  state = unicycle_model.State(x=10.0, y=5.0, yaw=0.0, v=-30.0 / 3.6)
     #  state = unicycle_model.State(
-    #  x=3.0, y=5.0, yaw=math.radians(-40.0), v=-10.0 / 3.6)
+    #  x=3.0, y=5.0, yaw=np.deg2rad(-40.0), v=-10.0 / 3.6)
     #  state = unicycle_model.State(
-    #  x=3.0, y=5.0, yaw=math.radians(40.0), v=50.0 / 3.6)
+    #  x=3.0, y=5.0, yaw=np.deg2rad(40.0), v=50.0 / 3.6)
 
     lastIndex = len(cx) - 1
     time = 0.0

PathPlanning/ClosedLoopRRTStar/unicycle_model.py

@@ -10,7 +10,7 @@
 
 dt = 0.05  # [s]
 L = 0.9  # [m]
-steer_max = math.radians(40.0)
+steer_max = np.deg2rad(40.0)
 curvature_max = math.tan(steer_max) / L
 curvature_max = 1.0 / curvature_max + 1.0
 
@@ -38,7 +38,7 @@ def update(state, a, delta):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 if __name__ == '__main__':
@@ -47,7 +47,7 @@ def pi_2_pi(angle):
 
     T = 100
     a = [1.0] * T
-    delta = [math.radians(1.0)] * T
+    delta = [np.deg2rad(1.0)] * T
     #  print(delta)
     #  print(a, delta)
 

PathPlanning/DubinsPath/dubins_path_planning.py

@@ -17,7 +17,7 @@ def mod2pi(theta):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def LSL(alpha, beta, d):
@@ -221,7 +221,7 @@ def generate_course(length, mode, c):
         if m is "S":
             d = 1.0 * c
         else:  # turning couse
-            d = math.radians(3.0)
+            d = np.deg2rad(3.0)
 
         while pd < abs(l - d):
             #  print(pd, l)
@@ -270,11 +270,11 @@ def main():
 
     start_x = 1.0  # [m]
     start_y = 1.0  # [m]
-    start_yaw = math.radians(45.0)  # [rad]
+    start_yaw = np.deg2rad(45.0)  # [rad]
 
     end_x = -3.0  # [m]
     end_y = -3.0  # [m]
-    end_yaw = math.radians(-45.0)  # [rad]
+    end_yaw = np.deg2rad(-45.0)  # [rad]
 
     curvature = 1.0
 
@@ -304,11 +304,11 @@ def test():
     for i in range(NTEST):
         start_x = (np.random.rand() - 0.5) * 10.0  # [m]
         start_y = (np.random.rand() - 0.5) * 10.0  # [m]
-        start_yaw = math.radians((np.random.rand() - 0.5) * 180.0)  # [rad]
+        start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0)  # [rad]
 
         end_x = (np.random.rand() - 0.5) * 10.0  # [m]
         end_y = (np.random.rand() - 0.5) * 10.0  # [m]
-        end_yaw = math.radians((np.random.rand() - 0.5) * 180.0)  # [rad]
+        end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0)  # [rad]
 
         curvature = 1.0 / (np.random.rand() * 5.0)