仿真Demo-MID360雷达避障

效果展示

提示

运行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_panner_p600_mid360.sh
 ./ego_panner_p600_mid360.sh

检查终端运行是否正常

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

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

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

4. 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. 到达后再次规划
   • 按照步骤再次指点规划

节点运行图

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

• /gazebo节点为gazebo ROS驱动节点，为/laserMapping节点提供点云数据，可来进行建图、定位

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

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

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

/uav1_ego_planner_node节点是将雷达从倾斜数据转到world系下

• /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/mavros/state：无人机状态，对应的消息为mavros_msgs/State
• /uav1/prometheus/fake_rc_in：仿真模拟PX4遥控器数据
• /livox/lidar 仿真中Mid360雷达扫描出的雷达数据
• /broadcast_bspline:广播的B样条
• /uav1/planning/bspline:发布的B样条
• /uav1/mavros/local_position/odom: 无人机位置数据
• /uav1/livox/lidar:雷达转换后的点云数据

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

代码讲解

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

UAV_ID=1
gnome-terminal --window -e 'bash -c "roscore; exec bash"' \
--tab -e 'bash -c "sleep 3; roslaunch prometheus_gazebo sitl_outdoor_1uav_P600_mid360.launch uav1_id:='$UAV_ID' uav1_init_x:=-0.0 uav1_init_y:=-0.0; exec bash"' \
--tab -e 'bash -c "sleep 4; roslaunch prometheus_uav_control uav_control_main_outdoor_mid60.launch uav_id:='$UAV_ID'; exec bash"' \
--tab -e 'bash -c "sleep 4; roslaunch ego_planner sitl_ego_planner_mid360.launch; exec bash"' \

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

1. sitl_outdoor_1uav_P600_mid360.launch

2. uav_control_main_outdoor_mid60.launch

3. sitl_ego_planner_mid360.launch

依次查看

1.sitl_outdoor_1uav_P600_mid360.launch

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

2.uav_control_main_outdoor.launch

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

3.sitl_ego_planner_mid360.launch

<launch>
  <arg name="uav_id" default="1"/>

  <!-- ego_planner 仿真 -->
  <include file="$(find ego_planner)/launch_for_prometheus/advanced_param_p600.xml">
    <!-- 无人机编号 -->
    <arg name="uav_id" value="$(arg uav_id)"/>
    <!-- 地图尺寸 -->
    <arg name="map_size_x_" value="100.0"/>
    <arg name="map_size_y_" value="100.0"/>
    <arg name="map_size_z_" value="3.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="depth_topic" value="/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="cloud_topic" value="/uav$(arg uav_id)/octomap_point_cloud_centers"/-->
	<arg name="cloud_topic" value="/uav1/livox/lidar"/>
    <!-- scan -->
    <arg name="scan_topic" value="no_scan"/>
    <!-- 最大速度及加速度 -->
    <arg name="max_vel" value="0.8" />
    <arg name="max_acc" value="6" />
    <!-- 规划的范围，一般设置为感知范围的1.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="4" />
    <arg name="point0_x" value="-6.0" />
    <arg name="point0_y" value="-3.0" />
    <arg name="point0_z" value="1.5" />

    <arg name="point1_x" value="0.0" />
    <arg name="point1_y" value="3.0" />
    <arg name="point1_z" value="1.5" />

    <arg name="point2_x" value="3.0" />
    <arg name="point2_y" value="-3.0" />
    <arg name="point2_z" value="1.5" />

    <arg name="point3_x" value="6.0" />
    <arg name="point3_y" value="-7.0" />
    <arg name="point3_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 uav_id)_traj_server_for_prometheus" type="traj_server_for_prometheus" output="screen">
    <param name="uav_id" value="$(arg uav_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="plan_env" name="uav$(arg uav_id)mid360_point2_world" type="mid360_point2_world" output="screen"/>
  
</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/

同时启动/uav1mid360_point2_world节点，用于将mid360雷达的数据从lidar_link转到world下，用于ego膨胀点云的数据生成。

操作演示视频

注意

终端显示的红色字体

WARNING: disk usage in log directory [/home/amov/.ros/log] is over 1GB. It's recommended that you use the 'rosclean' command.

可通过如下指令消除： rosclean purge 如下图所示: