replace math.radians to np.deg2rad

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -11,12 +11,12 @@ import math
 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 @@ import matplotlib.pyplot as plt
 
 # 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
+    # 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
+    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 math
 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 @@ class RRT():
 
         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 @@ class RRT():
     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 @@ import math
 
 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 @@ if __name__ == '__main__':
 
     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)
 

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -198,11 +198,11 @@ def main():
         oy.append(60.0 - i)
 
     # Set Initial parameters
-    start = [10.0, 10.0, math.radians(90.0)]
-    goal = [50.0, 50.0, math.radians(-90.0)]
+    start = [10.0, 10.0, np.deg2rad(90.0)]
+    goal = [50.0, 50.0, np.deg2rad(-90.0)]
 
     xyreso = 2.0
-    yawreso = math.radians(15.0)
+    yawreso = np.deg2rad(15.0)
 
     rx, ry, ryaw = hybrid_a_star_planning(
         start, goal, ox, oy, xyreso, yawreso)

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

@@ -12,7 +12,7 @@ import pandas as pd
 
 
 def calc_states_list():
-    maxyaw = math.radians(-30.0)
+    maxyaw = np.deg2rad(-30.0)
 
     x = np.arange(1.0, 30.0, 5.0)
     y = np.arange(0.0, 20.0, 2.0)

PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py

@@ -133,8 +133,8 @@ def optimize_trajectory(target, k0, p):
 
 def test_optimize_trajectory():
 
-    #  target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
-    target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(90.0))
+    #  target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0))
+    target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0))
     k0 = 0.0
 
     init_p = np.matrix([6.0, 0.0, 0.0]).T
@@ -152,8 +152,8 @@ def test_optimize_trajectory():
 def test_trajectory_generate():
     s = 5.0  # [m]
     k0 = 0.0
-    km = math.radians(30.0)
-    kf = math.radians(-30.0)
+    km = np.deg2rad(30.0)
+    kf = np.deg2rad(-30.0)
 
     #  plt.plot(xk, yk, "xr")
     #  plt.plot(t, kp)

PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py

@@ -182,12 +182,12 @@ def main():
 
     sx = 10.0  # start x position [m]
     sy = 10.0  # start y position [m]
-    syaw = math.radians(10.0)  # start yaw angle [rad]
+    syaw = np.deg2rad(10.0)  # start yaw angle [rad]
     sv = 1.0  # start speed [m/s]
     sa = 0.1  # start accel [m/ss]
     gx = 30.0  # goal x position [m]
     gy = -10.0  # goal y position [m]
-    gyaw = math.radians(20.0)  # goal yaw angle [rad]
+    gyaw = np.deg2rad(20.0)  # goal yaw angle [rad]
     gv = 1.0  # goal speed [m/s]
     ga = 0.1  # goal accel [m/ss]
     max_accel = 1.0  # max accel [m/ss]

PathPlanning/RRT/rrt_with_pathsmoothing.py

@@ -112,8 +112,8 @@ class RRT():
     def PlotCircle(self, x, y, size):
         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, "-k")
 
     def GetNearestListIndex(self, nodeList, rnd):

PathPlanning/RRTDubins/dubins_path_planning.py

@@ -10,6 +10,7 @@ License MIT
 
 """
 import math
+import numpy as np
 
 
 def mod2pi(theta):
@@ -17,7 +18,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):
@@ -230,7 +231,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)
@@ -281,11 +282,11 @@ if __name__ == '__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
 

PathPlanning/RRTDubins/rrt_dubins.py

@@ -96,10 +96,8 @@ class RRT():
 
         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)
@@ -234,8 +232,8 @@ def main():
     ]  # [x,y,size(radius)]
 
     # Set Initial parameters
-    start = [0.0, 0.0, math.radians(0.0)]
-    goal = [10.0, 10.0, math.radians(0.0)]
+    start = [0.0, 0.0, np.deg2rad(0.0)]
+    goal = [10.0, 10.0, np.deg2rad(0.0)]
 
     rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
     path = rrt.Planning(animation=show_animation)

PathPlanning/RRTStarDubins/dubins_path_planning.py

@@ -13,7 +13,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):
@@ -226,7 +226,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)
@@ -277,11 +277,11 @@ if __name__ == '__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
 

PathPlanning/RRTStarDubins/rrt_star_dubins.py

@@ -97,7 +97,7 @@ class RRT():
         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)
@@ -138,7 +138,7 @@ class RRT():
     def get_best_last_index(self):
         #  print("get_best_last_index")
 
-        YAWTH = math.radians(1.0)
+        YAWTH = np.deg2rad(1.0)
         XYTH = 0.5
 
         goalinds = []
@@ -281,8 +281,8 @@ def main():
     ]  # [x,y,size(radius)]
 
     # Set Initial parameters
-    start = [0.0, 0.0, math.radians(0.0)]
-    goal = [10.0, 10.0, math.radians(0.0)]
+    start = [0.0, 0.0, np.deg2rad(0.0)]
+    goal = [10.0, 10.0, np.deg2rad(0.0)]
 
     rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
     path = rrt.Planning(animation=show_animation)

PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py

@@ -106,7 +106,7 @@ class RRT():
         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):
 
@@ -148,7 +148,7 @@ class RRT():
     def get_best_last_index(self):
         #  print("get_best_last_index")
 
-        YAWTH = math.radians(3.0)
+        YAWTH = np.deg2rad(3.0)
         XYTH = 0.5
 
         goalinds = []
@@ -308,8 +308,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, 15.0], obstacleList=obstacleList)
     path = rrt.Planning(animation=show_animation)

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -331,7 +331,7 @@ def generate_local_course(L, lengths, mode, maxc, step_size):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
@@ -387,11 +387,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() * 20.0)
         step_size = 0.1
@@ -424,11 +424,11 @@ def main():
 
     start_x = -1.0  # [m]
     start_y = -4.0  # [m]
-    start_yaw = math.radians(-20.0)  # [rad]
+    start_yaw = np.deg2rad(-20.0)  # [rad]
 
     end_x = 5.0  # [m]
     end_y = 5.0  # [m]
-    end_yaw = math.radians(25.0)  # [rad]
+    end_yaw = np.deg2rad(25.0)  # [rad]
 
     curvature = 1.0
     step_size = 0.1

PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py

@@ -21,7 +21,7 @@ R = np.eye(2)
 # parameters
 dt = 0.1  # time tick[s]
 L = 0.5  # Wheel base of the vehicle [m]
-max_steer = math.radians(45.0)  # maximum steering angle[rad]
+max_steer = np.deg2rad(45.0)  # maximum steering angle[rad]
 
 show_animation = True
 
@@ -51,7 +51,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
 
 
 def solve_DARE(A, B, Q, R):

PathTracking/lqr_steer_control/lqr_steer_control.py

@@ -24,7 +24,7 @@ R = np.eye(1)
 # parameters
 dt = 0.1  # time tick[s]
 L = 0.5  # Wheel base of the vehicle [m]
-max_steer = math.radians(45.0)  # maximum steering angle[rad]
+max_steer = np.deg2rad(45.0)  # maximum steering angle[rad]
 
 show_animation = True
 #  show_animation = False
@@ -61,7 +61,7 @@ def PIDControl(target, current):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def solve_DARE(A, B, Q, R):

PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py

@@ -45,8 +45,8 @@ WHEEL_WIDTH = 0.2  # [m]
 TREAD = 0.7  # [m]
 WB = 2.5  # [m]
 
-MAX_STEER = math.radians(45.0)  # maximum steering angle [rad]
-MAX_DSTEER = math.radians(30.0)  # maximum steering speed [rad/s]
+MAX_STEER = np.deg2rad(45.0)  # maximum steering angle [rad]
+MAX_DSTEER = np.deg2rad(30.0)  # maximum steering speed [rad/s]
 MAX_SPEED = 55.0 / 3.6  # maximum speed [m/s]
 MIN_SPEED = -20.0 / 3.6  # minimum speed [m/s]
 MAX_ACCEL = 1.0  # maximum accel [m/ss]

SLAM/EKFSLAM/ekf_slam.py

@@ -12,11 +12,11 @@ import matplotlib.pyplot as plt
 
 
 # EKF state covariance
-Cx = np.diag([0.5, 0.5, math.radians(30.0)])**2
+Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2
 
 #  Simulation parameter
-Qsim = np.diag([0.2, math.radians(1.0)])**2
-Rsim = np.diag([1.0, math.radians(10.0)])**2
+Qsim = np.diag([0.2, np.deg2rad(1.0)])**2
+Rsim = np.diag([1.0, np.deg2rad(10.0)])**2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]
@@ -203,7 +203,7 @@ def jacobH(q, delta, x, i):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def main():

SLAM/FastSLAM1/fast_slam1.py

@@ -12,12 +12,12 @@ import matplotlib.pyplot as plt
 
 
 # Fast SLAM covariance
-Q = np.diag([3.0, math.radians(10.0)])**2
-R = np.diag([1.0, math.radians(20.0)])**2
+Q = np.diag([3.0, np.deg2rad(10.0)])**2
+R = np.diag([1.0, np.deg2rad(20.0)])**2
 
 #  Simulation parameter
-Qsim = np.diag([0.3, math.radians(2.0)])**2
-Rsim = np.diag([0.5, math.radians(10.0)])**2
+Qsim = np.diag([0.3, np.deg2rad(2.0)])**2
+Rsim = np.diag([0.5, np.deg2rad(10.0)])**2
 OFFSET_YAWRATE_NOISE = 0.01
 
 DT = 0.1  # time tick [s]
@@ -325,7 +325,7 @@ def motion_model(x, u):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def main():

SLAM/FastSLAM2/fast_slam2.py

@@ -12,12 +12,12 @@ import matplotlib.pyplot as plt
 
 
 # Fast SLAM covariance
-Q = np.diag([3.0, math.radians(10.0)])**2
-R = np.diag([1.0, math.radians(20.0)])**2
+Q = np.diag([3.0, np.deg2rad(10.0)])**2
+R = np.diag([1.0, np.deg2rad(20.0)])**2
 
 #  Simulation parameter
-Qsim = np.diag([0.3, math.radians(2.0)])**2
-Rsim = np.diag([0.5, math.radians(10.0)])**2
+Qsim = np.diag([0.3, np.deg2rad(2.0)])**2
+Rsim = np.diag([0.5, np.deg2rad(10.0)])**2
 OFFSET_YAWRATE_NOISE = 0.01
 
 DT = 0.1  # time tick [s]
@@ -354,7 +354,7 @@ def motion_model(x, u):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def main():

SLAM/GraphBasedSLAM/graph_based_slam.py

@@ -18,8 +18,8 @@ import matplotlib.pyplot as plt
 
 
 #  Simulation parameter
-Qsim = np.diag([0.2, math.radians(1.0)])**2
-Rsim = np.diag([0.1, math.radians(10.0)])**2
+Qsim = np.diag([0.2, np.deg2rad(1.0)])**2
+Rsim = np.diag([0.1, np.deg2rad(10.0)])**2
 
 DT = 2.0  # time tick [s]
 SIM_TIME = 100.0  # simulation time [s]
@@ -29,7 +29,7 @@ STATE_SIZE = 3  # State size [x,y,yaw]
 # Covariance parameter of Graph Based SLAM
 C_SIGMA1 = 0.1
 C_SIGMA2 = 0.1
-C_SIGMA3 = math.radians(1.0)
+C_SIGMA3 = np.deg2rad(1.0)
 
 MAX_ITR = 20  # Maximum iteration
 
@@ -253,7 +253,7 @@ def motion_model(x, u):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def main():

SLAM/iterative_closest_point/iterative_closest_point.py

@@ -138,7 +138,7 @@ def main():
     # simulation parameters
     nPoint = 10
     fieldLength = 50.0
-    motion = [0.5, 2.0, math.radians(-10.0)]  # movement [x[m],y[m],yaw[deg]]
+    motion = [0.5, 2.0, np.deg2rad(-10.0)]  # movement [x[m],y[m],yaw[deg]]
 
     nsim = 3  # number of simulation
 

tests/test_dubins_path_planning.py

@@ -1,6 +1,6 @@
 from unittest import TestCase
 from PathPlanning.DubinsPath import dubins_path_planning
-import math
+import numpy as np
 
 
 class Test(TestCase):
@@ -8,11 +8,11 @@ class Test(TestCase):
     def test1(self):
         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