From 2e6e4069ebd6117d86cc6e8fed4dfbfffbd802a3 Mon Sep 17 00:00:00 2001 From: reso1 Date: Thu, 9 Jul 2020 10:55:02 +0800 Subject: [PATCH 1/4] First commit of Spiral Spanning Tree Coverage --- .../SpiralSpanningTreeCPP/map/test.png | Bin 0 -> 156 bytes .../SpiralSpanningTreeCPP/map/test_2.png | Bin 0 -> 150 bytes .../SpiralSpanningTreeCPP/map/test_3.png | Bin 0 -> 132 bytes ...ral_spanning_tree_coverage_path_planner.py | 303 ++++++++++++++++++ ...ral_spanning_tree_coverage_path_planner.py | 56 ++++ 5 files changed, 359 insertions(+) create mode 100644 PathPlanning/SpiralSpanningTreeCPP/map/test.png create mode 100644 PathPlanning/SpiralSpanningTreeCPP/map/test_2.png create mode 100644 PathPlanning/SpiralSpanningTreeCPP/map/test_3.png create mode 100644 PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py create mode 100644 tests/test_spiral_spanning_tree_coverage_path_planner.py diff --git a/PathPlanning/SpiralSpanningTreeCPP/map/test.png b/PathPlanning/SpiralSpanningTreeCPP/map/test.png new file mode 100644 index 0000000000000000000000000000000000000000..4abca0bf3090e5b22cb5d6a48ed9c55a4024e787 GIT binary patch literal 156 zcmeAS@N?(olHy`uVBq!ia0vp^Iv~seBp9sUv)%<#>?NMQuIx|P8M&EELc9#81BC)S zT^vI!dXxX0cd$uNa(ufYN#sD2|D=hHjSuaGsttBZ-poD05Ss&JNE+~U7<-f(Z~R#> zVMk@i#}!F2466K(PSYECptdJh!x z@^oA^-pY literal 0 HcmV?d00001 diff --git a/PathPlanning/SpiralSpanningTreeCPP/map/test_3.png b/PathPlanning/SpiralSpanningTreeCPP/map/test_3.png new file mode 100644 index 0000000000000000000000000000000000000000..1a50b87ccff5a966f5e078282cefec2f14ba9112 GIT binary patch literal 132 zcmeAS@N?(olHy`uVBq!ia0vp^20*O90VEg-_b)jBq}WS5eO=j~u(R_RiwlJC@d1S_ zJY5_^EP9jwoNtgfSoCow$BN?&i>2I5d1QGtPRUU#f;|BY-CV2^o55JQI eekEFDFfjD$u~_NcJpT`927{-opUXO@geCxLEGFFm literal 0 HcmV?d00001 diff --git a/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py new file mode 100644 index 0000000000..93b08b4c25 --- /dev/null +++ b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py @@ -0,0 +1,303 @@ +""" +Terrain-Adaptive Spiral-STC Coverage Path Planner + +author: Todd Tang +paper: Spiral-STC: An On-Line Coverage Algorithm of Grid Environments + by a Mobile Robot - Gabriely et.al. +link: https://ieeexplore.ieee.org/abstract/document/1013479 +""" + +import os +import sys +import math + +import numpy as np +import matplotlib.pyplot as plt + +do_animation = True + + +class SpiralSTC(): + def __init__(self, occmap): + self.origin_map_height = occmap.shape[0] + self.origin_map_width = occmap.shape[1] + + # original map resolution must be 2^k + if math.log2(self.origin_map_height) % 1 == 0 and \ + math.log2(self.origin_map_width) % 1 == 0: + sys.exit('original map width/height must be 2^k \ + in grayscale .png format') + + self.occmap = occmap + self.merged_map_height = self.origin_map_height // 2 + self.merged_map_width = self.origin_map_width // 2 + + self.edge = [] + + def plan(self, start): + """plan + + performing Spiral Spanning Tree coverage path planning + + :param start: the start node of Spiral-STC + """ + + visit_times = np.zeros( + (self.merged_map_height, self.merged_map_width), dtype=np.int) + visit_times[start[0]][start[1]] = 1 + + route, path = [], [] + # counter-clockwise neighbor finding order + order = [[1, 0], [0, 1], [-1, 0], [0, -1]] + + def is_valid_node(i, j): + is_i_valid_bounded = i >= 0 and i < self.merged_map_height + is_j_valid_bounded = j >= 0 and j < self.merged_map_width + if is_i_valid_bounded and is_j_valid_bounded: + # free only when the 4 sub-cells are all free + return bool( + self.occmap[2*i][2*j] and self.occmap[2*i+1][2*j] + and self.occmap[2*i][2*j+1] and self.occmap[2*i+1][2*j+1]) + return False + + def STC(w, x): + """STC + + recursive function for function + + :param w: parent node + :param x: current node + """ + + found = False + route.append(x) + for inc in order: + ni, nj = x[0] + inc[0], x[1] + inc[1] + if is_valid_node(ni, nj) and visit_times[ni][nj] == 0: + y = (ni, nj) + self.edge.append((x, y)) + found = True + visit_times[ni][nj] += 1 + STC(x, y) + + # backtrace route from node with neighbors all visited + # to first node with unvisited neighbor + if not found: + has_node_with_unvisited_ngb = False + for node in reversed(route): + # drop nodes that have been visited twice + if visit_times[node[0]][node[1]] == 2: + continue + + visit_times[node[0]][node[1]] += 1 + route.append(node) + + for inc in order: + ni, nj = node[0] + inc[0], node[1] + inc[1] + if is_valid_node(ni, nj) and visit_times[ni][nj] == 0: + has_node_with_unvisited_ngb = True + break + + if has_node_with_unvisited_ngb: + break + + # generate route by recursing form start node + STC(None, start) + + # generate path from route + for idx in range(len(route)-1): + dp = abs(route[idx][0] - route[idx+1][0]) + \ + abs(route[idx][1] - route[idx+1][1]) + if dp == 0: + # special handle for round-trip path + path.append(self.round_trip(route[idx-1], route[idx])) + elif dp == 1: + path.append(self.move(route[idx], route[idx+1])) + elif dp == 2: + # special handle for non-adjecent route nodes + mid_node = self.get_intermediate_node(route[idx], route[idx+1]) + path.append(self.move(route[idx], mid_node)) + path.append(self.move(mid_node, route[idx+1])) + else: + sys.exit('adjecent path node distance larger than 2') + + return self.edge, route, path + + def move(self, p, q): + direction = self.get_vector_direction(p, q) + # move east + if direction == 'E': + p = self.get_sub_node(p, 'SE') + q = self.get_sub_node(q, 'SW') + # move west + elif direction == 'W': + p = self.get_sub_node(p, 'NW') + q = self.get_sub_node(q, 'NE') + # move south + elif direction == 'S': + p = self.get_sub_node(p, 'SW') + q = self.get_sub_node(q, 'NW') + # move north + elif direction == 'N': + p = self.get_sub_node(p, 'NE') + q = self.get_sub_node(q, 'SE') + else: + sys.exit('move direction error...') + return [p, q] + + def round_trip(self, last, pivot): + direction = self.get_vector_direction(last, pivot) + if direction == 'E': + return [self.get_sub_node(pivot, 'SE'), + self.get_sub_node(pivot, 'NE')] + elif direction == 'S': + return [self.get_sub_node(pivot, 'SW'), + self.get_sub_node(pivot, 'SE')] + elif direction == 'W': + return [self.get_sub_node(pivot, 'NW'), + self.get_sub_node(pivot, 'SW')] + elif direction == 'N': + return [self.get_sub_node(pivot, 'NE'), + self.get_sub_node(pivot, 'NW')] + else: + sys.exit('round_trip: last->pivot direction error.') + + def get_vector_direction(self, p, q): + # east + if p[0] == q[0] and p[1] < q[1]: + return 'E' + # west + elif p[0] == q[0] and p[1] > q[1]: + return 'W' + # south + elif p[0] < q[0] and p[1] == q[1]: + return 'S' + # north + elif p[0] > q[0] and p[1] == q[1]: + return 'N' + else: + sys.exit('get_vector_direction: Only E/W/S/N direction supported.') + + def get_sub_node(self, node, direction): + if direction == 'SE': + return [2*node[0]+1, 2*node[1]+1] + elif direction == 'SW': + return [2*node[0]+1, 2*node[1]] + elif direction == 'NE': + return [2*node[0], 2*node[1]+1] + elif direction == 'NW': + return [2*node[0], 2*node[1]] + else: + sys.exit('get_sub_node: sub-node direction error.') + + def get_interpolated_path(self, p, q): + # direction p->q: southwest / northeast + if (p[0] < q[0]) ^ (p[1] < q[1]): + ipx = [p[0], p[0], q[0]] + ipy = [p[1], q[1], q[1]] + # direction p->q: southeast / northwest + else: + ipx = [p[0], q[0], q[0]] + ipy = [p[1], p[1], q[1]] + return ipx, ipy + + def get_intermediate_node(self, p, q): + p_ngb, q_ngb = set(), set() + + for m, n in self.edge: + if m == p: + p_ngb.add(n) + if n == p: + p_ngb.add(m) + if m == q: + q_ngb.add(n) + if n == q: + q_ngb.add(m) + + itsc = p_ngb.intersection(q_ngb) + if len(itsc) == 0: + sys.exit('get_intermediate_node: \ + no intermediate node between', p, q) + elif len(itsc) == 1: + return list(itsc)[0] + else: + sys.exit('get_intermediate_node: \ + more than 1 intermediate node between', p, q) + + def viz_plan(self, edge, path, start): + def coord_transform(p): + return [2*p[1] + 0.5, 2*p[0] + 0.5] + + if do_animation: + last = path[0][0] + trajectory = [[last[1]], [last[0]]] + for p, q in path: + distance = math.hypot(p[0]-last[0], p[1]-last[1]) + if distance <= 1.0: + trajectory[0].append(p[1]) + trajectory[1].append(p[0]) + else: + ipx, ipy = self.get_interpolated_path(last, p) + trajectory[0].extend(ipy) + trajectory[1].extend(ipx) + + last = q + + trajectory[0].append(last[1]) + trajectory[1].append(last[0]) + + for idx, state in enumerate(np.transpose(trajectory)): + plt.cla() + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + + # draw spanning tree + plt.imshow(self.occmap, 'gray') + for p, q in edge: + p = coord_transform(p) + q = coord_transform(q) + plt.plot([p[0], q[0]], [p[1], q[1]], '-oc') + sx, sy = coord_transform(start) + plt.plot([sx], [sy], 'pr', markersize=10) + + # draw move path + plt.plot(trajectory[0][:idx+1], trajectory[1][:idx+1], '-k') + plt.plot(state[0], state[1], 'or') + plt.axis('equal') + plt.grid(True) + plt.pause(0.01) + + else: + # draw spanning tree + plt.imshow(self.occmap, 'gray') + for p, q in edge: + p = coord_transform(p) + q = coord_transform(q) + plt.plot([p[0], q[0]], [p[1], q[1]], '-oc') + sx, sy = coord_transform(start) + plt.plot([sx], [sy], 'pr', markersize=10) + + # draw move path + last = path[0][0] + for p, q in path: + distance = math.hypot(p[0]-last[0], p[1]-last[1]) + if distance == 1.0: + plt.plot([last[1], p[1]], [last[0], p[0]], '-k') + else: + ipx, ipy = self.get_interpolated_path(last, p) + plt.plot(ipy, ipx, '-k') + plt.arrow(p[1], p[0], q[1]-p[1], q[0]-p[0], head_width=0.2) + last = q + + plt.show() + + +if __name__ == "__main__": + dir_path = os.path.dirname(os.path.realpath(__file__)) + img = plt.imread(os.path.join(dir_path, 'map', 'test_2.png')) + STC_planner = SpiralSTC(img) + start = (10, 0) + edge, route, path = STC_planner.plan(start) + STC_planner.viz_plan(edge, path, start) diff --git a/tests/test_spiral_spanning_tree_coverage_path_planner.py b/tests/test_spiral_spanning_tree_coverage_path_planner.py new file mode 100644 index 0000000000..f4de4ba5aa --- /dev/null +++ b/tests/test_spiral_spanning_tree_coverage_path_planner.py @@ -0,0 +1,56 @@ +import os +import sys +import matplotlib.pyplot as plt +from unittest import TestCase + +sys.path.append(os.path.dirname( + os.path.abspath(__file__)) + "/../PathPlanning/SpiralSpanningTreeCPP") +try: + import spiral_spanning_tree_coverage_path_planner +except ImportError: + raise + +spiral_spanning_tree_coverage_path_planner.do_animation = False + + +class TestPlanning(TestCase): + def spiral_stc_cpp(self, img, start): + num_free = 0 + for i in range(img.shape[0]): + for j in range(img.shape[1]): + num_free += img[i][j] + + STC_planner = spiral_spanning_tree_coverage_path_planner.SpiralSTC(img) + edge, route, path = STC_planner.plan(start) + + covered_nodes = set() + for p, q in edge: + covered_nodes.add(p) + covered_nodes.add(q) + + # assert complete coverage + self.assertEqual(len(covered_nodes), num_free / 4) + + def test_spiral_stc_cpp_1(self): + img_dir = os.path.dirname( + os.path.abspath(__file__)) + \ + "/../PathPlanning/SpiralSpanningTreeCPP" + img = plt.imread(os.path.join(img_dir, 'map', 'test.png')) + start = (0, 0) + self.spiral_stc_cpp(img, start) + + def test_spiral_stc_cpp_2(self): + img_dir = os.path.dirname( + os.path.abspath(__file__)) + \ + "/../PathPlanning/SpiralSpanningTreeCPP" + img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png')) + start = (10, 0) + self.spiral_stc_cpp(img, start) + + def test_spiral_stc_cpp_3(self): + img_dir = os.path.dirname( + os.path.abspath(__file__)) + \ + "/../PathPlanning/SpiralSpanningTreeCPP" + img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png')) + start = (0, 0) + self.spiral_stc_cpp(img, start) From 92be015407b4eca116a838985045f070d2250456 Mon Sep 17 00:00:00 2001 From: reso1 Date: Thu, 9 Jul 2020 22:23:27 +0800 Subject: [PATCH 2/4] Modify followed by first code review --- ...ral_spanning_tree_coverage_path_planner.py | 60 ++++++++++--------- 1 file changed, 31 insertions(+), 29 deletions(-) diff --git a/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py index 93b08b4c25..8bed9bbab8 100644 --- a/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py +++ b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py @@ -1,5 +1,5 @@ """ -Terrain-Adaptive Spiral-STC Coverage Path Planner +Spiral Spanning Tree Coverage Path Planner author: Todd Tang paper: Spiral-STC: An On-Line Coverage Algorithm of Grid Environments @@ -17,18 +17,17 @@ do_animation = True -class SpiralSTC(): - def __init__(self, occmap): - self.origin_map_height = occmap.shape[0] - self.origin_map_width = occmap.shape[1] +class SpiralSTC: + def __init__(self, occ_map): + self.origin_map_height = occ_map.shape[0] + self.origin_map_width = occ_map.shape[1] - # original map resolution must be 2^k - if math.log2(self.origin_map_height) % 1 == 0 and \ - math.log2(self.origin_map_width) % 1 == 0: - sys.exit('original map width/height must be 2^k \ + # original map resolution must be even + if self.origin_map_height % 2 == 1 or self.origin_map_width % 2 == 1: + sys.exit('original map width/height must be even \ in grayscale .png format') - self.occmap = occmap + self.occ_map = occ_map self.merged_map_height = self.origin_map_height // 2 self.merged_map_width = self.origin_map_width // 2 @@ -51,34 +50,33 @@ def plan(self, start): order = [[1, 0], [0, 1], [-1, 0], [0, -1]] def is_valid_node(i, j): - is_i_valid_bounded = i >= 0 and i < self.merged_map_height - is_j_valid_bounded = j >= 0 and j < self.merged_map_width + is_i_valid_bounded = 0 <= i < self.merged_map_height + is_j_valid_bounded = 0 <= j < self.merged_map_width if is_i_valid_bounded and is_j_valid_bounded: # free only when the 4 sub-cells are all free return bool( - self.occmap[2*i][2*j] and self.occmap[2*i+1][2*j] - and self.occmap[2*i][2*j+1] and self.occmap[2*i+1][2*j+1]) + self.occ_map[2*i][2*j] and self.occ_map[2*i+1][2*j] + and self.occ_map[2*i][2*j+1] and self.occ_map[2*i+1][2*j+1]) return False - def STC(w, x): + def STC(current_node): """STC recursive function for function - :param w: parent node - :param x: current node + :param current_node: current node """ found = False - route.append(x) + route.append(current_node) for inc in order: - ni, nj = x[0] + inc[0], x[1] + inc[1] + ni, nj = current_node[0] + inc[0], current_node[1] + inc[1] if is_valid_node(ni, nj) and visit_times[ni][nj] == 0: - y = (ni, nj) - self.edge.append((x, y)) + neighbor_node = (ni, nj) + self.edge.append((current_node, neighbor_node)) found = True visit_times[ni][nj] += 1 - STC(x, y) + STC(neighbor_node) # backtrace route from node with neighbors all visited # to first node with unvisited neighbor @@ -101,8 +99,8 @@ def STC(w, x): if has_node_with_unvisited_ngb: break - # generate route by recursing form start node - STC(None, start) + # generate route by recusively call STC() from start node + STC(start) # generate path from route for idx in range(len(route)-1): @@ -114,12 +112,12 @@ def STC(w, x): elif dp == 1: path.append(self.move(route[idx], route[idx+1])) elif dp == 2: - # special handle for non-adjecent route nodes + # special handle for non-adjacent route nodes mid_node = self.get_intermediate_node(route[idx], route[idx+1]) path.append(self.move(route[idx], mid_node)) path.append(self.move(mid_node, route[idx+1])) else: - sys.exit('adjecent path node distance larger than 2') + sys.exit('adjacent path node distance larger than 2') return self.edge, route, path @@ -254,7 +252,7 @@ def coord_transform(p): lambda event: [exit(0) if event.key == 'escape' else None]) # draw spanning tree - plt.imshow(self.occmap, 'gray') + plt.imshow(self.occ_map, 'gray') for p, q in edge: p = coord_transform(p) q = coord_transform(q) @@ -271,7 +269,7 @@ def coord_transform(p): else: # draw spanning tree - plt.imshow(self.occmap, 'gray') + plt.imshow(self.occ_map, 'gray') for p, q in edge: p = coord_transform(p) q = coord_transform(q) @@ -294,10 +292,14 @@ def coord_transform(p): plt.show() -if __name__ == "__main__": +def main(): dir_path = os.path.dirname(os.path.realpath(__file__)) img = plt.imread(os.path.join(dir_path, 'map', 'test_2.png')) STC_planner = SpiralSTC(img) start = (10, 0) edge, route, path = STC_planner.plan(start) STC_planner.viz_plan(edge, path, start) + + +if __name__ == "__main__": + main() From 32d343d12ea935c0dc4b88b25763ed87729164f2 Mon Sep 17 00:00:00 2001 From: reso1 Date: Thu, 9 Jul 2020 22:29:44 +0800 Subject: [PATCH 3/4] fix pycodestyle error --- .../spiral_spanning_tree_coverage_path_planner.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py index 8bed9bbab8..7770b48db6 100644 --- a/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py +++ b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py @@ -55,8 +55,11 @@ def is_valid_node(i, j): if is_i_valid_bounded and is_j_valid_bounded: # free only when the 4 sub-cells are all free return bool( - self.occ_map[2*i][2*j] and self.occ_map[2*i+1][2*j] - and self.occ_map[2*i][2*j+1] and self.occ_map[2*i+1][2*j+1]) + self.occ_map[2*i][2*j] + and self.occ_map[2*i+1][2*j] + and self.occ_map[2*i][2*j+1] + and self.occ_map[2*i+1][2*j+1]) + return False def STC(current_node): From fb06dd63bf49f07a710982f4b652af49fc5abf89 Mon Sep 17 00:00:00 2001 From: reso1 Date: Sun, 12 Jul 2020 09:34:08 +0800 Subject: [PATCH 4/4] modifies following 2nd code review --- ...ral_spanning_tree_coverage_path_planner.py | 141 +++++++++--------- ...ral_spanning_tree_coverage_path_planner.py | 6 +- 2 files changed, 77 insertions(+), 70 deletions(-) diff --git a/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py index 7770b48db6..4a6b42db0b 100644 --- a/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py +++ b/PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py @@ -17,7 +17,7 @@ do_animation = True -class SpiralSTC: +class SpiralSpanningTreeCoveragePlanner: def __init__(self, occ_map): self.origin_map_height = occ_map.shape[0] self.origin_map_width = occ_map.shape[1] @@ -36,82 +36,28 @@ def __init__(self, occ_map): def plan(self, start): """plan - performing Spiral Spanning Tree coverage path planning + performing Spiral Spanning Tree Coverage path planning - :param start: the start node of Spiral-STC + :param start: the start node of Spiral Spanning Tree Coverage """ visit_times = np.zeros( (self.merged_map_height, self.merged_map_width), dtype=np.int) visit_times[start[0]][start[1]] = 1 - route, path = [], [] - # counter-clockwise neighbor finding order - order = [[1, 0], [0, 1], [-1, 0], [0, -1]] - - def is_valid_node(i, j): - is_i_valid_bounded = 0 <= i < self.merged_map_height - is_j_valid_bounded = 0 <= j < self.merged_map_width - if is_i_valid_bounded and is_j_valid_bounded: - # free only when the 4 sub-cells are all free - return bool( - self.occ_map[2*i][2*j] - and self.occ_map[2*i+1][2*j] - and self.occ_map[2*i][2*j+1] - and self.occ_map[2*i+1][2*j+1]) - - return False - - def STC(current_node): - """STC - - recursive function for function - - :param current_node: current node - """ - - found = False - route.append(current_node) - for inc in order: - ni, nj = current_node[0] + inc[0], current_node[1] + inc[1] - if is_valid_node(ni, nj) and visit_times[ni][nj] == 0: - neighbor_node = (ni, nj) - self.edge.append((current_node, neighbor_node)) - found = True - visit_times[ni][nj] += 1 - STC(neighbor_node) - - # backtrace route from node with neighbors all visited - # to first node with unvisited neighbor - if not found: - has_node_with_unvisited_ngb = False - for node in reversed(route): - # drop nodes that have been visited twice - if visit_times[node[0]][node[1]] == 2: - continue - - visit_times[node[0]][node[1]] += 1 - route.append(node) - - for inc in order: - ni, nj = node[0] + inc[0], node[1] + inc[1] - if is_valid_node(ni, nj) and visit_times[ni][nj] == 0: - has_node_with_unvisited_ngb = True - break - - if has_node_with_unvisited_ngb: - break - - # generate route by recusively call STC() from start node - STC(start) + # generate route by + # recusively call perform_spanning_tree_coverage() from start node + route = [] + self.perform_spanning_tree_coverage(start, visit_times, route) + path = [] # generate path from route for idx in range(len(route)-1): dp = abs(route[idx][0] - route[idx+1][0]) + \ abs(route[idx][1] - route[idx+1][1]) if dp == 0: # special handle for round-trip path - path.append(self.round_trip(route[idx-1], route[idx])) + path.append(self.get_round_trip_path(route[idx-1], route[idx])) elif dp == 1: path.append(self.move(route[idx], route[idx+1])) elif dp == 2: @@ -124,6 +70,65 @@ def STC(current_node): return self.edge, route, path + def perform_spanning_tree_coverage(self, current_node, visit_times, route): + """perform_spanning_tree_coverage + + recursive function for function + + :param current_node: current node + """ + + def is_valid_node(i, j): + is_i_valid_bounded = 0 <= i < self.merged_map_height + is_j_valid_bounded = 0 <= j < self.merged_map_width + if is_i_valid_bounded and is_j_valid_bounded: + # free only when the 4 sub-cells are all free + return bool( + self.occ_map[2*i][2*j] + and self.occ_map[2*i+1][2*j] + and self.occ_map[2*i][2*j+1] + and self.occ_map[2*i+1][2*j+1]) + + return False + + # counter-clockwise neighbor finding order + order = [[1, 0], [0, 1], [-1, 0], [0, -1]] + + found = False + route.append(current_node) + for inc in order: + ni, nj = current_node[0] + inc[0], current_node[1] + inc[1] + if is_valid_node(ni, nj) and visit_times[ni][nj] == 0: + neighbor_node = (ni, nj) + self.edge.append((current_node, neighbor_node)) + found = True + visit_times[ni][nj] += 1 + self.perform_spanning_tree_coverage( + neighbor_node, visit_times, route) + + # backtrace route from node with neighbors all visited + # to first node with unvisited neighbor + if not found: + has_node_with_unvisited_ngb = False + for node in reversed(route): + # drop nodes that have been visited twice + if visit_times[node[0]][node[1]] == 2: + continue + + visit_times[node[0]][node[1]] += 1 + route.append(node) + + for inc in order: + ni, nj = node[0] + inc[0], node[1] + inc[1] + if is_valid_node(ni, nj) and visit_times[ni][nj] == 0: + has_node_with_unvisited_ngb = True + break + + if has_node_with_unvisited_ngb: + break + + return route + def move(self, p, q): direction = self.get_vector_direction(p, q) # move east @@ -146,7 +151,7 @@ def move(self, p, q): sys.exit('move direction error...') return [p, q] - def round_trip(self, last, pivot): + def get_round_trip_path(self, last, pivot): direction = self.get_vector_direction(last, pivot) if direction == 'E': return [self.get_sub_node(pivot, 'SE'), @@ -161,7 +166,7 @@ def round_trip(self, last, pivot): return [self.get_sub_node(pivot, 'NE'), self.get_sub_node(pivot, 'NW')] else: - sys.exit('round_trip: last->pivot direction error.') + sys.exit('get_round_trip_path: last->pivot direction error.') def get_vector_direction(self, p, q): # east @@ -225,7 +230,7 @@ def get_intermediate_node(self, p, q): sys.exit('get_intermediate_node: \ more than 1 intermediate node between', p, q) - def viz_plan(self, edge, path, start): + def visualize_path(self, edge, path, start): def coord_transform(p): return [2*p[1] + 0.5, 2*p[0] + 0.5] @@ -298,10 +303,10 @@ def coord_transform(p): def main(): dir_path = os.path.dirname(os.path.realpath(__file__)) img = plt.imread(os.path.join(dir_path, 'map', 'test_2.png')) - STC_planner = SpiralSTC(img) + STC_planner = SpiralSpanningTreeCoveragePlanner(img) start = (10, 0) edge, route, path = STC_planner.plan(start) - STC_planner.viz_plan(edge, path, start) + STC_planner.visualize_path(edge, path, start) if __name__ == "__main__": diff --git a/tests/test_spiral_spanning_tree_coverage_path_planner.py b/tests/test_spiral_spanning_tree_coverage_path_planner.py index f4de4ba5aa..622b6ba18c 100644 --- a/tests/test_spiral_spanning_tree_coverage_path_planner.py +++ b/tests/test_spiral_spanning_tree_coverage_path_planner.py @@ -10,7 +10,7 @@ except ImportError: raise -spiral_spanning_tree_coverage_path_planner.do_animation = False +spiral_spanning_tree_coverage_path_planner.do_animation = True class TestPlanning(TestCase): @@ -20,7 +20,9 @@ def spiral_stc_cpp(self, img, start): for j in range(img.shape[1]): num_free += img[i][j] - STC_planner = spiral_spanning_tree_coverage_path_planner.SpiralSTC(img) + STC_planner = spiral_spanning_tree_coverage_path_planner.\ + SpiralSpanningTreeCoveragePlanner(img) + edge, route, path = STC_planner.plan(start) covered_nodes = set()