XTdrone进行PX4激光slam建图获取坐标及自主飞行仿真

最近在学习无人机的室内定位和自主飞行，xtdrone在这方面提供了丰富的模型和例程资源，可以快速实现相关算法的仿真验证和部署，下面分享下基于xtdrone的资源进行激光slam建图定位以及控制px4进行简单轨迹飞行的过程

本文内容主要参考xtdrone官方使用文档：XTDrone使用文档 · 语雀

1、XTdrone环境的部署

此部分不多赘述，可以参考官方使用文档进行平台部署，也可以自行搜索相关成品镜像文件进行使用

2、安装Cartographer

Cartographer是google推出的一套基于图优化的SLAM算法且提供了ROS的集成，因此使用apt安装即可，新建终端输入以下命令

sudo apt install ros-melodic-cartographer-ros
sudo apt install ros-melodic-cartographer-rviz

安装map server用于保存地图

sudo apt install ros-melodic-map-server

xtdrone已经为我们提供了相关的配置文件以及launch文件例程，复制到刚刚安装的catographer功能包目录下

roscd cartographer_ros
sudo cp -r  ~/XTDrone/sensing/slam/laser_slam/cartographer/* ./

3.仿真环境launch文件配置

xtdrone的仿真launch文件存放在路径/home/han/PX4_Firmware/launch中,官方文档中使用的是indoor3.launch这个文件

注意在启动仿真环境前更改对应launch文件的sdf设置

sdf文件保存在路径/home/han/PX4_Firmware/Tools/sitl_gazebo/model中，对应更改launch文件的相关参数即可变更仿真环境中所使用的飞行器模型

这里由于我们使用激光雷达slam进行定位，将sdf更改为改为带有激光雷达的模型iris_2d_lidar，我这里使用的是我自己的launch文件，其余文件同理，将标黄的部分修改为如下图所示

同理在编辑自己的launch文件时也可以通过修改读取地图文件的路径来更改仿真环境所使用的地图，xtdrone提供的地图保存在路径/home/han/PX4_Firmware/Tools/sitl_gazebo/worlds下

通过更改launch文件中的地图路径即可变更仿真环境所使用的地图，这里我使用我自己的地图hang30.world，官方文档中所使用的地图为indoor3.world

 

4、QGroundcontrol配置PX4飞控

启动仿真环境

roslaunch px4 indoor3x.launch 

 打开QGroundControl地面站此时会自动连接到仿真模型中的飞控，在参数中进行修改

将EKF2_AID_MASK修改为仅vision posithon和visionyaw fution进行数据融合定位

由于激光雷达位于飞行器顶部，因此我们还要限制其最大飞行速度以保证定位的稳定性，修改MPC_XY_VEL_MAX参数以限制飞行速度，我这里设置成0.2

5.启动仿真环境进行仿真

关闭仿真环境后重新启动以让刚刚修改的飞控参数生效

roslaunch px4 indoor3x.launch 

在新终端启动Cartographer

roslaunch cartographer_ros xtdrone_2d.launch

 此时打开地面站已经可以看到激光slam回传的位置信息了

官方文档中使用键盘进行飞行控制，我这里写了一个方形轨迹的功能包对无人机进行飞行控制因此没有启动通信和键盘控制脚本，键盘控制可以参照官方文档

将cartographer输出的2D位姿和高度值，转换为mavros话题

cd ~/XTDrone/sensing/slam/laser_slam/script/
python laser_transfer.py iris 0 cartographer

 下面是我编写的轨迹飞行功能包代码，功能包和代码都命名为offboard_multi_position，功能为借助激光雷达slam的位置信息进行正方形轨迹飞行并降落

//包含ROS和MAVROS相关头文件 
#include <string> 
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>
#include <geometry_msgs/Twist.h>

#define ALTITUDE  2

int flag  = 1;

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg);

//定义变量，用于接收无人机的里程计信息
tf::Quaternion quat; 
double roll, pitch, yaw;
float init_position_x_take_off =0;
float init_position_y_take_off =0;
float init_position_z_take_off =0;
bool  flag_init_position = false;
nav_msgs::Odometry local_pos;
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);

void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

//回调函数接收无人机的里程计信息
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
    local_pos = *msg;
    if (flag_init_position==false && (local_pos.pose.pose.position.z!=0))
    {
		init_position_x_take_off = local_pos.pose.pose.position.x;
	    init_position_y_take_off = local_pos.pose.pose.position.y;
	    init_position_z_take_off = local_pos.pose.pose.position.z;
        flag_init_position = true;		    
    }
    tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat);	
	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_multi_position");

    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/iris_0/mavros/state", 10, state_cb);

    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/iris_0/mavros/setpoint_position/local", 10);
    
    ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/iris_0/mavros/local_position/odom", 10, local_pos_cb);

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/iris_0/mavros/cmd/arming");

    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/iris_0/mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x =init_position_x_take_off + 0;
    pose.pose.position.y =init_position_y_take_off + 0;
    pose.pose.position.z =init_position_z_take_off + ALTITUDE;
     

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
 
 	//此处满足一次请求进入offboard模式即可，官方例成循环切入offboard会导致无人机无法使用遥控器控制
    while(ros::ok())
    {
    	//请求进入OFFBOARD模式
        if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
           	last_request = ros::Time::now();
       	}
        else 
		{
			//请求解锁
			if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
			{
		        if( arming_client.call(arm_cmd) && arm_cmd.response.success)
		       	{
		            ROS_INFO("Vehicle armed");
		        }
		        	last_request = ros::Time::now();
			}
		}
	    
	    if(fabs(local_pos.pose.pose.position.z- init_position_z_take_off -ALTITUDE)<0.2)
		{	
			if(ros::Time::now() - last_request > ros::Duration(3.0))
			{
				break;
			}
		}
		//发布期望位置信息
		pose.pose.position.x =init_position_x_take_off + 0;
		pose.pose.position.y =init_position_y_take_off + 0;
		pose.pose.position.z =init_position_z_take_off + ALTITUDE;
		local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }   
    
    
    while(ros::ok())
    {
        if((flag == 1)  && (ros::Time::now() - last_request > ros::Duration(5.0)))
		{ 
			ROS_INFO("Position_1");
            pose.pose.position.x =init_position_x_take_off + 0;
			pose.pose.position.y =init_position_y_take_off + 0;
			pose.pose.position.z =init_position_z_take_off + ALTITUDE;                     
			last_request = ros::Time::now();
            flag=2;
        }

		if((flag ==2) && (ros::Time::now() - last_request > ros::Duration(8.0)))
		{
			ROS_INFO("Position_2 ");
		    pose.pose.position.x =init_position_x_take_off + 2;
			pose.pose.position.y =init_position_y_take_off + 0;
			pose.pose.position.z =init_position_z_take_off + ALTITUDE;     
			last_request = ros::Time::now();
			flag=3;
		}
		                   
		if((flag ==3) && (ros::Time::now() - last_request > ros::Duration(8.0)))
		{
		    ROS_INFO("Position_3 ");
		    pose.pose.position.x =init_position_x_take_off + 2;
			pose.pose.position.y =init_position_y_take_off + 2;
			pose.pose.position.z =init_position_z_take_off + ALTITUDE;      
		    last_request = ros::Time::now();
			flag=4;
		}

启动轨迹飞行功能包

 rosrun offboard_multi_position offboard_multi_position

 通过rviz和gazebo可以看到无人机通过激光slam定位完成了既定仿真任务