企业官网建设流程全解析

淘宝客api网站架设教程,长沙正规企业网站制作平台,销售平台公司,广告设计制作公司简介9.3.5 解析SDF 文件SDF 文件是指 Simulation Description Format 文件#xff0c;是一种用于描述仿真环境、机器人、传感器和其他相关信息的文件格式。它通常用于机器人领域中的仿真和控制任务。SDF 文件使用 XML#xff08;可扩展标记语言#xff09;格式#xff0c;因此…9.3.5 解析SDF 文件SDF 文件是指 Simulation Description Format 文件是一种用于描述仿真环境、机器人、传感器和其他相关信息的文件格式。它通常用于机器人领域中的仿真和控制任务。SDF 文件使用 XML可扩展标记语言格式因此它可以包含层次结构、标签和属性以清晰地描述各种对象的特性和关系。在机器人仿真中SDF 文件常用于描述仿真世界中的物体、地形、障碍物、传感器等。文件sdf_reader.py实现了一个名为 SDFReader 的类用于解析包含障碍物信息的 SDFSimulation 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], dtypeobject) radia np.array([float(child[1][0][0].text) for child in data], dtypeobject) pose_radia list(zip(poses, radia)) obstacles np.array([[np.array([obstacle[0][0], obstacle[0][1]]), obstacle[1]] for obstacle in pose_radia], dtypeobject) 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 轴上的范围。9.3.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], dtypeobject) radia np.array([float(child[1][0][0].text) for child in data], dtypeobject) pose_radia list(zip(poses, radia)) obstacles np.array([[np.array([obstacle[0][0], obstacle[0][1]]), obstacle[1]] for obstacle in pose_radia], dtypeobject) 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, cgray) # 绘制节点 plt.scatter(graph.start[0], graph.start[1], c#49ab1f, s50) plt.scatter(graph.goal[0], graph.goal[1], cred, s50) 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]], cblack, alpha0.5) # 绘制障碍物 plt.gca().set_aspect(equal, adjustablebox) for obstacle in obstacles: circle plt.Circle(obstacle[0], obstacle[1], colorblack) 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_num500, mapmy_map, step_length25, node_radiusNODE_RADIUS, bias0) plot_graph(G, my_map.obstacles_c, xy_range) print(fRRT 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_num500, mapmy_map, step_length25, radius50, node_radiusNODE_RADIUS, bias0.01) print(fRRT_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_num2000, mapmy_map, step_length35, radius50在运行本项目前需要使用Gazebo仿真器中创建一个模拟环境然后运行上面的主程序文件main.py进行测试。执行后会可视化展示RRT、RRT*和RRT*-FN算法的路径规划信息并展示所消耗的时间。如图9-5所示。图9-5 路径规划可视