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

置顶

诗筱涵

已于 2024-03-29 17:08:56 修改

阅读量1.9k

点赞数 23

CC 4.0 BY-SA版权

分类专栏：无人机-SLAM-T265 用简单代码实现功能文章标签：无人机 T265

于 2024-02-21 13:04:04 首次发布

本文链接：https://blog.youkuaiyun.com/sinat_16643223/article/details/136209079

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::