mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 06:37:57 -05:00
b19476f175
* First Commit of Wavefront Coverage Planner * Update wavefront_coverage_path_planner.py * fix CI / CodeFactor problem * Fix mypy error * update * Fix PyCodeStyle * Fix Code Scanning Warning and code styling problem * add simple unittest * followed second code review suggestions
65 lines
2.0 KiB
Python
65 lines
2.0 KiB
Python
import os
|
|
import sys
|
|
import matplotlib.pyplot as plt
|
|
from unittest import TestCase
|
|
|
|
sys.path.append(os.path.dirname(
|
|
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP")
|
|
try:
|
|
import wavefront_coverage_path_planner
|
|
except ImportError:
|
|
raise
|
|
|
|
wavefront_coverage_path_planner.do_animation = False
|
|
|
|
|
|
class TestPlanning(TestCase):
|
|
def wavefront_cpp(self, img, start, goal):
|
|
num_free = 0
|
|
for i in range(img.shape[0]):
|
|
for j in range(img.shape[1]):
|
|
num_free += 1 - img[i][j]
|
|
|
|
DT = wavefront_coverage_path_planner.transform(
|
|
img, goal, transform_type='distance')
|
|
DT_path = wavefront_coverage_path_planner.wavefront(DT, start, goal)
|
|
self.assertEqual(len(DT_path), num_free) # assert complete coverage
|
|
|
|
PT = wavefront_coverage_path_planner.transform(
|
|
img, goal, transform_type='path', alpha=0.01)
|
|
PT_path = wavefront_coverage_path_planner.wavefront(PT, start, goal)
|
|
self.assertEqual(len(PT_path), num_free) # assert complete coverage
|
|
|
|
def test_wavefront_CPP_1(self):
|
|
img_dir = os.path.dirname(
|
|
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
|
|
img = plt.imread(os.path.join(img_dir, 'map', 'test.png'))
|
|
img = 1 - img
|
|
|
|
start = (43, 0)
|
|
goal = (0, 0)
|
|
|
|
self.wavefront_cpp(img, start, goal)
|
|
|
|
def test_wavefront_CPP_2(self):
|
|
img_dir = os.path.dirname(
|
|
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
|
|
img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png'))
|
|
img = 1 - img
|
|
|
|
start = (10, 0)
|
|
goal = (10, 40)
|
|
|
|
self.wavefront_cpp(img, start, goal)
|
|
|
|
def test_wavefront_CPP_3(self):
|
|
img_dir = os.path.dirname(
|
|
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
|
|
img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png'))
|
|
img = 1 - img
|
|
|
|
start = (0, 0)
|
|
goal = (30, 30)
|
|
|
|
self.wavefront_cpp(img, start, goal)
|