请问在python中for循环返回None值该如何解决?
目的:路径规划,在路径转折点添加抑制值,来使路径转折点尽可能少。在planning函数中定义了node值的计算式,其中当closed_set中current即路径点的数量大于1时,计算相邻两段斜率是否相等来判断是否为转折点,从而抑制转向点。
问题描述:在planning函数中定义了node值的计算式,其中当closed_set中current的数量大于1时,计算相邻两段斜率是否相等来判断是否为转折点,从而抑制转向点。
在transfer_extra_value函数中定义了抑制值的计算过程,但是在判断closed_set>1,后计算node值时返回None值,造成python报不支持小数和NoneType相加的错误。
def planning(self, sx, sy, gx, gy):
"""
1、sx,sy = nstart gx,gy = ngoal
2、open_set closed_set
3、open_set = nstart
4、将open表中代价最小的子节点=当前节点,并在plot上动态显示,按esc退出
5、如果当前节点等于ngoal,提示找到目标点
6、删除open表中的内容,添加到closed表中
7、基于运动模型定义搜索方式
8、pathx,pathy = 最终路径(传入ngoal,closed_set),返回pathx,pathy
"""
# 1、sx = nstart sy = ngoal 初始化nstart、ngoal坐标作为一个节点,传入节点全部信息
nstart = self.Node(self.calc_xyindex(sx, self.minx), # position min_pos
self.calc_xyindex(sy, self.miny),
0.0,
-1)
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
self.calc_xyindex(gy, self.miny),
0.0,
-1)
# 2、open表、closed表
# 3、起点加入open表
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart #openset储存节点的x,y索引、代价和父节点
while 1:
if len(open_set) == 0: # len(open_set)表示走到终点或无路可走
print("Open set is empty...")
break
# 4、将open表中代价最小的子节点 = 当前节点,并在plot上动态显示,按esc退出
# f(n)=g(n)+h(n)+ 实际代价+预估代价
# 第几个子节点 child_node_id
# 返回的第一个c_id即为self.calc_grid_index(nstart)
c_id = min(
open_set,
key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
current = open_set[c_id] # open表中当前节点 = 当前节点 current第一个点为起点
# 将当前节点显示出来
if show_animation:
plt.plot(self.calc_grid_position(current.x, self.minx),
self.calc_grid_position(current.y, self.miny),
"xc") # 青色x 搜索点
# 按esc退出
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None]
)
if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal!")
ngoal.parent_index = current.parent_index
ngoal.cost = current.cost
break
# 删除open表中的节点,并把它添加到closed表
del open_set[c_id]
closed_set[c_id] = current
if len(closed_set) > 1:
current_parent = closed_set[current.parent_index]
# 基于motion model做栅格扩展,也就是搜索方式,可进行改进
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0], # 当前x+motion列表中第0个元素dx
current.y + self.motion[i][1],
current.cost + self.motion[i][2] ,c_id)
if len(closed_set) > 1:
node = self.Node(current.x + self.motion[i][0], # 当前x+motion列表中第0个元素dx
current.y + self.motion[i][1],
current.cost + self.motion[i][2]+self.transfer_extra_value(current,node,current_parent), c_id)
n_id = self.calc_grid_index(node) # 返回该节点位置index
# If the node is not safe, do nothing
if not self.verify_node(node):
continue
if n_id in closed_set:
continue
if n_id not in open_set:
open_set[n_id] = node
else:
if open_set[n_id].cost > node.cost:
open_set[n_id] = node # 搜索的节点代价更小,替代open表中的节点
def transfer_extra_value(self, current_node,next_node,current_parent_node):
current_node_x = self.calc_grid_position(current_node.x,self.minx)
current_node_y = self.calc_grid_position(current_node.y,self.miny)
next_node_x = self.calc_grid_position(next_node.x,self.minx)
next_node_y = self.calc_grid_position(next_node.y,self.miny)
current_parent_x = self.calc_grid_position(current_parent_node.x, self.minx)
current_parent_y = self.calc_grid_position(current_parent_node.y, self.miny)
# 第一个点或直线上的点
if current_node_x == next_node_x or current_node_y == next_node_y:
return 0
else:
if current_parent_x != current_node_x and current_parent_y != current_node_y and current_node_x != next_node_x \
and current_node_y != next_node_y:
k1 = (current_node_y - current_parent_y) / (current_node_x - current_parent_x)
k2 = (next_node_y - current_node_y) / (next_node_x - current_node_x)
if k1 == k2:
return 0
else:
# 拐向终点的点
if next_node_x == gx or next_node_y == gy:
return 1
# 普通拐点
else:
return 6
推荐文章: