XTDrone——三维运动规划

语雀
先完成VINS-Fusion仿真教程启动仿真

一、编译ego_planner

cp -r  ~/XTDrone/motion_planning/3d/ego_planner ~/catkin_ws/src/
cd ~/catkin_ws/
catkin_make #或catkin build ego_planner

二、启动仿真

ego_planner需要输入深度图+相机位姿或是点云，这里以深度图+相机位姿的组合为例进行仿真，深度图来源于realsense_camera，(记得把indoor1.launch的iris_stereo_camera换成iris_realsense_camera)，相机位姿由VINS-Fusion计算得到。

打开仿真文件

cd PX4_Firmware/
roslaunch px4 indoor1.launch

运行vins-fusion

cd ~/catkin_ws
bash scripts/xtdrone_run_vio.sh

修改话题（融合视觉定位）

cd ~/XTDrone/sensing/slam/vio
python vins_transfer.py iris 0

建立通信

cd ~/XTDrone/communication
python multirotor_communication.py iris 0 

键盘控制起飞

cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 1 vel

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

cd ~/XTDrone/motion_planning/3d
python ego_transfer.py iris 0

启动rviz

cd ~/XTDrone/motion_planning/3d
rviz -d ego_rviz.rviz

启动ego_planner

roslaunch ego_planner single_uav.launch 

在~/catkin_ws/src/ego_planner/plan_manage/launch/single_uav.launch中，可以更改目的地

        <arg name="target_x" value="5.2"/>
        <arg name="target_y" value="-13"/>
        <arg name="target_z" value="1.0"/>

在~/catkin_ws/src/ego_planner/plan_manage/launch/run_in_xtdrone.launch中，可以选择多个waypoint

    <arg name="point_num" value="1" />

    <arg name="point0_x" value="$(arg target_x)" />
    <arg name="point0_y" value="$(arg target_y)" />
    <arg name="point0_z" value="$(arg target_z)" />

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

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

    <arg name="point3_x" value="0.0" />
    <arg name="point3_y" value="-15.0" />
    <arg name="point3_z" value="1.0" />

    <arg name="point4_x" value="-15.0" />
    <arg name="point4_y" value="0.0" />
    <arg name="point4_z" value="1.0" />

通过修改飞行类型，可以选择1，从而可以通过RVIZ中的2D Nav Goal来设置目的地。

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