路径规划算法(2) - A*寻路算法 python实现及解析

最新推荐文章于 2025-06-13 14:11:03 发布

原创

最新推荐文章于 2025-06-13 14:11:03 发布 · 3.7k 阅读

28 ·

CC 4.0 BY-SA版权

文章标签：

#ros #路劲规划 #A*

代码

#coding=utf-8
import math

# 启发距离, 当前点和目标点的启发距离; -- 就是简单的曼哈顿距离
def heuristic_distace(Neighbour_node,target_node):
    H = abs(Neighbour_node[0] - target_node[0]) + abs(Neighbour_node[1] - target_node[1])
    return H

def go_around(direction):
    box_length = 1
    diagonal_line = box_length * 1
    if (direction==0 or direction==2 or direction==6 or direction==8):
        return diagonal_line
    elif (direction==1 or direction==3 or direction==4 or direction==5 or direction==7):
        return diagonal_line

def find_coordinate(map,symble):
    #store coordinate
    result=[]
    for index1,value1 in enumerate(map):
        if symble in value1:
            row = index1
            for index2, value2 in enumerate(map[index1]):
                if symble==value2:
                   column = index2
                   result.append([row, column])
    return result

def show_map(map):
    for idx in map: