无人机安装MID360雷达并使用FAST_LIO建图，最后以MID-360作为定位源

一、安装

1、安装Livox-SDK2，安装流程：

git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd ./Livox-SDK2/
mkdir build
cd build
cmake .. && make -j
sudo make install

2、创建工作空间并下载FAST_LIO(建图算法)、livox_ros_driver（ros驱动），FAST_LIO依赖livox_ros_driver，所以需要下载livox_ros_driver。

step1:创建工作空间
mkdir -p ~/livox_ws/src
cd ~/livox_ws/src
 
step2:下载FAST_LIO
git clone https://github.com/hku-mars/FAST_LIO.git
cd FAST_LIO
git submodule update --init
 
step3:下载 livox_ros_driver
git clone https://github.com/Livox-SDK/livox_ros_driver.git
 
step4:编译
cd ~/livox_ws
catkin_make

3、下载livox_ros_driver2

git clone https://github.com/Livox-SDK/livox_ros_driver2.git
此处由于我使用的为ROS1,所以编译命令为：
./build.sh ROS1
若使用ROS2，则编译命令为：（暂时没试过）
./build.sh ROS2
 
最后
cd ~/livox_ws
catkin_make

4、修改ip地址

        4.1 修改雷达 Ip（雷达和电脑连接到一起，因此将雷达网线的ip固定为自己想要的Ip）

        4.2 修改MID360雷达json文件配置信息，地址要对应起来，其中第28行的1.100要根据雷达后几位进行修改，假设我的雷达为47MDL960020100，其最后两位即为当前雷达的默认ip地址，即雷达的出厂默认ip地址为192.168.1.100。

5、修改livox_ros_driver2下面的启动文件

       5.1修改 ~/livox_ws/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch 文件中的bd_list的参数为值为雷达上的二维码下面的值

    5.2 修改 ~/livox_ws/src/livox_ros_driver2/launch_ROS1/rviz_MID360.launch文件中的bd_list的参数为值为雷达上的二维码下面的值

6、测试建图效果

1、进入到Fast-lio 文件夹，启动 mapping_avia.launch  文件
2、启动 livox_ros_driver2 里的 msg_MID360.launch 文件

二、将雷达的里程计数据输出给无人机

这个后续补充，目前有点问题！！！

目的：从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;
};