（4-4-03）轨迹规划算法和优化：多无人机路径规划系统（3）

码农三叔

于 2024-04-14 19:48:19 发布

阅读量493

点赞数 5

CC 4.0 BY-SA版权

分类专栏：《运动控制算法--Python篇》文章标签：算法无人机 python 机器人运动控制路径规划

本文链接：https://blog.csdn.net/asd343442/article/details/137752709

《运动控制算法--Python篇》专栏收录该内容

95 篇文章 ¥29.90 ¥99.00

订阅专栏

超级会员免费看

本文介绍了使用ROS节点`drones_traj_generator.py`，订阅`rigiBodyPath`消息获取无人机路径，进行坐标变换后发布`drone1Path`和`drone2Path`轨迹。主要涉及无人机初始位姿处理、坐标系转换和路径信息的发布。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

4.10.4 轨迹可视化

文件scripts/drones_traj_generator.py是一个ROS节点，通过订阅名为'rigiBodyPath'的Path消息，获取无人机的路径信息，并进行坐标变换后分别发布两个无人机的轨迹信息，这两个无人机分别为'drone1Path'和'drone2Path'。在程序中还包含一个listener函数作为ROS节点的主函数，用于初始化节点，设置订阅者，接收路径信息，并将转换后的轨迹发布出去。通过ROS节点的spin函数保持节点运行，实现了无人机路径的监听和转换发布功能。

# 打印工作目录
print("当前工作目录:", os.getcwd())
# 两个无人机的初始位置
drone_positions = [
    [0.5, 0, 0],
    [-0.5, 0, 0]
]

# 第一个无人机的初始位姿
drone_pose = PoseStamped()
drone_pose.header.frame_id = 'rb_path'
drone_pose.pose.position = Point(drone_positions[0][0], drone_positions[0][1], drone_positions[0][2])
dron