Using utility rot_mat_2d (#683)

PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py

@@ -4,15 +4,18 @@ Grid based sweep planner
 author: Atsushi Sakai
 """
 
-import math
-import os
 import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+
+import math
 from enum import IntEnum
 
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
 import matplotlib.pyplot as plt
 
+from utils.angle import rot_mat_2d
+
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Mapping/")
 
@@ -148,16 +151,14 @@ def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position):
     tx = [ix - sweep_start_position[0] for ix in ox]
     ty = [iy - sweep_start_position[1] for iy in oy]
     th = math.atan2(sweep_vec[1], sweep_vec[0])
-    rot = Rot.from_euler('z', th).as_matrix()[0:2, 0:2]
-    converted_xy = np.stack([tx, ty]).T @ rot
+    converted_xy = np.stack([tx, ty]).T @ rot_mat_2d(th)
 
     return converted_xy[:, 0], converted_xy[:, 1]
 
 
 def convert_global_coordinate(x, y, sweep_vec, sweep_start_position):
     th = math.atan2(sweep_vec[1], sweep_vec[0])
-    rot = Rot.from_euler('z', -th).as_matrix()[0:2, 0:2]
-    converted_xy = np.stack([x, y]).T @ rot
+    converted_xy = np.stack([x, y]).T @ rot_mat_2d(-th)
     rx = [ix + sweep_start_position[0] for ix in converted_xy[:, 0]]
     ry = [iy + sweep_start_position[1] for iy in converted_xy[:, 1]]
     return rx, ry

PathPlanning/HybridAStar/car.py

@@ -6,11 +6,16 @@ author: Zheng Zh (@Zhengzh)
 
 """
 
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+
 from math import cos, sin, tan, pi
 
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 WB = 3.0  # rear to front wheel
 W = 2.0  # width of car
@@ -45,7 +50,7 @@ def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
 
 def rectangle_check(x, y, yaw, ox, oy):
     # transform obstacles to base link frame
-    rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2]
+    rot = rot_mat_2d(yaw)
     for iox, ioy in zip(ox, oy):
         tx = iox - x
         ty = ioy - y

PathPlanning/InformedRRTStar/informed_rrt_star.py

@@ -9,15 +9,19 @@ Direct Sampling of an Admissible Ellipsoidal Heuristic
 https://arxiv.org/pdf/1404.2334.pdf
 
 """
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
 import copy
 import math
 import random
 
 import matplotlib.pyplot as plt
-from scipy.spatial.transform import Rotation as Rot
 import numpy as np
 
+from utils.angle import rot_mat_2d
+
 show_animation = True
 
 
@@ -308,8 +312,7 @@ class InformedRRTStar:
         t = np.arange(0, 2 * math.pi + 0.1, 0.1)
         x = [a * math.cos(it) for it in t]
         y = [b * math.sin(it) for it in t]
-        rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
-        fx = rot @ np.array([x, y])
+        fx = rot_mat_2d(-angle) @ np.array([x, y])
         px = np.array(fx[0, :] + cx).flatten()
         py = np.array(fx[1, :] + cy).flatten()
         plt.plot(cx, cy, "xc")