test_wavefront_coverage_path_planner.py

importconftest# Add root path to sys.path
importos
importmatplotlib.pyplotasplt
fromPathPlanning.WavefrontCPPimportwavefront_coverage_path_planner

wavefront_coverage_path_planner.do_animation=False


defwavefront_cpp(img, start, goal):
num_free=0
foriinrange(img.shape[0]):
forjinrange(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)
assertlen(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)
assertlen(PT_path) ==num_free# assert complete coverage


deftest_wavefront_CPP_1():
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)

wavefront_cpp(img, start, goal)


deftest_wavefront_CPP_2():
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)

wavefront_cpp(img, start, goal)


deftest_wavefront_CPP_3():
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)

wavefront_cpp(img, start, goal)


if__name__=='__main__':
conftest.run_this_test(__file__)