仿真Demo-D435i视觉避障

效果展示

提示

运行Demo前请将该模块编译

编译代码如下：

cd ~/Prometheus
catkin_make --source Modules/ego_planner_swarm --build build/ego_planner_swarm

仿真脚本启动流程

1. 将FS-i6s遥控器开机并通过USB接口接入电脑

2. 输入以下命令启动二维激光雷达集群避障仿真demo

cd ~/Prometheus/Scripts/simulation/ego_planner/
# 第一次启动该脚本时，需要添加可执行权限
chmod +x ego_planner_p450_d435i.sh
 ./ego_planner_p450_d435i.sh

检查终端运行是否正常

1. ROS主节点
   • 下图所示为正常运行

2. PX4仿真
   • 下图所示为正常运行

3. Prometheus控制
   • 下图所示为正常运行

4. Octomap建图
   • 下图所示为正常运行

5. Ego-planner规划避障
   • 下图所示为正常运行

开始仿真

1. 遥控器拨动SW-A，解锁无人机

2. 拨动SW-B，到中间，进入Prometheus的RC_POS_CONTROL控制

3. 拨动SW-B，到最下面，进入Prometheus的COMMAND_CONTROL控制，无人机会自主起飞

4. 选择 3D Nav Goal
   • 选择后鼠标的图表会发生变化

5. 开始指点
   • 鼠标左键点击目标点，然后不松开，拖向无人机方向（如下图一绿色箭头，提供目标X和Y的位置），然后按住鼠标左键的同时，按下鼠标右键，在画面内向上划，可以提供Z的高度，如下图二所示

• 开始自主规划避障

6. 到达后再次规划
   • 按照步骤再次指点规划

节点运行图

起飞降落例程主要包含/joy_node、/uav1/mavros、/uav_control_main_1、/octomap_server、/gazebo、 /uav1_ego_planner_node、/uav1_traj_server_for_prometheus等ROS节点。

• /joy_node节点为遥控器ROS驱动节点，用以获取遥控器数据,通过话题uav1/prometheus/fake_rc_in将遥控器数据传输给/uav_control_main_1节点。

• /uav1/mavros节点为飞控ROS驱动节点，与飞控进行数据交互。在仿真中，该驱动节点与模拟飞控进行数据交互。

• /uav_control_main_1节点为Prometheus项目中最基础的ROS节点，所有Prometheus项目的功能模块都通过该节点与无人机进行数据交互。

• /octomap_server节点为建图服务节点，通过仿真D435i点云数据来进行建图。

• /gazebo节点为gazebo ROS驱动节点，为/octomap_server节点提供局部点云数据，来进行建图

• /uav1_ego_planner_node节点为ego_planner规划节点，通过/octomap_server节点的话题数据进行建图，输出/uav1/planning/bspline，来进行路线规划

• /uav1_traj_server_for_prometheus节点为轨迹生成节点，由B样条（bspline）生成轨迹，转化为command命令控制无人机飞行

从节点运行图中，我们可以看到有如下的数据话题

• /uav1/prometheus/command：无人机控制接口，对应的消息为prometheus_msgs/UAVCommand
• /uav1/prometheus/state：无人机状态，对应的消息为prometheus_msgs/UAVState
• /uav1/prometheus/fake_rc_in：仿真模拟PX4遥控器数据
• /camera/depth/color/points：局部点云话题输入
• /octomap_point_cloud_centers:点云信息，仿真中由map_generator产生
• /broadcast_bspline:广播的B样条
• /uav1/planning/bspline:发布的B样条

建议阅读/Prometheus/Modules/common/prometheus_msgs/msg内对应的消息文件，了解上面提到的消息详细定义。

代码讲解

脚本位于/home/amov/Prometheus/Scripts/simulation/ego_planner/ego_planner_p450_d435i.sh，打开规划脚本如下

除了运行ros必备启动的roscore，脚本依次运行了四个launch文件

1.sitl_p450_d435i.launch

2.uav_control_main_outdoor.launch

3.depth_to_octomap.launch

4.sitl_ego_planner_basic_octomap.launch

依次查看

1.sitl_p450_d435i.launch

启动prometheus_gazebo下gazebo环境，带有仿真D435i的无人机模型，以及rviz

2.uav_control_main_outdoor.launch

在prometheus_uav_control，启动UAV1控制节点，Prometheus下控制，为后面路径规划发送控制指令做准备

3.depth_to_octomap.launch

在prometheus_gazebo下启动octomap建图服务，为后面ego规划做准备。octomap_server是ROS中一个基于octomap的功能包，具有将点云地图转化为基于Octree的OctoMap的功能。

在ROS中，octomap_server和octomap_map都是用于构建三维地图的软件包。octomap_server软件包提供了一种将输入数据转换为OctoMap格式的方法。它可以将来自激光雷达或深度摄像头等传感器的数据转换为三维点云，并使用这些点云构建OctoMap。octomap_server还可以将OctoMap数据发布为ROS中的OctoMap消息，以便其他ROS节点可以使用它们。octomap_map软件包提供了一种使用OctoMap数据构建环境模型的方法。它可以订阅来自octomap_server的OctoMap消息，并使用该消息构建一个三维地图。该地图可以用于机器人的定位、路径规划和避障等任务。octomap_server用于构建OctoMap数据，而octomap_map用于将OctoMap数据转换为实际可用的地图。

可以查看octomap_server讲解来深入了解octomap

4.sitl_ego_planner_basic_octomap.launch

在ego_planner包下启动ego_planner 仿真，并由B样条生成轨迹，发送控制指令给无人机，控制飞行。其中包含两部分/uav1_ego_planner_node和/uav1_traj_server_for_prometheus，

前者/uav1_ego_planner_node进行ego_planner规划，相关源代码参见ego_planner_node.cpp ，ego_replan_fsm.cpp，planner_manager.cpp，路径：/home/amov/Prometheus/Modules/ego_planner_swarm/plan_manage/src/。

EGO-Planner是一个无ESDF（欧几里得符号距离场）的基于梯度的局部路径规划框架，EGO-Planner由基于梯度的样条优化器和后细化两部分组成。

可以查看EGO-Planner代码讲解来深入了解EGO-Planner

后者/uav1_traj_server_for_prometheus订阅ego_planner规划结果，由B样条（bspline）生成轨迹，转化为command命令控制无人机飞行。其源代码为traj_server_for_prometheus.cpp，路径：/home/amov/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/

操作演示视频