第0章：Python模拟路径算法+创建迷宫地图

        就像所有的大型工程一样，在正式进行ROS1导航部署前，我们可以用python先模拟一下算法和地图，熟悉一下算法的结构，启动的思路，以及采用python这样更便捷的方式将地图文件转换为Gazebo可以识别的world文件，方便后续ROS用Gazebo搭建虚拟环境，这对后续进行ROS开发非常有帮助。本章我们将会实现通过python搭建一张地图，并用python实现A* 算法，并在生成的地图上生成A* 算法导航及路径。

运行环境和IDE：Windows，Anaconda，Pycharm

        本章节的工程将会通过pycharm这个非常好用的IDE进行搭建，采用anaconda虚拟环境，方便管理python工程。工程结构如下：

实现思路：首先写好地图接口，包括读取地图信息并用Pygame显示出地图的样子。其次是需要写一个将地图文件转换成可以被gazebo识别的.world文件。算法部分也用python暂时实现，在地图上用蓝色线条做出标注。

        第一步是实现地图接口，原始的地图是一串字符，其中0代表空地，1代表障碍物，第一步要做的事情就是将这张地图正确读取并且用Pygame显示地图：

#地图类：
class Map:
    def __init__(self, data):
        self.data = data
        self.entry = None
        self.exit = None

    def find_boundary_roads(self):
        size = int(len(self.data) ** 0.5)
        for i in range(size):
            if self.data[i] == 0:
                self.entry = (0, i)
                break
        for i in range(size * (size - 1), size * size):
            if self.data[i] == 0:
                self.exit = (size - 1, i % size)
                break

def load_map(data):
    map_instance = Map(data)
    map_instance.find_boundary_roads()
    return map_instance

def load_map_from_file(file_path):
    map_data = []
    with open(file_path, 'r') as file:
        for line in file:
            # 分割每行的数字，转换为整数，并扩展到map_data列表中
            map_data.extend([int(num) for num in line.split()])
    map_instance = Map(map_data)
    map_instance.find_boundary_roads()
    return map_instance

        接下来第二步，写一个非常简单的导航算法，这里用A*举例，其中的方法主要返回的是探索到的有效路径 ：

# 算法部分：
import heapq
class Node:
    def __init__(self, position=None, parent=None):
        self.parent = parent # 父节点
        self.position = position # 坐标
        self.g = 0 # 起点开始的实际代价
        self.h = 0 # 到终点的代价
        self.f = 0 # g+h总代价

    def __eq__(self, other): # 判断两个节点位置上是否相等
        return self.position == other.position
    def __lt__(self, other):
        # 比较两个节点的 f 值，用于 heapq 维护优先队列
        return self.f < other.f
def astar(map_data, start, end):
    start_node = Node(start)
    end_node = Node(end)
    open_list = [] # 开放列表，待评估的节点
    closed_list = [] # 关闭列表，已经被评估的节点
    heapq.heappush(open_list, (start_node.f, start_node))
    while open_list:
        current_node = heapq.heappop(open_list)[1] # 持续取出f最小的点作为当前节点
        closed_list.append(current_node)

        if current_node == end_node: # 如果这个节点等于 最终的节点则通过父节点反向构建路径
            path = []
            while current_node:
                path.append(current_node.position)
                current_node = current_node.parent # 通过父节点逆向搜索
            return path[::-1]

        (x, y) = current_node.position
        neighbors = [(x - 1, y), (x + 1, y), (x, y - 1), (x, y + 1)] # 获得邻居坐标

        for next in neighbors:
            if next[0] < 0 or next[0] >= len(map_data) ** 0.5 or next[1] < 0 or next[1] >= len(map_data) ** 0.5: # 检查邻居是否越界
                continue
            if map_data[next[0] * int(len(map_data) ** 0.5) + next[1]] == 1: # 检查邻居是否为障碍
                continue
            neighbor = Node(next, current_node)
            if neighbor in closed_list: # 邻居是否为曾经的路径
                continue
            neighbor.g = current_node.g + 1
            neighbor.h = ((neighbor.position[0] - end_node.position[0]) ** 2) + (
                        (neighbor.position[1] - end_node.position[1]) ** 2)
            neighbor.f = neighbor.g + neighbor.h
            if add_to_open(open_list, neighbor):
                heapq.heappush(open_list, (neighbor.f, neighbor))
                
def add_to_open(open_list, neighbor):
    for node in open_list:
        if neighbor == node[1] and neighbor.g > node[1].g:
            return False
    return True

        第三步就是画出地图，并在地图上标记出路径，这里用到了pygame这个比较好用的库用于建图：

# 画出地图
import pygame
import time
from Map1 import load_map_from_file # 前面写的提取地图
from Navi_AStar import astar # 前面写的算法

def draw_map(map_instance, path):
    pygame.init()
    size = int(len(map_instance.data) ** 0.5)
    screen = pygame.display.set_mode((size * 10, size * 10))
    clock = pygame.time.Clock()

    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False

        screen.fill((255, 255, 255))
        for y in range(size):
            for x in range(size):
                rect = pygame.Rect(x * 10, y * 10, 10, 10)
                if map_instance.data[y * size + x] == 1:
                    pygame.draw.rect(screen, (0, 0, 0), rect)
                elif (y, x) in path:
                    pygame.draw.rect(screen, (0, 0, 255), rect)

        pygame.display.flip()
        clock.tick(60)

    pygame.quit()


if __name__ == "__main__":
    # 示例地图数据
    file_path = 'map/map1.txt'  # 文件路径
    map_instance = load_map_from_file(file_path);
    start, end = map_instance.entry, map_instance.exit
    path = astar(map_instance.data, start, end)
    draw_map(map_instance, path)

运行结果：

（蓝色是规划得到的路径，黑色是障碍物搭建的地图）

        最后，因为我们的ROS1模拟实验需要在Gazebo上面运行，所以我们需要将生成的这张地图转换成world类型，方便gazebo去读取：

def generate_gazebo_world(input_file, output_file):
    with open(input_file, 'r') as file:
        lines = file.readlines()
    block_size = 0.5  # 根据实际情况调整
    height = 1  # 物块高度
    # 计算地图偏移量
    last_line = lines[-1].strip().split()
    zero_indices = [i for i, x in enumerate(last_line) if x == '0']
    if zero_indices:
        mid_index = zero_indices[len(zero_indices) // 2]
        map_offset_x = -mid_index * block_size
        map_offset_z = -len(lines) * block_size
    with open(output_file, 'w') as file:
        file.write('<?xml version="1.0"?>\n')
        file.write('<sdf version="1.6">\n')
        file.write('<world name="simple_map_world">\n')
        # 添加全局地板
        file.write('<model name="ground_plane">\n')
        file.write('<static>true</static>\n')
        file.write('<pose>0 0 0 0 0 0</pose>\n')
        file.write('<link name="link">\n')
        file.write('<collision name="collision">\n')
        file.write('<geometry>\n')
        file.write('<plane><normal>0 0 1</normal><size>50 50</size></plane>\n')
        file.write('</geometry>\n')
        file.write('</collision>\n')
        file.write('<visual name="visual">\n')
        file.write('<geometry>\n')
        file.write('<plane><normal>0 0 1</normal><size>50 50</size></plane>\n')
        file.write('</geometry>\n')
        file.write('</visual>\n')
        file.write('</link>\n')
        file.write('</model>\n')
        for i, line in enumerate(lines):
            cells = line.strip().split(' ')
            for j, cell in enumerate(cells):
                if cell == '1':
                    x = j * block_size + map_offset_x + block_size / 2
                    z = i * block_size + map_offset_z + block_size / 2
                    file.write(f'<model name="block_{i}_{j}">\n')
                    file.write('<static>true</static>\n')
                    file.write(f'<pose>{x} {z} {height / 2} 0 0 0</pose>\n')
                    file.write('<link name="link">\n')
                    file.write('<collision name="collision">\n')
                    file.write('<geometry>\n')
                    file.write(f'<box><size>{block_size} {block_size} {height}</size></box>\n')
                    file.write('</geometry>\n')
                    file.write('</collision>\n')
                    file.write('<visual name="visual">\n')
                    file.write('<geometry>\n')
                    file.write(f'<box><size>{block_size} {block_size} {height}</size></box>\n')
                    file.write('</geometry>\n')
                    # 设置材质为黑色
                    file.write('<material>\n')
                    file.write('<ambient>0 0 0 1</ambient>\n')  # 黑色环境光
                    file.write('<diffuse>0 0 0 1</diffuse>\n')  # 黑色漫反射光
                    file.write('<specular>0 0 0 1</specular>\n')  # 黑色镜面光
                    file.write('</material>\n')
                    file.write('</visual>\n')
                    file.write('</link>\n')
                    file.write('</model>\n')

        file.write('</world>\n')
        file.write('</sdf>\n')
# 使用脚本
generate_gazebo_world('map/map1.txt', 'map/mapG.world')

Gazebo运行后的情况：

总结：

        本章主要介绍了用python搭建地图，并简单查看算法运行的结果，将地图转换为Gazebo可以识别的world格式（Gazebo用于ROS的仿真实验），为之后真正将算法部署在ROS系统中做铺垫。

接下来是制定方案，在虚拟环境中测试方案的可行性：

第1章：在ROS1上用Gazebo模拟导航