（11-4-02）基于SLAM的自主路径导航系统：路径规划(1)

11.5.2  A*算法路径规划

文件a_star_main.py实现了一个 ROS 节点，实现了 A* 算法进行路径规划，并利用 ROS 框架中的消息订阅和发布功能与机器人进行交互。具体来说，实现了如下所示的功能。

1. 订阅了机器人的激光雷达数据和里程计信息，以获取机器人当前的位置和地图信息。
2. 通过 A* 算法在给定地图上规划路径，其中包括设置起始点和目标点，以及定义地图的邻居类型、启发式因子和启发式函数。
3. 将路径转换为机器人的全局坐标系，并根据路径点发布路径信息以在 RVIZ 中可视化路径。
4. 通过控制机器人的速度和角度，使其沿着规划的路径移动，直到到达目标点。

该节点在 ROS 框架中运行，通过订阅机器人的激光雷达和里程计信息，实时更新机器人的位置和地图信息，并根据新的目标点进行路径规划和路径跟踪。

文件a_star_main.py的具体实现流程如下所示。

（1）下面这段代码定义了类Node，用于表示路径规划算法中的节点。每个节点具有位置信息、前驱节点、代价等属性，并包含更新代价和判断节点相等的方法。全局变量包括机器人位置、目标位置、地图信息等，以及一些初始参数的设置。

class Node:
    def __init__(self,position):
        self.location = position
        self.prev = None
        self.g_cost = 0
        self.h_cost = 0
        self.f_cost = 0
        self.Occupied = False
    def update_cost(self,g,h):
        self.g_cost = g
        self.h_cost = h
        self.f_cost = g+h
    def __eq__(self, node):
        return self.location == node.location
global robot_location, robot_rotation, final_goal_location, robot_start_location, global_map, final_path, map_odom_trans, goal_reached

global_map = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0,0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0,0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0],
                       [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0],
                       [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0],
                       [0, 0, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0,0, 0],
                       [0, 0, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0,0, 0],
                       [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,1, 0],
                       [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,1, 1],
                       [0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,1, 1],
                       [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,1, 1],
                       [0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,1, 0],
                       [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0,0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1,1, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1,1, 0],
                       [0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 1,1, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1,1, 1]])


print('Loaded Map of Shpae - ',global_map.shape)
final_path = None
laser_ranges = np.zeros((361,))
laser_intensities = np.zeros((361,))
robot_start_location = [-8,-2,0]
robot_location = robot_start_location
robot_rotation = [0,0,0]
goal_reached = False
iter_break = False


final_goal_location = [rospy.get_param('/goalx'), rospy.get_param('/goaly'), 0]

（2）下面这段代码实现了感知部分的功能，包括了多个功能函数，这些函数共同实现了机器人感知环境并在RVIZ中进行可视化的功能。

1. 函数goal_location_marker：发布目标位置的标记，用于在RVIZ中显示目标位置的文本标记。
2. 函数callback_laser：处理激光消息，将激光范围和强度保存到全局变量中。
3. 函数callback_odom：获取里程计读数并分配给全局变量，同时发布机器人的标记和占据栅格地图。
4. 函数robot_cube_publisher：发布机器人的立方体标记，用于在RVIZ中显示机器人的位置和方向。
5. 函数OccupancyGrid_publish：发布占据栅格地图，用于在RVIZ中显示地图。
6. 函数points_publisher：发布路径点，用于在RVIZ中显示路径。

##################### 感知部分 #####################

def goal_location_marker(final_goal_location):
    '''
    发布目标位置文本标记到RVIZ
    '''
    marker_pub = rospy.Publisher('goal_location_marker', Marker, queue_size=1) # 发布机器人位置到RVIZ
    marker_data = Marker()
    marker_data.type = marker_data.TEXT_VIEW_FACING
    marker_data.action = marker_data.ADD
    marker_data.header.frame_id = '/odom'

    marker_data.scale.z = 1 # 高度

    marker_data.pose.position.x = final_goal_location[0]
    marker_data.pose.position.y = final_goal_location[1]

    marker_data.color.a = 1
    marker_data.color.r = 1
    marker_data.color.g = 1
    marker_data.color.b = 1
    p = final_goal_location
    marker_data.text = 'GOAL'

    # for p in points_list:
    #     marker_data.points.append(Point(p[0],p[1],p[2]))
    marker_pub.publish(marker_data)

def callback_laser(msg):
    '''
    激光消息处理
    '''
    global laser_ranges, laser_intensities
    laser_ranges = np.array(msg.ranges)
    laser_intensities = np.array(msg.intensities)

def callback_odom(msg):
    '''
    获取里程计读数并分配给全局变量
    '''
    global robot_location, robot_rotation, robot_orientation, global_map
    location = [msg.pose.pose.position.x, msg.pose.pose.position.y]
    robot_location = location
    orientation = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
    (roll, pitch, yaw) = euler_from_quaternion(orientation)
    rot = [roll, pitch, yaw]
    robot_rotation = rot
    robot_orientation = orientation
    robot_cube_publisher(location,robot_orientation)
    map_g = np.copy(global_map)
    OccupancyGrid_publish(map_g[::-1]) # 发布占据栅格地图
    goal_location_marker(final_goal_location) # 发布目标位置

def robot_cube_publisher(trans,rot):
    '''
    发布机器人立方体标记到RVIZ
    '''
    marker_pub = rospy.Publisher('robot_marker', Marker, queue_size=1) # 发布机器人位置到RVIZ
    marker_data = Marker()
    marker_data.type = marker_data.CUBE
    marker_data.pose.position.x = trans[0]
    marker_data.pose.position.y = trans[1]
    qot = rot
    marker_data.pose.orientation.x = qot[0]
    marker_data.pose.orientation.y = qot[1]
    marker_data.pose.orientation.z = qot[2]
    marker_data.pose.orientation.w = qot[3]
    marker_data.header.frame_id = '/odom'
    marker_data.scale.x = 0.35
    marker_data.scale.y = 0.35
    marker_data.scale.z = 0.25
    marker_data.color.a = 1
    marker_data.color.r = 0
    marker_data.color.g = 0
    marker_data.color.b = 255
    marker_pub.publish(marker_data)

def OccupancyGrid_publish(global_map):
    '''
    发布占据栅格地图到RVIZ
    '''
    data = np.array(100*global_map,dtype=np.int8)
    map_msg = OccupancyGrid()
    map_msg.header.frame_id = '/odom'
    map_msg.info = MapMetaData()
    map_msg.info.resolution = 1
    pose_msg = Pose()
    pose_msg.position.x = -9-0.5
    pose_msg.position.y = -10-0.5
    pose_msg.orientation.x = 0
    map_msg.info.origin = pose_msg
    map_msg.info.height = data.shape[0]
    map_msg.info.width = data.shape[1]
    map_msg.data = data.ravel()
    grid_publisher = rospy.Publisher('map', OccupancyGrid, queue_size=1) # 发布占据栅格地图到RVIZ
    grid_publisher.publish(map_msg)

def points_publisher(points_list):
    marker_pub = rospy.Publisher('path_points', Marker, queue_size=1) # 发布机器人位置到RVIZ
    marker_data = Marker()
    marker_data.type = marker_data.LINE_STRIP
    marker_data.action = marker_data.ADD
    marker_data.header.frame_id = '/odom'

    marker_data.scale.x = 0.5 # 宽度
    # marker_data.scale.y = 1 # 高度

    marker_data.color.a = 0.6
    marker_data.color.r = 0
    marker_data.color.g = 1
    marker_data.color.b = 0

    for p in points_list:
        marker_data.points.append(Point(p[0],p[1],0))
    marker_pub.publish(marker_data)

（3）下面代码实现了基于A*算法的路径规划功能，核心功能是A_STAR函数，用于在给定网格地图上计算从起始位置到目标位置的最优路径。通过使用不同的邻居类型和距离方法，可以灵活地适应不同的地图和需求。除了A_STAR函数外，还包括了功能函数convert_path和Distance_compute，分别用于转换和旋转路径以及计算两点之间的距离。

#################### 规划部分 #########################

def A_STAR(global_map, start, end, Type='8c', e=1, heuristic='eu'):
    """
    实现给定网格地图的A*算法
    参数:
    global_map - np.array()
    start - 起始位置 [x,y]
    end - 结束位置 [x,y]
    Type - 邻居类型 ('8c' 或 '4c')
    e - 启发因子
    heuristic - 距离方法 ('d' - 距离, 'eu' - 欧几里得距离, 'manhattan' - 曼哈顿距离)
    参考 - https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2
    """
    print('生成新路径')
    start_node = Node(start) # 初始化起始节点
    end_node = Node(end) # 初始化结束节点

    # 检查起点和终点是否在地图范围内
    if (start_node.location[1] > global_map.shape[0]-1 or start_node.location[1] < 0 or start_node.location[0] > global_map.shape[1]-1 or start_node.location[0] < 0):
        sys.exit('[错误] 起始位置超出范围')
    if (end_node.location[1] > global_map.shape[0]-1 or end_node.location[1] < 0 or end_node.location[0] > global_map.shape[1]-1 or end_node.location[0] < 0):
        sys.exit('[错误] 结束位置超出范围')

    open_list = [start_node] # 初始化开放列表
    closed_nodes = [] # 初始化关闭列表
    iterations = 0

    while open_list:
        current_node = open_list[0]
        index = 0
        iterations = iterations + 1

        # 如果迭代次数超过4000且类型为'8c'，则尝试使用4个邻居
        if(iterations > 4000 and Type == '8c'): 
            print('尝试使用4个邻居')
            return None

        # 检查当前节点的成本是否最低
        for i, x in enumerate(open_list):
            if x.f_cost < current_node.f_cost:
                current_node = x
                index = i
        open_list.pop(index)
        closed_nodes.append(current_node)

        # 检查是否到达目标节点
        if current_node == end_node:
            path = []
            node = current_node
            while node is not None:
                path.append(node.location)
                node = node.prev
            # 返回从终点到起点的反向路径
            return path[::-1]

        neibhours = []
        # 8个邻居方法的索引更改
        i_list = [0, 0, -1, 1, -1, -1, 1, 1]
        j_list = [-1, 1, 0, 0, -1, 1, -1, 1]
        # 4个邻居方法的索引更改
        if(Type == '4c'):
            i_list = [0, 0, 1, -1]
            j_list = [1, -1, 0, 0]

        # 检查当前节点的所有邻居
        for k in range(len(i_list)):

            node_pos = [current_node.location[0]+i_list[k], current_node.location[1]+j_list[k]]
            # 地图边界检查
            if (node_pos[1] > global_map.shape[0]-1 or node_pos[1] < 0 or node_pos[0] > global_map.shape[1]-1 or node_pos[0] < 0):
                continue
            # 网格中的占据检查
            if global_map[node_pos[1]][node_pos[0]] == 1:
                continue

            # 对角线/角切割检查
            try:
                if(abs(Distance_compute([node_pos[0], node_pos[1]], current_node.location, 'd') - 1.4143) < 0.001 and 
                    global_map[current_node.location[1]+1][current_node.location[0]] == 1 and 
                    global_map[current_node.location[1]][current_node.location[0]+1] == 1):
                    continue
                if(abs(Distance_compute([node_pos[0], node_pos[1]], current_node.location, 'd') - 1.4143) < 0.001 and 
                    global_map[current_node.location[1]-1][current_node.location[0]] == 1 and 
                    global_map[current_node.location[1]][current_node.location[0]-1] == 1):
                    continue
            except IndexError:
                continue

            neibhour_node = Node((node_pos[0], node_pos[1]))
            neibhour_node.prev = current_node # 更新邻居节点的父节点
            neibhours.append(neibhour_node) # 添加到其他邻居列表中

        for neibhour in neibhours:
            if neibhour in closed_nodes:
                continue
            
            # 避免位置与障碍物挤在一起
            comb_nw = [(-1, 0), (-1, 1), (0, 1)]
            comb_ne = [(1, 1), (1, 0), (0, 1)]
            comb_sw = [(-1, 0), (-1, -1), (0, -1)]
            comb_se = [(1, -1), (1, 0), (0, -1)]
            n_fac = 0
            nw_flag = True
            ne_flag = True
            sw_flag = True
            se_flag = True
            try:
                for xp, yp in comb_nw:
                    if(global_map[neibhour.location[1]+yp][neibhour.location[0]+xp] != 1):
                        nw_flag = False
                
                for xp, yp in comb_ne:
                    if(global_map[neibhour.location[1]+yp][neibhour.location[0]+xp] != 1):
                        ne_flag = False
                for xp, yp in comb_se:
                    if(global_map[neibhour.location[1]+yp][neibhour.location[0]+xp] != 1):
                        se_flag = False
                for xp, yp in comb_sw:
                    if(global_map[neibhour.location[1]+yp][neibhour.location[0]+xp] != 1):
                        sw_flag = False
                if(nw_flag == True):
                    n_fac = n_fac + 3
                if(ne_flag == True):
                    n_fac = n_fac + 3
                if(se_flag == True):
                    n_fac = n_fac + 3
                if(sw_flag == True):
                    n_fac = n_fac + 3
            except IndexError:
                pass

            # 更新邻居节点的成本
            g = neibhour.g_cost + 1 + (n_fac**2)*10
            # g = Distance_compute(neibhour.location, start_node.location, heuristic) + n_fac
            h = Distance_compute(neibhour.location, end_node.location, heuristic)
            neibhour.update_cost(g, h*e)
            for onode in open_list:
                if neibhour == onode and neibhour.g_cost > onode.g_cost:
                    continue
            open_list.append(neibhour)
    
def convert_path(path, trans, t):
    '''
    转换并旋转给定的坐标集
    '''
    npath = []
    for x in path:
        mat = [x[0], x[1]]
        mat = rot2d(mat, t)
        npath.append((mat[0]+trans[0], mat[1]+trans[1]))
    return npath

def Distance_compute(pos1, pos2, Type='d'):
    '''
    计算两个位置之间的距离
    '''
    x1 = pos1[0]
    y1 = pos1[1]
    x2 = pos2[0]
    y2 = pos2[1]
    d = ((x1-x2)**2) + ((y1-y2)**2)
    if Type == 'd':
        return math.sqrt(d)
    if Type == 'eu':
        return d
    if Type == 'manhattan':
        return abs(x1-x2)+abs(y1-y2)

def rot2d(v, t):
    '''
    2D旋转点
    '''
    x, y = v[0], v[1]
    xr = x*cos(t)-y*sin(t)
    yr = x*sin(t)+y*cos(t)
    return [xr, yr]

（4）下面代码实现了机器人的运动控制功能，包含了如下所示的功能函数。

1. 函数go_to_goal ：控制机器人朝着目标位置移动，并发布速度和角速度指令。
2. 函数Follow_path：使机器人按照给定路径依次移动，直到达到最终目标点。

def go_to_goal(goal):
    '''
    命令机器人在ROS stage中根据/odom坐标系移动到给定目标位置的函数。
    '''
    global robot_rotation, robot_location
    d = Distance_compute(robot_location,goal)
    theta = robot_rotation[2]
    kl = 1
    ka = 4
    vx = 0
    va = 0
    heading = math.atan2(goal[1]-robot_location[1],goal[0]-robot_location[0])
    err_theta = heading - theta
    if(d>0.01):
        vx = kl*abs(d)
        vx = 1
    if(abs(err_theta)>0.01):
        va = ka*(err_theta)

    vel_1 = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist,queue_size=10) # 向robot_1发布命令
    cmd = geometry_msgs.msg.Twist()
    cmd.linear.x = vx
    cmd.angular.z = va
    vel_1.publish(cmd)

def Follow_path(path):
    '''
    跟随给定路径移动的函数
    - 依次到达路径列表中的所有点
    '''
    global final_goal_location, goal_reached
    cpath = path
    goal_point = cpath[-1]
    print('Following Path -->',cpath)
    for loc in cpath:
        while(Distance_compute(robot_location,loc)>0.1):
            goal_location_marker(final_goal_location)
            points_publisher(cpath)
            go_to_goal(loc)
            if(loc==goal_point):
                goal_reached = True

（5）下面的代码是一个ROS节点，实现了基于A*算法的路径规划以及机器人导航功能。订阅了来自激光传感器和里程计的数据，根据接收到的目标位置参数进行路径规划，并通过控制机器人的运动来导航至目标位置。同时，在RVIZ中可视化路径和目标位置。

if __name__ == '__main__':
    rospy.init_node('A_STAR')
    rate = rospy.Rate(10.0)
    # 订阅/odom和激光传感器
    sub = rospy.Subscriber('/base_scan', LaserScan, callback_laser) # 从/stage接收激光信息
    sub_odom = rospy.Subscriber('/odom', Odometry, callback_odom) # 接收Odom读数
    start = (int(robot_location[0]+9),int(robot_location[1]+10))
    final_goal_location = [rospy.get_param('/goalx'), rospy.get_param('/goaly'), 0]
    end = (int(final_goal_location[0]+9), int(final_goal_location[1]+10))
    # global_map[13][8] = 1 

    # A星因素
    neibhour_type = '8c'
    heuristic = 'eu'
    heuristic_factor = 2
    
    # 在地图坐标中获取路径
    final_path = A_STAR(global_map[::-1],start,end,neibhour_type,heuristic_factor,heuristic)
    if(final_path==None):
        neibhour_type = '4c'
        final_path = A_STAR(global_map[::-1],start,end,neibhour_type,heuristic_factor,heuristic)
    odom_trans = [-9,-10]
    # 将地图坐标中的点转换为全局坐标
    path_odom_frame = convert_path(final_path,odom_trans,0)
    goal_reached = False
    
    while not rospy.is_shutdown():
        # 当目标参数更改时获取新路径
        if(final_goal_location != [rospy.get_param('/goalx'), rospy.get_param('/goaly'), 0]):
            final_goal_location = [rospy.get_param('/goalx'), rospy.get_param('/goaly'), 0]
            start = (int(robot_location[0]+9),int(robot_location[1]+10))
            end = (int(final_goal_location[0]+9), int(final_goal_location[1]+10))
            goal_reached = False
            final_path = A_STAR(global_map[::-1],start,end,neibhour_type,heuristic_factor,heuristic)
            path_odom_frame = convert_path(final_path,odom_trans,0)

        # 在RVIZ中可视化路径
        points_publisher(path_odom_frame)
        goal_location_marker(final_goal_location)
        if(goal_reached==False):
            # 跟随路径直到达到目标
            Follow_path(path_odom_frame)
        elif(goal_reached==True):
            print('Goal Reached')