仿真Demo - D435i视觉避障

效果展示

提示

运行Demo前请将该模块编译

编译代码如下：

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

仿真脚本启动流程

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

  2. 输入以下命令启动D435i 集群避障仿真demo

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

检查终端运行是否正常

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

image.png

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

image.png

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

image.png

  1. 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,然后在rviz界面点击鼠标左键选择目标点,同时按住鼠标右键往上拖动

image.png

3D Nav Goal 位置如上图所示

注意

如果不按住鼠标左键，在画面内向上划，提供Z的高度，那么Z的高度默认为0，则会出现到达不了目标点的情况！
  1. 当飞机到达目标点后可再次选择目标点进行规划

节点运行图

image.png

ego-swarm仿真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等节点。

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

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

  • /uav1_ego_planner_node 节点为ego-swarm的规划节点，根据输入的深度点云数据建立膨胀地图，并规划出可行路径，输出/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的规划节点，根据输入的深度点云数据建立膨胀地图，并规划出可行路径，输出/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的规划节点，根据输入的深度点云数据建立膨胀地图，并规划出可行路径，输出/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驱动节点，与飞控进行数据交互。在仿真中，该驱动节点与模拟飞控进行数据交互。

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

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

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

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

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

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

  • /uav*/camera/depth/image_raw： 深度点云话题输入

  • /broadcast_bspline： 广播的B样条

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

代码讲解

脚本位于~/Prometheus/Scripts/simulation/ego_planner/ego_planner_p450_d435i_depth_swarm.sh，内容如下：

# 脚本描述: 
gnome-terminal --window -e 'bash -c "roscore; exec bash"' \
--tab -e 'bash -c "sleep 3; roslaunch prometheus_gazebo sitl_p450_d435i_swarm.launch ; exec bash"' \
--tab -e 'bash -c "sleep 4; roslaunch prometheus_uav_control uav_control_main_intdoor_swarm.launch ; exec bash"' \
--tab -e 'bash -c "sleep 6; roslaunch ego_planner sitl_ego_planner_basic_octomap_d435i_swarm_depth.launch; exec bash"' \

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

2、uav_control_main_intdoor_swarm.launch

3、sitl_ego_planner_basic_octomap_d435i_swarm_depth.launch

依次查看

1、sitl_p450_d435i_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/visual_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_D435i"/>
  <!--无人机编号-->
  <arg name="uav1_id" default="1"/>
  <arg name="uav2_id" default="2"/>
  <arg name="uav3_id" default="3"/>
  <!-- 无人机初始位置 -->
  <!--arg name="uav1_init_x" default="-6"/>
  <arg name="uav1_init_y" default="2.5"/>
  <arg name="uav1_init_yaw" default="0.0"/>
  <arg name="uav2_init_x" default="-6"/>
  <arg name="uav2_init_y" default="0.5"/>
  <arg name="uav2_init_yaw" default="0.0"/>
  <arg name="uav3_init_x" default="-6"/>
  <arg name="uav3_init_y" default="-2.5"/>
  <arg name="uav3_init_yaw" default="0.0"/>
  <arg name="uav4_init_x" default="-6"/>
  <arg name="uav4_init_y" default="-5"/>
  <arg name="uav4_init_yaw" default="0.0"/-->
  <arg name="uav1_init_x" default="0"/>
  <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="0"/>
  <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.15"/>
    <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.15"/>
    <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.15"/>
    <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>

上面launch文件用于启动prometheus_gazebo下gazebo环境，带有仿真D435i的无人机模型，以及rviz显示。

2、uav_control_main_intdoor_swarm.launch

<launch>
  <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>

上面launch文件用于启动UAV控制节点，在Prometheus下控制，为后面路径规划发送控制指令做准备

3、sitl_ego_planner_basic_octomap_d435i_swarm_depth.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)/camera/depth/image_raw"/>
    <arg name="depth2_topic" value="/uav$(arg uav2_id)/camera/depth/image_raw"/>
    <arg name="depth3_topic" value="/uav$(arg uav3_id)/camera/depth/image_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=""/>
    <arg name="cloud2_topic" value=""/>
    <arg name="cloud3_topic" value=""/>
    
    <!-- scan -->
    <arg name="scan_topic" value="no_scan"/>
    <!-- 最大速度及加速度 -->
    <!--arg name="max_vel" value="0.8" /-->
    <!--arg name="max_acc" value="6" /-->
    <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/**

操作演示视频