无人机以MID-360作为定位信息，_mid360,如何进行室内定位

目的：从TF（Transform Frame）树中获取机器人的当前位置和姿态信息，并将其发布到MAVROS以供无人机使用。

<launch>
<!--向飞控发送位置信息-->
<include file = "$(find fast_lio)/launch/mapping_avia.launch">
< / include>
<include file = "$(find livox_ros_driver2)/launch_ROS1/msg_MID360.launch">
< / include>
<include file = "$(find mavros)/launch/px4.launch">
<arg name = "fcu_url"           default = "/dev/ttyACM0:921600" / >
< / include>
<include file = "$(find t265_to_mavros)/launch/mid360_tf_to_mavros.launch">
< / include>
 
< / launch>

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
ros::Publisher position_pub;
using namespace std;
int main(int argc, char** argv) {
    ros::init(argc, argv, "position_to_mavros");
 
    ros::NodeHandle node("~");
 
    geometry_msgs::PoseStamped cur_position;//创建一个名为 cur_position 的变量，用于存储当前位置信息。
 
    position_pub = node.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);
 
    tf2_ros::Buffer tfBuffer;//创建一个坐标变换的缓存对象。
    tf2_ros::TransformListener tfListener(tfBuffer);//创建一个监听坐标变换的对象，并将缓存对象传递给它。
 
    //view path in rviz
    nav_msgs::Path body_path;
    std::string target_frame_id;
    std::string source_frame_id;
    //std::string target_frame_id = "carto_odom";
    //std::string source_frame_id = "base_link";
 
    double output_rate = 50, roll_obj = 0, pitch_obj = 0, yaw_obj = 0;
 
    node.getParam("target_frame_id", target_frame_id);
    node.getParam("source_frame_id", source_frame_id);
    node.getParam("output_rate", output_rate);
    node.getParam("roll_obj", roll_obj);
    node.getParam("pitch_obj", pitch_obj);
    node.getParam("yaw_obj", yaw_obj);
 
    cout << "target_frame_id:" << target_frame_id << endl;
    cout << "source_frame_id:" << source_frame_id << endl;
    cout << "output_rate:" << output_rate << endl;
    cout << "roll_obj:" << roll_obj << endl;
    cout << "pitch_obj:" << pitch_obj << endl;
    cout << "yaw_obj:" << yaw_obj << endl;
 
    ros::Rate rate(output_rate);
    while (node.ok()) {
        geometry_msgs::TransformStamped transformStamped;
        try {
            transformStamped = tfBuffer.lookupTransform(target_frame_id, source_frame_id,
                ros::Time(0), ros::Duration(3.0));//创建一个存储坐标变换信息的对象。
 
            static tf2::Quaternion quat_obj, quat_body;//创建两个静态变量，用于存储对象的四元数。
            quat_obj = tf2::Quaternion(transformStamped.transform.rotation.x, transformStamped.transform.rotation.y, transformStamped.transform.rotation.z, transformStamped.transform.rotation.w);//根据获取的坐标变换信息，将旋转部分的四元数存储到 quat_obj 中。
 
            quat_body.setRPY(roll_obj, pitch_obj, yaw_obj);//设置 quat_body 的欧拉角（滚转、俯仰和偏航）。
            //ROS_INFO_STREAM(quat_body);
            quat_body = quat_obj * quat_body;//将 quat_body 和 quat_obj 进行四元数乘法。
            quat_body.normalize();//将四元数归一化，确保其长度为1。
            //
            cur_position.pose.position.x = transformStamped.transform.translation.x;
            cur_position.pose.position.y = transformStamped.transform.translation.y;
            cur_position.pose.position.z = transformStamped.transform.translation.z;
            cout << "cur_position.pose.position.x:" << cur_position.pose.position.x << endl;
            cout << "cur_position.pose.position.y:" << cur_position.pose.position.y << endl;
            cout << "cur_position.pose.position.z:" << cur_position.pose.position.z << endl;
 
            cur_position.pose.orientation.x = quat_body.y();
            cur_position.pose.orientation.y = quat_body.x();
            cur_position.pose.orientation.z = quat_body.z();
            cur_position.pose.orientation.w = quat_body.w();
            cout << "cur_position.pose.orientation.x:" << cur_position.pose.orientation.x << endl;
            cout << "cur_position.pose.orientation.y:" << cur_position.pose.orientation.y << endl;
            cout << "cur_position.pose.orientation.z:" << cur_position.pose.orientation.z << endl;
            cout << "cur_position.pose.orientation.w:" << cur_position.pose.orientation.w << endl;
            cur_position.header.stamp = ros::Time::now();
            cur_position.header.frame_id = transformStamped.header.frame_id;
            position_pub.publish(cur_position);
 
        }
        catch (tf2::TransformException& ex) {
            ROS_WARN("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        rate.sleep();
    }
    return 0;
};