[ROS2] CMU 团队的 Autonomous Exploration 导航算法+MID-360 部署到实车记录
Autonomous Exploration 框架如图所示：
其中State estimation和Motion control部分是需要我们去实现的，而在本次部署到实车过程中采用FAST_LIO算法实现了 MID-360 到导航算法之间的桥接，大致流程框图如下所示：
MID-360 节点
1. 安装 Livox-SDK2
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd ./Livox-SDK2/
mkdir build
cd build
cmake .. && make -j4
sudo make install
2. 安装 livox_ros_driver2
mkdir -p ws_livox/src && cd ws_livox/src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
source /opt/ros/humble/setup.sh
./build.sh humble
完成以上安装之后，测试 MID-360 是否能正常运行。将雷达连接到电脑上后，可以通过wireshark来查看雷达的 IP 地址，得到雷达地址后，并将本机的有线 IPv4 地址设置为 192.168.1.50，子网掩码设置为 255.255.255.0。
接着进入 config 文件夹中，编辑MID360_config.json文件，将"cmd_data_ip"、"push_msg_ip"、"point_data_ip"、"imu_data_ip"的 ip 设置为本机的有线 ip，即刚才设置的 192.168.1.50，然后将"ip"设置为雷达的 ip 即可。
3. 测试 MID-360
运行下面 launch 文件查看点云:
cd ws_livox
source install/setup.bash
ros2 launch livox_ros_driver2 rviz_MID360.launch
此时可以通过 rviz 界面看到点云，但是注意，在实际使用该导航框架的时候不能运行该 launch 文件，因为 MID-360 原生的点云数据不是按照线束顺序进行排列的，因此不符合 loam 系列算法所支持的数据类型。
4. 运行 MID-360
在实际运行的时候通过运行下面的 launch 文件启动雷达:
ros2 launch livox_ros_driver2 msg_MID360.launch
这样雷达节点所发布的点云数据类型是livox_ros_driver::CustomMsg，而 FAST_LIO 算法算是对 MID360 做了专门的适配，其接收的点云数据默认就是livox_ros_driver::CustomMsg。
FAST_LIO 算法节点
1. 安装 FAST_LIO
这里在编译的时候，官方文档进行了说明，需要先在 livox_ros_driver2 工作空间中进行 source，即先要找到 livox 的包才能成功编译。
cd ws_livox
source install/setup.bash
cd ..
mkdir -p fast_lio_Ws/src && cd fast_lio_Ws/src
git clone https://github.com/Ericsii/FAST_LIO.git --recursive
git checkout ROS2
cd ..
colcon build --symlink-install
source install/setup.bash
2. 运行 FAST_LIO
cd fast_lio_Ws
source install/setup.bash
ros2 launch fast_lio mapping.launch.py config_file:=avia.yaml
Autonomous Exploration 导航算法
1. 安装
sudo apt update
sudo apt install libusb-dev ros-humble-joy ros-humble-gazebo-msgs \
ros-humble-gazebo-plugins ros-humble-gazebo-ros ros-humble-gazebo-ros2-control \
ros-humble-gazebo-ros-pkgs ros-humble-pcl-ros
git clone https://github.com/HongbiaoZ/autonomous_exploration_development_environment.git
git checkout humble
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.sh
2. 测试
通过运行仿真环境来测试安装是否成功:
cd autonomous_exploration_development_environment
source install/setup.bash
ros2 launch vehicle_simulator system_indoor.launch
接着会打开一个 rviz 界面，里面有小车、雷达扫描到的点云，通过点击 waypoint 按键并在地图上设置目标点，如果小车会跟着目标点走，就说明整个的环境已经配置完成。
3. 实机运行
在实机运行之前需要对 loam_interface 节点进行修改，用于适配我们本次所需要的内容。
首先是 loam_interface.launch 文件中，将stateEstimationTopic对应的话题修改为**/Odometry**，将registeredScanTopic对应的话题修改为**/cloud_registered**，这两个话题就是 FAST_LIO 节点所发布的。
接着修改 loamInterface.cpp 文件，在laserCloudHandler和odometryHandler回调函数中，原本的代码是将各 x、y、z 以及四元数等部分进行了调换，估计是跟他们团队在测试的时候雷达的安装方式有关，而在本次使用过程中，激光雷达正装，即前方是 x 轴，左边是 y 轴，上方是 z 轴，故需要将所有对方位进行了调整的地方都还原成原本的即可。
同时还注意到src/local_planner/src/pathFollower.cpp中发布的控制话题数据类型为geometry_msgs::msg::TwistStamped，而我们通常使用的小车所对应的控制话题为geometry_msgs/Twist，这里只需要将对应的话题替换掉，然后删掉时间戳部分即可。
最后重新编译运行:
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash
ros2 launch vehicle_simulator system_real_robot.launch
然后就可以和测试的时候一样了，设置一个地图上的目标点，然后打开终端查看话题信息:
ros2 topic echo /cmd_vel
小车节点
最后就是启动自己的小车节点，订阅/cmd_vel 话题，在 rviz 中设置目标点就可以实现定点导航了。
