Jetson nano+T265实现定点和指点飞行

原创于 2025-01-07 21:53:17 发布 · 1.8k 阅读

36 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

标签

#无人机

Jetson nano+T265实现定点和指点飞行

前言
地面站
- 遥控器
VIO
定点飞行
指点飞行
坐标关系

前言

为了测试无人机的定位是否准确，还有确定切换Offboard模式的方式，所以进行了T265的定点和指点飞行，在此记录下来。

操作系统：Ubuntu20.04 + noetic + mavros + realsense-ros + realsense

地面站

我地面站用的是QGC，不过QGC好像不能在Jetson nano中安装，我直接安装在笔记本，并且搞了两个数传，一个安装在无人机上，一个接在电脑，这样无人机上电后，就能直接连接上地面站了。
在这里插入图片描述
没有数传的话，像这样修改px4.launch文件

在这里插入图片描述
然后运行文件后就能自动连接上地面站了

roslaunch mavros px4.launch

主要修改下面的参数，从而启动视觉定位

EKF2_AID_MASK 修改为24
EKF2_HGT_MODE 修改为Vision

遥控器

我用的遥控器是AT9s，这是我在地面站设置的飞行模式

在这里插入图片描述
对应的拨杆

在这里插入图片描述
我个人的习惯是先起飞再切换成offboard，所以增加了一个切换offboard的拨杆，如果是直接通过程序切换offboard模式的可以不用设置

VIO

我用的是PX4官方写的功能包，它之前的链接已经失效了，我当时也找了好久，我已经把这个功能包上传到百度网盘了，有需要的自己拿

链接: https://pan.baidu.com/s/1UYm0jPymjqCS2gm7DZ_-TQ?pwd=tan6
提取码: tan6

直接解压到工作空间下，然后编译就行了。主要就是这三个launch文件，他们之间的区别是：
bridge.launch：启动T265相机节点
bridge_mavros.launch：启动T265相机节点和mavros
bridge_mavros_sitl.launch：用于仿真
在这里插入图片描述

启动前要先修改以下第一个文件bridge.launch的参数，开头的三个参数为飞控中心指向T265中心的向量，我这个T265在飞控前方8cm，下方9.6cm，参数如图所示；后面三个参数则为T265的旋转角（偏航，俯仰，横滚），我的相机照射的是机头方向，所以不用修改，如果相机是照射地面的话，则参数为[0, 1.5708, 0]
在这里插入图片描述
如果用的是bridge_mavros.launch的话，记得把fcu_url和gcs_url更改一下

这个功能包的核心就是将T265的t265/odom/sample的位置信息发送到飞控/mavros/vision_pose/pose这样飞控的/mavros/local_position/*才会有位置信息，无人机才能解锁起飞

定点飞行

直接启动VIO中的launch文件

roslaunch px4_realsense_bridge bridge_mavros.launch

一般会报两个错，第一个报错是因为Jetson nano没有识别到T265，重新插拔一下T265即可

在这里插入图片描述
第二个错误是接口权限不够，输入sudo chmod 777 /dev/ttyUSB0即可

在这里插入图片描述

重新运行launch文件，输入命令

rostopic echo /mavros/local_position/pose

即可看到飞控的当前位置

在这里插入图片描述
朝着机头方向，向前移动x的坐标增大，向左移动y的坐标增大，向上移动z的坐标增大
如果位置漂移比较大的话，需要拿起无人机晃一晃，等到稳定下来后，就能开始解锁起飞了

指点飞行

测试完定点飞行后，接下来测试指点飞行，目的是飞一个边长为1m的正方形
这是我根据官方例程修改的代码，不过我是先手动起飞到一定高度，再切换成offboard模式，所以去掉了官方关于飞机解锁的程序

/**
 * @file square_setpoint.cpp
 * @brief 用于实机测试T265指点，目的是先让手动起飞到一定高度，再切换为offboard指点
*/

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <Eigen/Eigen>

mavros_msgs::State currentState;
geometry_msgs::PoseStamped position;
geometry_msgs::PoseStamped pose;
Eigen::Vector3d dronePose;
int flag;

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

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    position = *msg;
    dronePose(0) = msg->pose.position.x;
    dronePose(1) = msg->pose.position.y;
    dronePose(2) = msg->pose.position.z;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "square_setpoint");
    ros::NodeHandle nh;
    ros::NodeHandle nh1("~");
    
    float HEIGHT = 1;                   // 飞行高度
    float SIDE = 1;                     // 飞行边长

    
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, pos_cb);
    ros::Subscriber state_subu = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 10);

    ros::Rate rate(20.0);

    while(ros::ok() && !currentState.connected) {
        ros::spinOnce();
        rate.sleep();
        ROS_WARN("Waiting for connect");
    }
    ROS_INFO("Connected Success!");
    ros::Time last_request = ros::Time::now();

    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = HEIGHT;
    while(ros::ok()) 
    {
        if(currentState.mode == "OFFBOARD")
        {
            ROS_INFO("Offboard Success");
            if((ros::Time::now() - last_request) > ros::Duration(5.0))
            {
                if(abs(dronePose(0)) < 0.2 && abs(dronePose(1)) < 0.2) {
                    pose.pose.position.x = SIDE;
                    pose.pose.position.y = 0;
                    pose.pose.position.z = HEIGHT;
                    last_request = ros::Time::now();  
                }
                if(abs(dronePose(0) - SIDE) < 0.2 && abs(dronePose(1)) < 0.2) {
                    pose.pose.position.x = SIDE;
                    pose.pose.position.y = SIDE;
                    pose.pose.position.z = HEIGHT;
                    last_request = ros::Time::now(); 
                }
                if(abs(dronePose(0) - SIDE) < 0.2 && abs(dronePose(1) - SIDE) < 0.2) {
                    pose.pose.position.x = 0;
                    pose.pose.position.y = SIDE;
                    pose.pose.position.z = HEIGHT;
                    last_request = ros::Time::now();                    
                }
                if(abs(dronePose(0)) < 0.2 && abs(dronePose(1) - SIDE) < 0.2) {
                    pose.pose.position.x = 0;
                    pose.pose.position.y = 0;
                    pose.pose.position.z = HEIGHT;
                    last_request = ros::Time::now();                     
                }
            }
            
        }
        else {
            ROS_WARN("Waiting for Offboard");
        }
        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }
    return 0;

}