无人机路径规划3：ego-planner三维运动规划实现

XTDrone实现ego-planner三维运动规划

编译ego-palnner

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

• 遇到报错，安装各种依赖库，把显示未安装的全部安装一遍

apt search occupancy

• eigen3报错

eigen/double重载出错，经探索是版本问题，卸载eigen3

sudo updatedb
locate eigen3
#手动卸载
sudo rm -rf /usr/local/include/eigen3

重装eigen3.3.7

mkdir build
cd build
cmake ..
sudo make
sudo make install
#复制Eigen库到 /usr/local/include 中 （这一步很重要，一定要执行，否则后面编译一些程序，会提示没有Eigen库）
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include

sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include 
sudo cp -r /usr/local/include/eigen3 /usr/include/eigen3

• 解决ceres和eigen版本冲突问题

https://zhuanlan.zhihu.com/p/149775218?from_voters_page=true

XTDrone仿真实现

启动仿真程序，注意launch文件中使用的是iris_stereo_camera.sdf

cd PX4_Firmware
roslaunch px4 indoor1.launch

cd catkin_ws
bash scripts/xtdrone_run_vio.sh

由于VINS-Fusion发布的是Odometry类型的话题，我们要将其对应转为PX4所需的话题

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

启动rtabmap仿真。注意把仿真launch文件的iris_stereo_camera换成iris_realsense_camera，这样才能有深度图数据。

roslaunch vins rtabmap_vins.launch

建立通信

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

键盘控制起飞（将飞机用键盘控制起飞后悬停，关闭键盘控制）

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

键盘起飞改为自动悬停的脚本

cd /home/tju/catkin_ws/src/offboard/scripts
python hover.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 

效果展示

建立自己的仿真环境

修改empty.world文件，其中注意引用同一个sdf模型时，要不同命名否则报错

编写plan.sh脚本一起启动

#!/bin/bash
gnome-terminal -x bash -c  "cd ~/PX4_Firmware; roslaunch px4 indoor1.launch"
sleep 5
gnome-terminal -x bash -c  "cd ~/catkin_ws; bash scripts/xtdrone_run_vio.sh"
sleep 5
gnome-terminal -x bash -c  "cd ~/XTDrone/sensing/slam/vio; python vins_transfer.py iris 0"
sleep 2
gnome-terminal -x bash -c  "roslaunch vins rtabmap_vins.launch "
sleep 5
gnome-terminal -x bash -c  "cd ~/XTDrone/communication; python multirotor_communication.py iris 0"
sleep 2
gnome-terminal -x bash -c  "cd /home/tju/catkin_ws/src/offboard/scripts; python hover.py iris 1 vel"
sleep 2
gnome-terminal -x bash -c  "cd ~/XTDrone/motion_planning/3d; python ego_transfer.py iris 0"
sleep 2
gnome-terminal -x bash -c  "cd ~/XTDrone/motion_planning/3d; rviz -d ego_rviz.rviz"
sleep 5
gnome-terminal -x bash -c  "roslaunch ego_planner single_uav.launch"
wait
exit 0

效果展示