仿真Demo - 二维激光雷达避障

效果展示

提示

运行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_2dlidar_swarm.sh
 ./ego_planner_p450_2dlidar_swarm.sh

检查终端运行是否正常

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

image.png

  1. PX4 仿真 - 下图所示为正常运行

image.png

  1. Prometheus控制 - 下图所示为正常运行

image.png

  1. 雷达滤波 - 下图所示为正常运行

image.png

  1. Octomap建图 - 下图所示为正常运行

image.png

6. Ego-swarm 集群规划避障 - 下图所示为正常运行

image.png

开始仿真

  1. 等待rviz显示三台无人机

image.png

  1. 将遥控器SWA向下拨动以解锁无人机

image.png

解锁成功,第三个终端会出现“vehicle arming, success!”字样,并且Gazebo中无人机解锁。

  1. 遥控器SWB档杆拨到中间位置将无人机控制状态切换到 RC_POS_CONTROL

image.png

切换成功,第三个终端会出现“Switch to RC_POS_CONTROL”字样,且无人机螺旋桨加速旋转。
  1. 遥控器SWB档杆拨到最底部将无人机控制状态切换到 COMMAND_CONTROL

image.png

切换成功,第三个终端会出现“Switch to COMMAND_CONTROL”字样,且无人机会自动起飞到2m高度。
  1. 等待三台无人机都起飞到指定高度后,鼠标左键点击rviz里的3D Nav Goal(位置在下图箭头1所示),然后在rviz界面点击鼠标左键选择目标点,同时按住鼠标右键往上拖动(如箭头2所示)

image.png

注意

如果不按住鼠标左键，在画面内向上划，提供Z的高度，那么Z的高度默认为0，则会出现到达不了目标点的情况！
  1. 目标点设置成功后，会出现三个终点(如下图中箭头1所示); ego-planner会规划出可行路径(如下图中箭头2所示)，三台无人机按照路径飞行。

image.png

  1. 在无人机到达指定目标点后，可重复步骤5 6 ，再次规划下一个目标点。

节点运行图

image.png

二维激光雷达集群避障仿真demo主要包含： /gazebo 、/motion_goal_swarm 、/uav1_ego_planner_node 、/uav2_ego_planner_node 、/uav3_ego_planner_node 、/uav1_traj_server_forprometheus 、/uav2_traj_server_forprometheus 、/uav3_traj_server_forprometheus 、/joy_node 、/uav_control_main_1 、/uav_control_main_2 、/uav_control_main_3 、/uav1_mavros 、/uav2_mavros 、/uav3_mavros、/laser_filter1、/laser_filter2、/laser_filter3、/laser_to_pointcloud_1、/laser_to_pointcloud_2、/laser_to_pointcloud_3、/octomap_server_1、/octomap_server_2、/octomap_server_3 等节点。

  • /gazebo 节点为gazebo ROS驱动节点，为ego-swarm提供深度点云数据，来进行建图

  • /motion_goal_swarm 节点为rviz指点节点，为每台无人机提供规划的目标点

  • /uav1_ego_planner_node 节点为ego-swarm的规划节点，根据/octomap_server_1输入的点云数据建立膨胀地图，并规划出可行路径，输出/broadcast_bspline话题到/uav2_ego_planner_node、/uav3_ego_planner_node节点，这两个节点根据1号无人机的轨迹再生成安全的轨迹，避免与1号无人机发生碰撞。同时输出/uav1/planning/bspline话题，将B样条曲线给到/uav1_traj_server_for_prometheus节点

  • /uav1_traj_server_for_prometheus 节点为轨迹生成节点，根据B样条（bspline）生成一系列轨迹点，然后转化为/uav1/prometheus/command话题控制1号无人机飞行

  • /uav2_ego_planner_node 节点为ego-swarm的规划节点，根据/octomap_server_2输入的点云数据建立膨胀地图，并规划出可行路径，输出/broadcast_bspline话题到/uav2_ego_planner_node、/uav1_ego_planner_node节点，这两个节点根据2号无人机的轨迹再生成安全的轨迹，避免与2号无人机发生碰撞。同时输出/uav2/planning/bspline话题，将B样条曲线给到/uav2_traj_server_for_prometheus节点

  • /uav2_traj_server_for_prometheus 节点为轨迹生成节点，根据B样条（bspline）生成一系列轨迹点，然后转化为/uav2/prometheus/command话题控制2号无人机飞行

  • /uav3_ego_planner_node 节点为ego-swarm的规划节点，根据/octomap_server_3输入的点云数据建立膨胀地图，并规划出可行路径，输出/broadcast_bspline话题到/uav1_ego_planner_node、/uav2_ego_planner_node节点，这两个节点根据3号无人机的轨迹再生成安全的轨迹，避免与3号无人机发生碰撞。同时输出/uav3/planning/bspline话题，将B样条曲线给到/uav3_traj_server_for_prometheus节点

  • /uav3_traj_server_for_prometheus 节点为轨迹生成节点，根据B样条（bspline）生成一系列轨迹点，然后转化为/uav3/prometheus/command话题控制2号无人机飞行

  • /joy_node 节点为遥控器ROS驱动节点，用以获取遥控器数据,将遥控器数据传输给/uav_control_main_1、/uav_control_main_2、/uav_control_main_3节点。

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

  • /uav2/mavros 节点为飞控ROS驱动节点，与2号无人机飞控进行数据交互。在仿真中，该驱动节点与模拟飞控进行数据交互。

  • /uav3/mavros 节点为飞控ROS驱动节点，与3号无人机飞控进行数据交互。在仿真中，该驱动节点与模拟飞控进行数据交互。

  • /laser_filter1 节点为ROS感知模块节点，它为1号无人机提供了一系列滤波器，用于处理来自激光雷达传感器的数据。这些滤波器可以帮助你消除噪声、平滑扫描或者根据需要进行其他定制化操作。为/laser_to_pointcloud_1节点提供滤波后雷达数据

  • /laser_filter2 节点为ROS感知模块节点，它为2号无人机提供了一系列滤波器，用于处理来自激光雷达传感器的数据。这些滤波器可以帮助你消除噪声、平滑扫描或者根据需要进行其他定制化操作。为/laser_to_pointcloud_2节点提供滤波后雷达数据

  • /laser_filter3 节点为ROS感知模块节点，它为1号无人机提供了一系列滤波器，用于处理来自激光雷达传感器的数据。这些滤波器可以帮助你消除噪声、平滑扫描或者根据需要进行其他定制化操作。为/laser_to_pointcloud_3节点提供滤波后雷达数据

  • laser_to_pointcloud_1 节点将1号无人机的激光雷达数据转换为sensor_msgs::PointCloud2 点云格式，为/octomap_server建图提供数据

  • laser_to_pointcloud_2 节点将2号无人机的激光雷达数据转换为sensor_msgs::PointCloud2 点云格式，为/octomap_server建图提供数据

  • laser_to_pointcloud_3 节点将3号无人机的激光雷达数据转换为sensor_msgs::PointCloud2 点云格式，为/octomap_server建图提供数据

  • /octomap_server_1 节点根据获取1号无人机的点云数据并进行建图

  • /octomap_server_2 节点根据获取2号无人机的点云数据并进行建图

  • /octomap_server_3 节点根据获取3号无人机的点云数据并进行建图

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

  • /uav*/prometheus/command： 无人机控制接口，对应的消息为prometheus_msgs/UAVCommand

  • /uav*/prometheus/state： 无人机状态，对应的消息为prometheus_msgs/UAVState

  • /uav*/prometheus/fake_rc_in： 仿真模拟PX4遥控器数据

  • /uav*/scan： 二维激光雷达点云话题输入

  • /uav*/scan_filtered : 过滤后的点云数据

  • /uav*/prometheus/scan_point_cloud : octomap生成的全局点云数据

  • /broadcast_bspline： 广播的B样条

  • /uav*/planning/bspline： 发布的B样条

代码讲解

脚本位置: /home/amov/Prometheus/Scripts/simulation/ego_planner/ego_planner_p450_2dlidar_swarm.sh,内容如下：

# 脚本描述: 
gnome-terminal --window -e 'bash -c "roscore; exec bash"' \
--tab -e 'bash -c "sleep 3; roslaunch prometheus_gazebo sitl_p450_2dlidar_swarm.launch; exec bash"' \
--tab -e 'bash -c "sleep 5; roslaunch prometheus_uav_control uav_control_main_intdoor_swarm.launch; exec bash"' \
--tab -e 'bash -c "sleep 6; roslaunch prometheus_gazebo filter_lidar_swarm.launch; exec bash"' \
--tab -e 'bash -c "sleep 7; roslaunch prometheus_gazebo scan_to_octomap_swarm.launch; exec bash"' \
--tab -e 'bash -c "sleep 8; roslaunch ego_planner sitl_ego_planner_basic_octomap_lidar_swarm.launch; exec bash"' \

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

  1. sitl_p450_2dlidar_swarm.launch
  2. uav_control_main_intdoor_swarm.launch
  3. filter_lidar_swarm.launch
  4. scan_to_octomap_swarm.launch
  5. sitl_ego_planner_basic_octomap_lidar_swarm.launch

依次查看每个文件

  1. sitl_p450_2dlidar_swarm.launch
<launch>  
	<!-- Gazebo configs -->
    <arg name="gazebo_enable" default="true"/>
	<arg name="world" default="$(find prometheus_gazebo)/gazebo_worlds/planning_worlds/planning_easy_swarm.world"/>
    <!-- 启动Gazebo -->
    <group if="$(arg gazebo_enable)">
        <include file="$(find gazebo_ros)/launch/empty_world.launch">
            <arg name="world_name" value="$(arg world)"/>
			<!-- 设置为false，使用系统时间 -->
			<arg name="use_sim_time" value="true"/>
			<arg name="gui" value="true"/>
        </include>
    </group>

    <!-- Rviz config -->
    <arg name="rviz_enable" default="true"/>
    <arg name="rivz_config" default="$(find prometheus_gazebo)/launch_uav_with_sensor/2dlidar_mapping_swarm.rviz"/>
	<!-- 启动Rviz-->
	<group if="$(arg rviz_enable)">
        <node type="rviz" name="rviz" pkg="rviz" args="-d $(arg rivz_config)"/>
    </group>
	<arg name="vehicle" default="p450_2Dlidar"/>

    <!--无人机编号-->
    <arg name="uav1_id" default="1"/>
    <arg name="uav2_id" default="2"/>
    <arg name="uav3_id" default="3"/>
	<!-- 无人机初始位置 -->
	<arg name="uav1_init_x"   default="1"/>
    <arg name="uav1_init_y"   default="1"/>
	<arg name="uav1_init_yaw" default="0.0"/>
    <arg name="uav2_init_x"   default="0"/>
    <arg name="uav2_init_y"   default="0"/>
	<arg name="uav2_init_yaw" default="0.0"/>
    <arg name="uav3_init_x"   default="1"/>
    <arg name="uav3_init_y"   default="-1"/>
	<arg name="uav3_init_yaw" default="0.0"/>

	<!-- 1号无人机 -->
	<include file="$(find prometheus_gazebo)/launch_basic/sitl_px4_indoor.launch">
		<arg name="uav_id" value="$(arg uav1_id)"/>
		<arg name="vehicle" value="$(arg vehicle)"/>
		<arg name="uav_init_x" value="$(arg uav1_init_x)"/>
		<arg name="uav_init_y" value="$(arg uav1_init_y)"/>
		<arg name="uav_init_z" value="0.1"/>
		<arg name="uav_init_yaw" value="$(arg uav1_init_yaw)"/>
	</include>
	<!-- 2号无人机 -->
	<include file="$(find prometheus_gazebo)/launch_basic/sitl_px4_indoor.launch">
		<arg name="uav_id" value="$(arg uav2_id)"/>
		<arg name="vehicle" value="$(arg vehicle)"/>
		<arg name="uav_init_x" value="$(arg uav2_init_x)"/>
		<arg name="uav_init_y" value="$(arg uav2_init_y)"/>
		<arg name="uav_init_z" value="0.1"/>
		<arg name="uav_init_yaw" value="$(arg uav2_init_yaw)"/>
	</include>
	<!-- 3号无人机 -->
	<include file="$(find prometheus_gazebo)/launch_basic/sitl_px4_indoor.launch">
		<arg name="uav_id" value="$(arg uav3_id)"/>
		<arg name="vehicle" value="$(arg vehicle)"/>
		<arg name="uav_init_x" value="$(arg uav3_init_x)"/>
		<arg name="uav_init_y" value="$(arg uav3_init_y)"/>
		<arg name="uav_init_z" value="0.1"/>
		<arg name="uav_init_yaw" value="$(arg uav3_init_yaw)"/>
	</include>

	<!-- TF for world and map-->
	<!-- 如果模型中使用了D435i，将下列TF转换打开 -->
     <node pkg="tf" type="static_transform_publisher" name="tf_camera_d435i_$(arg uav1_id)"
        args="0 0 0 0 0 0 /uav$(arg uav1_id)/camera_link /uav$(arg uav1_id)/d435i_link 100"/>

    <node pkg="tf" type="static_transform_publisher" name="tf_camera_d435i_$(arg uav2_id)"
        args="0 0 0 0 0 0 /uav$(arg uav2_id)/camera_link /uav$(arg uav2_id)/d435i_link 100"/>

    <node pkg="tf" type="static_transform_publisher" name="tf_camera_d435i_$(arg uav3_id)"
        args="0 0 0 0 0 0 /uav$(arg uav3_id)/camera_link /uav$(arg uav3_id)/d435i_link 100"/>
</launch>

启动prometheus_gazebo下gazebo环境，带有仿真二维激光雷达的无人机模型，以及rviz显示。

  1. uav_control_main_intdoor_swarm.launch
<launch>
    <!--arg name="uav_id" default="1"/-->
    <arg name="uav1_id" default="1"/>
    <arg name="uav2_id" default="2"/>
    <arg name="uav3_id" default="3"/>

    <arg name="sim_mode" default="true"/>
    <arg name="flag_printf" default="true"/>
    <!-- 如果是matlab配置为ATT_CTRL_MODE，则此处要设置为True-->
    <arg name="control/enable_external_control" default="false"/> 
    <arg name="launch_prefix" default="" />
    
    <arg name="agent_num" default="3"/>
    
    <!-- 启动uav_control_main -->
    <!-- 1号飞机 -->
	<node pkg="prometheus_uav_control" type="uav_control_main" name="uav_control_main_$(arg uav1_id)" output="screen">
		<param name="uav_id" value="$(arg uav1_id)" />
        <param name="sim_mode" value="$(arg sim_mode)" />
		<param name="flag_printf" value="$(arg flag_printf)" />
	<param name="control/enable_external_control" value="$(arg control/enable_external_control)" />
		<rosparam command="load" file="$(find prometheus_uav_control)/launch/uav_control_indoor.yaml" />
		<param name="control/Takeoff_height" value="2" />
        <rosparam command="load" file="$(find prometheus_uav_control)/launch/sensor_tf_offset.yaml" />
	</node>

    <!-- 2号飞机 -->
	<node pkg="prometheus_uav_control" type="uav_control_main" name="uav_control_main_$(arg uav2_id)" output="screen">
		<param name="uav_id" value="$(arg uav2_id)" />
        <param name="sim_mode" value="$(arg sim_mode)" />
		<param name="flag_printf" value="$(arg flag_printf)" />
	<param name="control/enable_external_control" value="$(arg control/enable_external_control)" />
		<rosparam command="load" file="$(find prometheus_uav_control)/launch/uav_control_indoor.yaml" />
		<param name="control/Takeoff_height" value="2" />
        <rosparam command="load" file="$(find prometheus_uav_control)/launch/sensor_tf_offset.yaml" />
	</node>

    <!-- 3号飞机 -->
	<node pkg="prometheus_uav_control" type="uav_control_main" name="uav_control_main_$(arg uav3_id)" output="screen">
		<param name="uav_id" value="$(arg uav3_id)" />
        <param name="sim_mode" value="$(arg sim_mode)" />
		<param name="flag_printf" value="$(arg flag_printf)" />
	<param name="control/enable_external_control" value="$(arg control/enable_external_control)" />
		<rosparam command="load" file="$(find prometheus_uav_control)/launch/uav_control_indoor.yaml" />
		<param name="control/Takeoff_height" value="2" />
        <rosparam command="load" file="$(find prometheus_uav_control)/launch/sensor_tf_offset.yaml" />
	</node>
       
    <!-- 启动虚拟摇杆驱动 -->
    <arg name="joy_enable" default="true"/>
    <group if="$(arg joy_enable)">
        <node pkg="prometheus_uav_control" type="joy_node" name="joy_node" output="screen">
            <param name="agent_num" value="$(arg agent_num)"/>
        </node>
    </group>
</launch>

启动prometheus_uav_control，将传感器通过tf变换，将数据转到全局坐标系下，Prometheus下控制，为后面路径规划发送控制指令做准备

  1. filter_lidar_swarm.launch
<launch>
  <arg name="uav1_id" default="1"/>
  <arg name="uav2_id" default="2"/>
  <arg name="uav3_id" default="3"/>
  
  <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter_$(arg uav1_id)">
    <rosparam command="load" file="$(find prometheus_gazebo)/config/filter_lidar.yaml"/>
    <remap from="/scan" to="/uav$(arg uav1_id)/scan"/>
    <remap from="/scan_filtered" to="/uav$(arg uav1_id)/scan_filtered"/>
  </node>
  
    <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter_$(arg uav2_id)">
    <rosparam command="load" file="$(find prometheus_gazebo)/config/filter_lidar_2.yaml"/>
    <remap from="/scan" to="/uav$(arg uav2_id)/scan"/>
    <remap from="/scan_filtered" to="/uav$(arg uav2_id)/scan_filtered"/>
  </node>
  
    <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter_$(arg uav3_id)">
    <rosparam command="load" file="$(find prometheus_gazebo)/config/filter_lidar_3.yaml"/>
    <remap from="/scan" to="/uav$(arg uav3_id)/scan"/>
    <remap from="/scan_filtered" to="/uav$(arg uav3_id)/scan_filtered"/>
  </node>
  
</launch>

对每台无人机上二维激光雷达的数据进行滤波处理，屏蔽扫描到的无人机

  1. scan_to_octomap_swarm.launch
<launch>
  <arg name="uav1_id" default="1"/>
  <arg name="uav2_id" default="2"/>
  <arg name="uav3_id" default="3"/>
	<!-- run the laser_to_pointcloud -->
  <node pkg="plan_env" type="laser_to_pointcloud" name="laser_to_pointcloud_$(arg uav1_id)" output="screen">
      <param name="uav_id" value="$(arg uav1_id)" />
  </node>
  <node pkg="plan_env" type="laser_to_pointcloud" name="laser_to_pointcloud_$(arg uav2_id)" output="screen">
      <param name="uav_id" value="$(arg uav2_id)" />
  </node>
  <node pkg="plan_env" type="laser_to_pointcloud" name="laser_to_pointcloud_$(arg uav3_id)" output="screen">
      <param name="uav_id" value="$(arg uav3_id)" />
  </node>

  <!-- 启动octomap建图 -->
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server_$(arg uav1_id)" output="screen">
      <remap from="/octomap_point_cloud_centers" to="/uav$(arg uav1_id)/octomap_point_cloud_centers"/>
      <remap from="/octomap_full" to="/uav$(arg uav1_id)/octomap_full"/>
      <remap from="/octomap_binary" to="/uav$(arg uav1_id)/octomap_binary"/>
      
      <!-- 地图分辨率 -->
      <param name="resolution" value="0.1" />
      <!-- 发布地图的坐标系 -->
      <param name="frame_id" type="string" value="world" />
      <!-- 传感器最大感知范围 (speedup!) -->
      <param name="sensor_model/max_range" value="10" />
      <!-- 局部点云话题输入 -->
      <remap from="cloud_in" to="/uav$(arg uav1_id)/prometheus/scan_point_cloud" />
      <!-- 直通滤波的 Z 轴范围，保留 [-1.0, 10.0] 范围内的点 -->
      <!-- <param name = "pointcloud_max_z" value = "1.7" />
      <param name = "pointcloud_min_z" value = "1.4" /> -->
      <!-- 机器人坐标系 base_link，滤除地面需要该 frame -->
      <param name = "base_frame_id" type = "string" value = "world" />
      <!-- filter ground plane, distance value should be big! 项目并不需要滤除地面 -->
      <param name = "filter_ground" type = "bool" value = "true" />
      <param name = "ground_filter/distance" type = "double" value = "0.5" />
      <param name = "ground_filter/angle" type = "double" value = "0.7853" />
      <param name = "ground_filter/plane_distance" type = "double" value = "0.5" />

  </node>
  
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server_$(arg uav2_id)" output="screen">
      <remap from="/octomap_point_cloud_centers" to="/uav$(arg uav2_id)/octomap_point_cloud_centers"/>
      <remap from="/octomap_full" to="/uav$(arg uav2_id)/octomap_full"/>
      <remap from="/octomap_binary" to="/uav$(arg uav2_id)/octomap_binary"/>
      
      <!-- 地图分辨率 -->
      <param name="resolution" value="0.1" />
      <!-- 发布地图的坐标系 -->
      <param name="frame_id" type="string" value="world" />
      <!-- 传感器最大感知范围 (speedup!) -->
      <param name="sensor_model/max_range" value="10" />
      <!-- 局部点云话题输入 -->
      <remap from="cloud_in" to="/uav$(arg uav2_id)/prometheus/scan_point_cloud" />
      <!-- 直通滤波的 Z 轴范围，保留 [-1.0, 10.0] 范围内的点 -->
      <!-- <param name = "pointcloud_max_z" value = "1.7" />
      <param name = "pointcloud_min_z" value = "1.4" /> -->
      <!-- 机器人坐标系 base_link，滤除地面需要该 frame -->
      <param name = "base_frame_id" type = "string" value = "world" />
      <!-- filter ground plane, distance value should be big! 项目并不需要滤除地面 -->
      <param name = "filter_ground" type = "bool" value = "true" />
      <param name = "ground_filter/distance" type = "double" value = "0.5" />
      <param name = "ground_filter/angle" type = "double" value = "0.7853" />
      <param name = "ground_filter/plane_distance" type = "double" value = "0.5" />

  </node>
  
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server_$(arg uav3_id)" output="screen">
      <remap from="/octomap_point_cloud_centers" to="/uav$(arg uav3_id)/octomap_point_cloud_centers"/>
      <remap from="/octomap_full" to="/uav$(arg uav3_id)/octomap_full"/>
      <remap from="/octomap_binary" to="/uav$(arg uav3_id)/octomap_binary"/>
      
      <!-- 地图分辨率 -->
      <param name="resolution" value="0.1" />
      <!-- 发布地图的坐标系 -->
      <param name="frame_id" type="string" value="world" />
      <!-- 传感器最大感知范围 (speedup!) -->
      <param name="sensor_model/max_range" value="10" />
      <!-- 局部点云话题输入 -->
      <remap from="cloud_in" to="/uav$(arg uav3_id)/prometheus/scan_point_cloud" />
      <!-- 直通滤波的 Z 轴范围，保留 [-1.0, 10.0] 范围内的点 -->
      <!-- <param name = "pointcloud_max_z" value = "1.7" />
      <param name = "pointcloud_min_z" value = "1.4" /> -->
      <!-- 机器人坐标系 base_link，滤除地面需要该 frame -->
      <param name = "base_frame_id" type = "string" value = "world" />
      <!-- filter ground plane, distance value should be big! 项目并不需要滤除地面 -->
      <param name = "filter_ground" type = "bool" value = "true" />
      <param name = "ground_filter/distance" type = "double" value = "0.5" />
      <param name = "ground_filter/angle" type = "double" value = "0.7853" />
      <param name = "ground_filter/plane_distance" type = "double" value = "0.5" />

  </node>
</launch>

将雷达数据类型从"sensor_msgs/LaserScan" 类型转到 “sensor_msgs/PointCloud2”，同时启动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数据转换为实际可用的地图。

  1. sitl_ego_planner_basic_octomap_lidar_swarm.launch
<launch>
  <arg name="uav1_id" default="1"/>
  <arg name="uav2_id" default="2"/>
  <arg name="uav3_id" default="3"/>
   
  <!-- ego_planner 仿真 -->
  <include file="$(find ego_planner)/launch_for_prometheus/advanced_param_swarm.xml">
    <!-- 无人机编号 -->
    <arg name="uav1_id" value="$(arg uav1_id)"/>
    <arg name="uav2_id" value="$(arg uav2_id)"/>
    <arg name="uav3_id" value="$(arg uav3_id)"/>
    <!-- 地图尺寸 -->
    <arg name="map_size_x_" value="50.0"/>
    <arg name="map_size_y_" value="50.0"/>
    <arg name="map_size_z_" value="4.0"/>
    <arg name="map_origin_x_" value="-25.0"/>
    <arg name="map_origin_y_" value="-25.0"/>
    <!-- 可以通过限制ground_height和map_size_z来使得飞机在特定高度飞行 -->
    <arg name="ground_height" value="0.5"/>
    <!-- 虚拟天花板高度要小于等于ground_height+z_size，否则重置该高度 -->
    <!-- 里程计话题 -->
    <arg name="odometry_topic" value="/mavros/local_position/odom"/>
    <!-- 相机及深度信息仿真中没有使用，此处可忽略(真机中使用) -->
    <arg name="camera_pose_topic" value="/depth/no_set"/>
    <arg name="depth1_topic" value="/uav$(arg uav1_id)/depth/image_rect_raw"/>
    <arg name="depth2_topic" value="/uav$(arg uav2_id)/depth/image_rect_raw"/>
    <arg name="depth3_topic" value="/uav$(arg uav3_id)/depth/image_rect_raw"/>
    
    <arg name="cx" value="321.04638671875"/>
    <arg name="cy" value="243.44969177246094"/>
    <arg name="fx" value="387.229248046875"/>
    <arg name="fy" value="387.229248046875"/>
    <!-- 点云信息，仿真中由map_generator产生 -->
    <arg name="cloud1_topic" value="/uav$(arg uav1_id)/octomap_point_cloud_centers"/>
    <arg name="cloud2_topic" value="/uav$(arg uav2_id)/octomap_point_cloud_centers"/>
    <arg name="cloud3_topic" value="/uav$(arg uav3_id)/octomap_point_cloud_centers"/>
       
    <!-- scan -->
    <arg name="scan_topic" value="no_scan"/>
    <!-- 最大速度及加速度 -->
    <arg name="max_vel" value="0.8" />
    <arg name="max_acc" value="3" />
    <!-- 规划的范围，一般设置为感知范围的1.5倍 7.5-->
    <arg name="planning_horizon" value="7.5" /> 
    <!-- ？ -->
    <arg name="use_distinctive_trajs" value="true" />
    <!-- 1: use 2D Nav Goal to select goal  -->
    <!-- 2: use global waypoints below  -->
    <!-- 单机建议直接使用rviz指定目标点,多机情况请预设目标点 -->
    <arg name="flight_type" value="1" />
    <!-- global waypoints -->
    <!-- It generates a piecewise min-snap traj passing all waypoints -->
    <arg name="point_num" value="1" />
    <arg name="point0_x" value="7.0" />
    <arg name="point0_y" value="0.0" />
    <arg name="point0_z" value="1.5" />
    
    <!-- 仿真 -->
    <arg name="realworld_experiment" value="true" />
  </include>

  <!-- trajectory server：由B样条生成轨迹 -->
  <!-- 偏航角初始值，重要参数 -->
  <arg name="yaw_init" default="0.0"/>
  <node pkg="ego_planner" name="uav$(arg uav1_id)_traj_server_for_prometheus" type="traj_server_for_prometheus" output="screen">
    <param name="uav_id" value="$(arg uav1_id)"/>
    <param name="control_flag" value="0"/>
    <param name="traj_server/time_forward" value="1.0"/>
    <param name="traj_server/last_yaw" value="$(arg yaw_init)"/>
  </node>
  
  <node pkg="ego_planner" name="uav$(arg uav2_id)_traj_server_for_prometheus" type="traj_server_for_prometheus" output="screen">
    <param name="uav_id" value="$(arg uav2_id)"/>
    <param name="control_flag" value="0"/>
    <param name="traj_server/time_forward" value="1.0"/>
    <param name="traj_server/last_yaw" value="$(arg yaw_init)"/>
  </node>
  
  <node pkg="ego_planner" name="uav$(arg uav3_id)_traj_server_for_prometheus" type="traj_server_for_prometheus" output="screen">
    <param name="uav_id" value="$(arg uav3_id)"/>
    <param name="control_flag" value="0"/>
    <param name="traj_server/time_forward" value="1.0"/>
    <param name="traj_server/last_yaw" value="$(arg yaw_init)"/>
  </node>
  <node pkg="ego_planner" name="motion_goal_swarm" type="motion_goal_swarm" output="screen"/>

</launch>

上面 launch文件在ego_planner包下启动ego_planner 仿真，并由B样条生成轨迹，发送控制指令给每台无人机，控制飞行。其中包含两部分/uav*_ego_planner_node和/uav*_traj_server_for_prometheus:

  • /uav*_ego_planner_node进行ego_planner规划，相关源代码参见ego_planner_node.cpp ，ego_replan_fsm.cpp，planner_manager.cpp，文件路径: ~/Prometheus/Modules/ego_planner_swarm/plan_manage/src/。
  • /uav*_traj_server_for_prometheus订阅ego_planner规划结果，由B样条（bspline）生成轨迹，转化为command命令控制无人机飞行。其源代码为traj_server_for_prometheus.cpp，文件路径: ~/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/**

操作演示视频