def generate_cw_csc_test_1():
turning_radius = 5
sqrt_2 = np.sqrt(2)
start = np.array([-5 / sqrt_2, -5 / sqrt_2, to_rad(135)])
end = np.array([30 + 5 / sqrt_2, -5 / sqrt_2, to_rad(225)])
dc = dubin.DubinsCurve(start, end, turning_radius)
return dc
def generate_ccc_test_2():
turning_radius = 5
sqrt_3 = np.sqrt(3)
start = np.array([5 / 2 * sqrt_3, 5 / 2, to_rad(120)])
end = np.array([5, 0, to_rad(270)])
dc = dubin.DubinsCurve(start, end, turning_radius)
return dc
def generate_ccw_csc_test_2():
turning_radius = 5
sqrt_2 = np.sqrt(2)
start = np.array([0, 5, to_rad(180)])
end = np.array([30 / sqrt_2, -30 / sqrt_2 - 5, to_rad(0)])
dc = dubin.DubinsCurve(start, end, turning_radius)
return dc
def _heuristics(self, start_config, end_config):
# Dubin heuristics
dubin = dc.DubinsCurve(start_config, end_config, self.turning_radius)
dubin_path, dubin_length = dubin.compute()
# Euclidean heuristics
distance = np.linalg.norm(
np.array(start_config[0:2]) - np.array(end_config[0:2]))
return np.fmax(dubin_length, distance)
def generate_random_test():
turning_radius = 5
x_lower = 2 * turning_radius
x_high = 50 - 2 * turning_radius
y_lower = 2 * turning_radius
y_high = 50 - 2 * turning_radius
start = np.array([
random.uniform(x_lower, x_high),
random.uniform(y_lower, y_high),
random.uniform(0, 2 * np.pi)
])
end = np.array([
random.uniform(x_lower, x_high),
random.uniform(y_lower, y_high),
random.uniform(0, 2 * np.pi)
])
dc = dubin.DubinsCurve(start, end, turning_radius)
return dc
def _connect_using_dubin(self, start, end):
min_turning_radius = self.motion_model.get_min_turning_radius()
dubins_curve = dcrv.DubinsCurve(start, end, min_turning_radius)
return dubins_curve.compute()