（7-4-05）RRT算法：基于Gazebo仿真的路径规划系统(5)

7.4.5 解析SDF 文件

SDF 文件是指 Simulation Description Format 文件，是一种用于描述仿真环境、机器人、传感器和其他相关信息的文件格式。它通常用于机器人领域中的仿真和控制任务。SDF 文件使用 XML（可扩展标记语言）格式，因此它可以包含层次结构、标签和属性，以清晰地描述各种对象的特性和关系。在机器人仿真中，SDF 文件常用于描述仿真世界中的物体、地形、障碍物、传感器等。

文件sdf_reader.py实现了一个名为 SDFReader 的类，用于解析包含障碍物信息的 SDF（Simulation Description Format）文件。该类使用库xml.etree.ElementTree来解析 XML 格式的 SDF 文件，并使用 numpy 来处理数据。

import xml.etree.ElementTree as ET
from typing import Optional, List, Tuple, Union
import numpy as np
from numpy import ndarray

class SDFReader:
    def __init__(self):
        self.tree = None
        self.root = None

    def parse(self, filepath: str) -> None:
        """ Parse file to tree """
        if filepath is None:
            return filepath
        self.tree = ET.parse(filepath)
        self.root = self.tree.getroot()

    def get_obstacles(self):
        if self.tree is None:
            return None
        data = self.root.findall(".//visual/*[cylinder]/..")
        poses = np.array([np.array(list(map(float, child[0].text.split(" ")))) for child in data], dtype=object)
        radia = np.array([float(child[1][0][0].text) for child in data], dtype=object)
        pose_radia = list(zip(poses, radia))
        obstacles = np.array([[np.array([obstacle[0][0], obstacle[0][1]]), obstacle[1]] for obstacle in pose_radia], dtype=object)

        x = poses[:, 0] * 100
        y = poses[:, 1] * 100
        x_range = [np.min(x) - 100, np.max(x) + 100]
        y_range = [np.min(y) - 100, np.max(y) + 100]

        return obstacles, (x_range, y_range)
对上述代码的具体说明如下所示。

对上述代码的具体说明如下所示。

parse 方法用于解析 SDF 文件并构建相应的树结构。
get_obstacles 方法从解析后的 SDF 文件中提取障碍物信息，返回障碍物的位置和半径，并计算出障碍物在 X 和 Y 轴上的范围。

7.4.6 主程序

文件main.py主要实现了从SDF文件中解析出障碍物的位置和大小，并利用RRT、RRT和RRT_FN算法在给定地图上规划路径。通过解析SDF文件，创建了一个地图对象，然后在该地图上使用RRT系列算法寻找从起始点到目标点的路径，并绘制出路径和地图的可视化结果。

class SDFReader:
    def __init__(self):
        self.tree = None
        self.root = None

    def parse(self, filepath: str) -> None:
        """ 解析文件并构建树 """
        if filepath is None:
            return filepath
        self.tree = ET.parse(filepath)
        self.root = self.tree.getroot()

    def get_obstacles(self):
        if self.tree is None:
            return None
        data = self.root.findall(".//visual/*[cylinder]/..")
        poses = np.array([np.array(list(map(float, child[0].text.split(" ")))) for child in data], dtype=object)
        radia = np.array([float(child[1][0][0].text) for child in data], dtype=object)
        pose_radia = list(zip(poses, radia))
        obstacles = np.array([[np.array([obstacle[0][0], obstacle[0][1]]), obstacle[1]] for obstacle in pose_radia], dtype=object)

        x = poses[:, 0] * 100
        y = poses[:, 1] * 100
        x_range = [np.min(x) - 100, np.max(x) + 100]
        y_range = [np.min(y) - 100, np.max(y) + 100]

        return obstacles, (x_range, y_range)

NODE_RADIUS = 20

import warnings
import matplotlib.pyplot as plt
import random

from algorithm import RRT, RRT_star, RRT_star_FN
from graph import Graph, OutOfBoundsException
from map import Map
from sdf_reader import SDFReader

warnings.filterwarnings("error")
import matplotlib
matplotlib.use('TkAgg')

def plot_graph(graph: Graph, obstacles: list, xy_range: tuple = None):
    """
    绘制图形和障碍物。
    :param graph: 要绘制的图形
    :param obstacles: 障碍物列表
    :param xy_range: x和y轴范围的元组
    """
    xes = [pos[0] for id, pos in graph.vertices.items()]
    yes = [pos[1] for id, pos in graph.vertices.items()]

    plt.scatter(xes, yes, c='gray')  # 绘制节点
    plt.scatter(graph.start[0], graph.start[1], c='#49ab1f', s=50)
    plt.scatter(graph.goal[0], graph.goal[1], c='red', s=50)

    edges = [(graph.vertices[id_ver], graph.vertices[child]) for pos_ver, id_ver in graph.id_vertex.items()
             for child in graph.children[id_ver]]
    for edge in edges:
        plt.plot([edge[0][0], edge[1][0]], [edge[0][1], edge[1][1]], c='black', alpha=0.5)

    # 绘制障碍物
    plt.gca().set_aspect('equal', adjustable='box')
    for obstacle in obstacles:
        circle = plt.Circle(obstacle[0], obstacle[1], color='black')
        plt.gca().add_patch(circle)

    if xy_range is not None:
        plt.xlim(xy_range[0][0], xy_range[0][1])
        plt.ylim(xy_range[1][0], xy_range[1][1])
    else:
        plt.xlim(0, graph.width)
        plt.ylim(0, graph.height)

if __name__ == '__main__':
    filepath = "/home/grzesiek/turtlebot3_ws/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_world/model.sdf"
    reader = SDFReader()
    reader.parse(filepath)
    obstacles, xy_range = reader.get_obstacles()

    map_width = 200
    map_height = 200
    start_node = (random.uniform(xy_range[0][0], xy_range[0][1]), random.uniform(xy_range[1][0], xy_range[1][1]))
    goal_node = (random.uniform(xy_range[0][0], xy_range[0][1]), random.uniform(xy_range[1][0], xy_range[1][1]))

    my_map = Map((map_width, map_height), start_node, goal_node, NODE_RADIUS)

    my_map.add_obstacles(obstacles * 100)

    G = Graph(start_node, goal_node, map_width, map_height, xy_range)
    iteration = RRT(G, iter_num=500, map=my_map, step_length=25, node_radius=NODE_RADIUS, bias=0)
    plot_graph(G, my_map.obstacles_c, xy_range)
    print(f"RRT algorithm stopped at iteration number: {iteration}")
    plt.show()

    G = Graph(start_node, goal_node, map_width, map_height, xy_range)
    iteration, _ = RRT_star(G, iter_num=500, map=my_map, step_length=25, radius=50, node_radius=NODE_RADIUS, bias=0.01)
    print(f"RRT_star algorithm stopped at iteration number: {iteration}")
    plot_graph(G, my_map.obstacles_c, xy_range)
    plt.show()

    G = Graph(start_node, goal_node, map_width, map_height, xy_range)
    iteration, _ = RRT_star_FN(G, iter_num=2000, map=my_map, step_length=35,
                               radius=50

在运行本项目前需要使用Gazebo仿真器中创建一个模拟环境，然后运行上面的主程序文件main.py进行测试。执行后会可视化展示RRT、RRT*和RRT*-FN算法的路径规划信息，并展示所消耗的时间。如图7-7所示。

图7-7 路径规划可视化