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

dueen1123

已于 2024-05-29 19:22:30 修改

阅读量9.2k

点赞数 18

文章标签：自动驾驶人工智能机器学习

于 2022-03-28 14:18:24 首次发布

本文链接：https://blog.csdn.net/dueen1123/article/details/123790583

版权

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

请添加图片描述
效果展示
hover.py 自动悬停脚本

# encoding = utf-8
from time import sleep
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
import tty, termios
from std_msgs.msg import String


MAX_LINEAR = 20
MAX_ANG_VEL = 3
LINEAR_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.01

cmd_vel_mask = False
ctrl_leader = False

if __name__=="__main__":

    settings = termios.tcgetattr(sys.stdin)

    multirotor_type = sys.argv[1]
    multirotor_num = int(sys.argv[2])
    control_type = sys.argv[3]
    
    if multirotor_num == 18:
        formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond']
    elif multirotor_num == 9:
        formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
    elif multirotor_num == 6:
        formation_configs = ['waiting', 'T', 'diamond', 'triangle']
    
    cmd= String()
    twist = Twist()   
    
    rospy.init_node('hover')
        
        
    if control_type == 'vel':
        multi_cmd_vel_flu_pub = [None]*multirotor_num
        multi_cmd_pub = [None]*multirotor_num
        for i in range(multirotor_num):
            multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
            multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
        leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
        leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
 
    else:
        multi_cmd_accel_flu_pub = [None]*multirotor_num
        multi_cmd_pub = [None]*multirotor_num
        for i in range(multirotor_num):
            multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10)
            multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
        leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)
        leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
    
    forward  = 0.0
    leftward  = 0.0
    upward  = 0.0
    angular = 0.0

    icount = 0
    while(1):  
        icount +=1
        # print(icount)
        if icount == 1:
            upward = 1.0
            print(upward)

        elif icount == 2:
            cmd = 'OFFBOARD'
            print(upward)
            print(cmd)
            sleep(1)
        elif icount == 3:
            cmd = 'ARM'
            print(upward)
            print(cmd)
            sleep(1)
        elif icount == 4:
            forward   = 0.0
            leftward   = 0.0
            upward   = 0.0
            angular  = 0.0
            cmd = 'HOVER'
            print(upward)
            print(cmd)
            sleep(3)
        else:
            m = icount
            sleep(5)
            
            
        twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward
        twist.angular.x = 0.0; twist.angular.y = 0.0;  twist.angular.z = angular
        
        print(twist)
        
        for i in range(multirotor_num):
            if ctrl_leader:
                if control_type == 'vel':
                    leader_cmd_vel_flu_pub.publish(twist)
                else:
                    leader_cmd_aceel_flu_pub.publish(twist)
                leader_cmd_pub.publish(cmd)
            else:
                if not cmd_vel_mask:
                    if control_type == 'vel':
                        multi_cmd_vel_flu_pub[i].publish(twist)   
                    else:
                        multi_cmd_accel_flu_pub[i].publish(twist)
                multi_cmd_pub[i].publish(cmd)
                
        cmd = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

dueen1123

关注

18
点赞
踩
122

收藏

觉得还不错? 一键收藏
23
评论
无人机路径规划3：ego-planner三维运动规划实现

XTDrone实现ego-planner编译ego-palnnercp -r ~/XTDrone/motion_planning/3d/ego_planner ~/catkin_ws/src/cd ~/catkin_ws/catkin_make #或catkin build遇到报错，安装各种依赖库，把显示未安装的全部安装一遍apt search occupancy
复制链接

扫一扫