|
5 | 5 | author: AtsushiSakai(@Atsushi_twi) |
6 | 6 |
|
7 | 7 | """ |
8 | | - |
| 8 | +import matplotlib.pyplot as plt |
| 9 | +import numpy as np |
| 10 | +import copy |
| 11 | +import math |
| 12 | +import random |
9 | 13 | import sys |
10 | 14 | sys.path.append("../ReedsSheppPath/") |
11 | 15 |
|
12 | | -import random |
13 | | -import math |
14 | | -import copy |
15 | | -import numpy as np |
16 | | -import reeds_shepp_path_planning |
17 | | -import matplotlib.pyplot as plt |
| 16 | +try: |
| 17 | + import reeds_shepp_path_planning |
| 18 | +except: |
| 19 | + raise |
18 | 20 |
|
19 | 21 | show_animation = True |
20 | 22 | STEP_SIZE = 0.1 |
@@ -80,7 +82,7 @@ def Planning(self, animation=True): |
80 | 82 | return path |
81 | 83 |
|
82 | 84 | def choose_parent(self, newNode, nearinds): |
83 | | - if len(nearinds) == 0: |
| 85 | + if not nearinds: |
84 | 86 | return newNode |
85 | 87 |
|
86 | 88 | dlist = [] |
@@ -166,7 +168,7 @@ def get_best_last_index(self): |
166 | 168 | # print("OK YAW TH num is") |
167 | 169 | # print(len(fgoalinds)) |
168 | 170 |
|
169 | | - if len(fgoalinds) == 0: |
| 171 | + if not fgoalinds: |
170 | 172 | return None |
171 | 173 |
|
172 | 174 | mincost = min([self.nodeList[i].cost for i in fgoalinds]) |
@@ -194,9 +196,9 @@ def find_near_nodes(self, newNode): |
194 | 196 | nnode = len(self.nodeList) |
195 | 197 | r = 50.0 * math.sqrt((math.log(nnode) / nnode)) |
196 | 198 | # r = self.expandDis * 5.0 |
197 | | - dlist = [(node.x - newNode.x) ** 2 + |
198 | | - (node.y - newNode.y) ** 2 + |
199 | | - (node.yaw - newNode.yaw) ** 2 |
| 199 | + dlist = [(node.x - newNode.x) ** 2 |
| 200 | + + (node.y - newNode.y) ** 2 |
| 201 | + + (node.yaw - newNode.yaw) ** 2 |
200 | 202 | for node in self.nodeList] |
201 | 203 | nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] |
202 | 204 | return nearinds |
@@ -247,9 +249,9 @@ def DrawGraph(self, rnd=None): |
247 | 249 | # input() |
248 | 250 |
|
249 | 251 | def GetNearestListIndex(self, nodeList, rnd): |
250 | | - dlist = [(node.x - rnd.x) ** 2 + |
251 | | - (node.y - rnd.y) ** 2 + |
252 | | - (node.yaw - rnd.yaw) ** 2 for node in nodeList] |
| 252 | + dlist = [(node.x - rnd.x) ** 2 |
| 253 | + + (node.y - rnd.y) ** 2 |
| 254 | + + (node.yaw - rnd.yaw) ** 2 for node in nodeList] |
253 | 255 | minind = dlist.index(min(dlist)) |
254 | 256 |
|
255 | 257 | return minind |
|
0 commit comments