无人机运动规划4：ego-swarm无人机群运动规划

配置

启动python脚本生成多机launch文件

cd ~/XTDrone/coordination/launch_generator
python3 generator.py

将生成出来的launch文件复制到PX4固件的launch文件夹

cp ~/XTDrone/coordination/launch_generator/multi_vehicle.launch ~/PX4_Firmware/launch/

将XTDrone/stil_config/worlds中的ego_swarm.world复制到PX4_Firmware/Tools/sitl_gazebo/worlds中

cp ~/XTDrone/sitl_config/worlds/ego_swarm.world ~/PX4_Firmware/Tools/sitl_gazebo/worlds

并且更改launch文件，同时将launch文件中的iris_stereo_camera都替换为iris_realsense_camera

<!-- arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor2.world"/ -->
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/ego_swarm.world"/>

修改run_in_xtdrone.launch文件

<!-- number of moving objects -->
<arg name="obj_num" value="4" />

<!-- maximum velocity and acceleration the drone will reach -->
<arg name="max_vel" value="1.0" />
<arg name="max_acc" value="0.2" />   #根据需要调整

<!-- 1: use 2D Nav Goal to select goal  -->
<!-- 2: use global waypoints below  -->
<arg name="flight_type" value="1" />

修改advanced_param_xtdrone.xml

<!-- main node -->
<!-- <node pkg="ego_planner" name="ego_planner_node" type="ego_planner_node" output="screen" launch-prefix="valgrind"> -->
<node pkg="ego_planner" name="iris_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">
  
  <remap from="~odom_world" to="/iris_$(arg drone_id)/$(arg odometry_topic)"/>
  <remap from="~planning/bspline" to = "/xtdrone/iris_$(arg drone_id)/planning/bspline"/>
  <remap from="~planning/data_display" to = "/xtdrone/iris_$(arg drone_id)/planning/data_display"/>
  <remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/>
  <remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>
  <remap from="/move_base_simple/goal" to = "/iris_$(arg drone_id)/move_base_simple/goal"/>
  <remap from="~grid_map/odom" to="/xtdrone/iris_$(arg drone_id)/$(arg odometry_topic)"/>
  <remap from="~grid_map/cloud" to="/iris_$(arg drone_id)/$(arg cloud_topic)"/>
  <remap from="~grid_map/pose"  to = "/iris_$(arg drone_id)/$(arg camera_pose_topic)"/> 
  <remap from="~grid_map/depth" to = "/iris_$(arg drone_id)/$(arg depth_topic)"/>

原版

  <node pkg="ego_planner" name="iris_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">

   <!-- ~odom_world是vins_estimator/odometry -->
    <remap from="~odom_world" to="/$(arg odometry_topic)"/>
    <remap from="~planning/bspline" to = "/xtdrone/iris_$(arg drone_id)/planning/bspline"/>
    <remap from="~planning/data_display" to = "/xtdrone/iris_$(arg drone_id)/planning/data_display"/>
    <remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/>
    <remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>
    
    <!-- ~odom_world也是vins_estimator/odometry -->
    <remap from="~grid_map/odom" to="/xtdrone/iris_$(arg drone_id)/$(arg odometry_topic)"/>
    <remap from="~grid_map/cloud" to="/iris_$(arg drone_id)/$(arg cloud_topic)"/>

    <!--  /grid_map/pose是iris_0/camera_pose（由vins_estimator/camera_pose -> vinsion_pose/pose -> iris_0/camera_pose）
      remap:1 重命名一个已经存在的主题 2将别人发布的主题映射到自己订阅的主题上，这个类型应该属于2，grid_map/pose需要 /iris_0/camera_pose的值-->
    <remap from="~grid_map/pose"   to = "/iris_$(arg drone_id)/$(arg camera_pose_topic)"/> 
    <remap from="~grid_map/depth" to = "/iris_$(arg drone_id)/$(arg depth_topic)"/>

修改multi_vins_transfer.py中的bias的值根据iris的初始位置相应改变

bias = np.array([[0,3,0],[3,3,0],[6,3,0]])

修改ego_swarm_goal.sh，确定对应飞机目标点和个数

#!/bin/bash
while(true)
do
    python2.7 ego_swarm_goal.py iris 0 9 9 0.7&
    python2.7 ego_swarm_goal.py iris 1 9 9 0.7&
    python2.7 ego_swarm_goal.py iris 2 9 9 0.7&
    python2.7 ego_swarm_goal.py iris 3 6 0 0.7
done

启动

启动仿真

cd ~/PX4_Firmware
roslaunch px4 multi_vehicle.launch

启动多无人机的VINS-Fusion，同时将VINS-Fusion发布的Odometry类型的话题对应转为PX4所需的话题
multi_vins_transfer.py中的bias的值根据iris的初始位置相应改变

cd ~/catkin_ws
roslaunch vins xtdrone_run_vio.launch

cd ~/XTDrone/sensing/slam/vio
python2.7 multi_vins_transfer.py iris 3   # 4是iris的数量

建立多机通信，然后键盘控制起飞后悬停，关闭键盘控制

cd ~/XTDrone/communication
bash multi_vehicle_communication.sh

cd ~/XTDrone/control/keyboard
python2.7 multirotor_keyboard_control.py iris 3 vel

cd ~/XTDrone/control/keyboard
python2.7 hover.py iris 3 vel

转换相机位姿的坐标系方向

cd ~/XTDrone/motion_planning/3d
python2.7 ego_swarm_transfer.py iris 3  # 4是iris的数量

启动rviz

rviz -d ego_swarm_rviz.rviz

启动ego_planner_swarm

roslaunch ego_planner multi_uav.launch

发布目的地（代码中的4个数字依次代表无人机编号、x、y、z）

cd /home/zj/XTDrone/motion_planning/3d
bash ego_swarm_goal.sh   #统一发布
python2.7 ego_swarm_goal.py iris 0 -7 -9 0.7 #单独发布