T265定点指点基本原理与极简代码实现

T265定点指点飞行

T265定点指点功能包: GitHub - maxibooksiyi/t265_to_mavros: 用于T265定点指点的功能包用于T265定点指点的功能包. Contribute to maxibooksiyi/t265_to_mavros development by creating an account on GitHub.https://github.com/maxibooksiyi/t265_to_mavros

基于T265或vins px4定点指点原理讲解与gazebo仿真演示20240329

 

PX4控制环

T265定点

飞控参数修改：EKF2_AID_MASK选择vision position fusion和vision yaw fusion , EKF2_HGT_MODE选择Vision T265的odom话题/t265/odom/sample转为/mavros/vision_pose/pose发给mavros

t265_to_mavros.cpp

https://github.com/maxibooksiyi/t265_to_mavros/blob/master/src/t265_to_mavros.cpp

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Eigen>
#include <nav_msgs/Odometry.h>
​
Eigen::Vector3d pos_drone_t265;
Eigen::Quaterniond q_t265;
​
void t265_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
        pos_drone_t265 = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
        q_t265 = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
}
​
int main(int argc, char **argv)
{
    ros::init(argc, argv, "t265_to_mavros");
    ros::NodeHandle nh;
​
        //  【订阅】t265估计位置
    ros::Subscriber t265_sub = nh.subscribe<nav_msgs::Odometry>("/t265/odom/sample", 100, t265_cb);
​
    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);
​
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(30.0);
​
    while(ros::ok()){
    
        geometry_msgs::PoseStamped vision;
​
        vision.pose.position.x = pos_drone_t265[0];
        vision.pose.position.y = pos_drone_t265[1];
        vision.pose.position.z = pos_drone_t265[2];
​
        vision.pose.orientation.x = q_t265.x();
        vision.pose.orientation.y = q_t265.y();
        vision.pose.orientation.z = q_t265.z();
        vision.pose.orientation.w = q_t265.w();
​
        vision.header.stamp = ros::Time::now();
        vision_pub.publish(vision);
​
        ros::spinOnce();
        rate.sleep();
    }
​
    return 0;
}

发送期望位置

http://wiki.ros.org/mavros http://wiki.ros.org/mavros_extras 常用mavros话题 /mavros/setpoint_raw/local /mavros/setpoint_position/local

mavros_msgs/PositionTarget Documentation

/mavros/setpoint_raw/local话题可以发送期望位置，速度，加速度，偏航，偏航角速度信息给飞控，可以通过掩码来控制发送哪些量给飞控，即哪些量生效。对应掩码位置为1为不使用，为0为使用，所以想通过/mavros/setpoint_raw/local话题给无人机发送期望位置与偏航，type_mask赋值为0b100111111000，即可实现，注意coordinate_frame 需要赋值，一般为1，即为本地系。示例代码如下：

    ros::Publisher setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
    mavros_msgs::PositionTarget pos_setpoint;
    pos_setpoint.type_mask = 0b100111111000;  // 100 111 111 000  xyz + yaw
    pos_setpoint.coordinate_frame = 1;
    pos_setpoint.position.x = 1.0;
    pos_setpoint.position.y = 0;
    pos_setpoint.position.z = 0.7;
    pos_setpoint.yaw = 0;
    setpoint_raw_local_pub.publish(pos_setpoint);

发xyz速度和xy位置和偏航

    ros::Publisher setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
    mavros_msgs::PositionTarget pos_setpoint;
    pos_setpoint.type_mask = 0b100111000100;
    pos_setpoint.coordinate_frame = 1;
    pos_setpoint.velocity.x = 0;
    pos_setpoint.velocity.y = 0;
    pos_setpoint.velocity.z = 0.1;
    pos_setpoint.position.x = 0;
    pos_setpoint.position.y = 0;
    pos_setpoint.yaw = 0;
    setpoint_raw_local_pub.publish(pos_setpoint);

解锁上锁

/mavros/cmd/arming mavros_msgs/CommandBool mavros_msgs/CommandBool Documentation

value 赋值为true是解锁，赋值为false是上锁。

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
​
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }

如果arming_client.call(arm_cmd)函数返回 true 表示执行请求服务成功，但并不代表执行服务成功，还需要判断服务执行后返回的值，解锁服务返回的状态存储在 arm_cmd.response.success 中，为true代表执行服务成功。 在 ros 中所有服务参数都在 xxx.request 中，所有返回状态都在 xxx.response 中。

模式切换

/mavros/set_mode mavros_msgs::SetMode mavros_msgs/SetMode Documentation

mode_sent为true代表模式切换正确

custom_mode 可以赋值的有下面这些 mavros/CustomModes - ROS Wiki

切offboard custom_mode 可以赋值为"OFFBOARD" 切land custom_mode 可以赋值为"AUTO.LAND" 切manual custom_mode 可以赋值为"MANUAL" 后面都会有用到

    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    mavros_msgs::SetMode land_set_mode;
    land_set_mode.request.custom_mode = "AUTO.LAND";
​
         set_mode_client.call(land_set_mode);
         if(land_set_mode.response.mode_sent)
           {
             ROS_INFO("land enabled");    
           }  

​

/mavros/state话题

mavros基础offboard例程

可仿真演示 https://docs.px4.io/main/zh/ros/mavros_offboard_cpp.html

//首先导入一系列头文件
#include <ros/ros.h>//ros库
#include <geometry_msgs/PoseStamped.h>  
//发布的位置消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped
//用来进行发送目标位置
​
#include <mavros_msgs/CommandBool.h>  
//CommandBool服务的头文件，该服务的类型为mavros_msgs：：CommandBool
//用来进行无人机解锁
​
#include <mavros_msgs/SetMode.h>     
//SetMode服务的头文件，该服务的类型为mavros_msgs：：SetMode
//用来设置无人机的飞行模式，切换offboard
​
#include <mavros_msgs/State.h>  
//订阅的消息体的头文件，该消息体的类型为mavros_msgs：：State
//查看无人机的状态
 
//建立一个订阅消息体类型的变量，用于存储订阅的信息
mavros_msgs::State current_state;
 
//订阅时的回调函数，接受到该消息体的内容时执行里面的内容，这里面的内容就是赋值
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
 
 
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node"); //ros系统的初始化，最后一个参数为节点名称
    ros::NodeHandle nh;
 
    //订阅。<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为1000）、回调函数
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
 
    //发布之前需要公告，并获取句柄，发布的消息体的类型为：geometry_msgs::PoseStamped
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
 
    //启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
 
    //启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
 
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);
 
    // 等待飞控连接mavros，current_state是我们订阅的mavros的状态，连接成功在跳出循环
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }
 
 
    //先实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;
 
    //建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"OFFBOARD"，作用便是用于后面的
    //客户端与服务端之间的通信（服务）
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
 
    //建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的
    //客户端与服务端之间的通信（服务）
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
 
    //更新时间
    ros::Time last_request = ros::Time::now();
 
    while(ros::ok())//进入大循环
    {
        //首先判断当前模式是否为offboard模式，如果不是，则客户端set_mode_client向服务端offb_set_mode发起请求call，
        //然后服务端回应response将模式返回，这就打开了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 //else指已经为offboard模式，然后进去判断是否解锁，如果没有解锁，则客户端arming_client向服务端arm_cmd发起请求call
            //然后服务端回应response成功解锁，这就解锁了
        {
            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();
            }
        }
 
        local_pos_pub.publish(pose); 
        //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
 
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}

指点飞正方形程序流程

可仿真演示 square_setpoint.cpp

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */
​
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
​
mavros_msgs::State current_state;
​
double position[3]={0,0,0};
​
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
​
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
        {
            position[0] = msg->pose.position.x;
            position[1] = msg->pose.position.y;
            position[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;
    float SIDE;
    bool AUTO_ARM_OFFBOARD;
    nh1.param<float>("height", HEIGHT, 0.65);
    nh1.param<float>("side", SIDE, 1.5);
    nh1.param<bool>("auto_arm_offboard", AUTO_ARM_OFFBOARD, false);
​
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, pos_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("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 = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = HEIGHT;
​
    //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::SetMode land_set_mode;
    land_set_mode.request.custom_mode = "AUTO.LAND";
​
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
​
    int offboard_flag = 0;//防止切land降落之后又去解锁切offboard
    int land_flag = 0;
​
    ros::Time last_request = ros::Time::now();
​
    while(ros::ok()){
        if(AUTO_ARM_OFFBOARD==true)
        {
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))&&(offboard_flag==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))&&(offboard_flag==0)){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();//可以通过这个方式保持当前指点持续几秒钟。
            }
        }
        }
        if(AUTO_ARM_OFFBOARD==false)
        {
        }
​
​
        //ros::Duration(1.0).sleep();
        if((abs(position[0])<0.2&&abs(position[1])<0.2&&abs(position[2]-HEIGHT)<0.2)&&(ros::Time::now() - last_request > ros::Duration(5.0))&&(land_flag==0))
        {
         pose.pose.position.x = SIDE;
         pose.pose.position.y = 0;
         pose.pose.position.z = HEIGHT; 
         last_request = ros::Time::now();
        }
        //last_request = ros::Time::now();
        //ros::Duration(1.0).sleep();
        if((abs(position[0]-SIDE)<0.2&&abs(position[1])<0.2&&abs(position[2]-HEIGHT)<0.2)&&(ros::Time::now() - last_request > ros::Duration(5.0)))
        {
         pose.pose.position.x = SIDE;
         pose.pose.position.y = SIDE;
         pose.pose.position.z = HEIGHT; 
         last_request = ros::Time::now();
        }
        if((abs(position[0]-SIDE)<0.2&&abs(position[1]-SIDE)<0.2&&abs(position[2]-HEIGHT)<0.2)&&(ros::Time::now() - last_request > ros::Duration(5.0)))
        {
         pose.pose.position.x = 0;
         pose.pose.position.y = SIDE;
         pose.pose.position.z = HEIGHT; 
         last_request = ros::Time::now();
        }
        if((abs(position[0])<0.2&&abs(position[1]-SIDE)<0.2&&abs(position[2]-HEIGHT)<0.2)&&(ros::Time::now() - last_request > ros::Duration(5.0)))
        {
         pose.pose.position.x = 0;
         pose.pose.position.y = 0;
         pose.pose.position.z = HEIGHT; 
         last_request = ros::Time::now();
         land_flag = 1;
        }
        if((abs(position[0])<0.2&&abs(position[1])<0.2&&abs(position[2]-HEIGHT)<0.2)&&(ros::Time::now() - last_request > ros::Duration(5.0)))
        {
         //pose.pose.position.x = 2;
         //pose.pose.position.y = 2;
         //pose.pose.position.z = 2; 
         set_mode_client.call(land_set_mode);
         if(land_set_mode.response.mode_sent)
           {
             ROS_INFO("land enabled");
             offboard_flag = 1;    
           }  
        }
​
        local_pos_pub.publish(pose);
​
        ros::spinOnce();
        rate.sleep();
    }
​
    return 0;
}