ubuntu22.04 ros2 fast_lio2 复现

源项目：

https://github.com/liangheming/FASTLIO2_ROS2https://github.com/liangheming/FASTLIO2_ROS2

项目依赖以下第三方库，

pcl Eigen sophus gtsam livox_ros_driver2

在我们编译完mid360的sdk和ros2驱动包（可以参考我之前的文章）后，我们只需编译sophus gtsam即可，其他几个之前编译好了，反正缺啥编译啥

https://blog.csdn.net/m0_53931365/article/details/153976259?spm=1001.2014.3001.5502https://blog.csdn.net/m0_53931365/article/details/153976259?spm=1001.2014.3001.5502

1.sophus编译

https://github.com/strasdat/Sophushttps://github.com/strasdat/Sophus

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout 1.22.10 #旧版本
mkdir build && cd build
cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON
make
sudo make install

原文说新的Sophus依赖fmt，可以在CMakeLists.txt中添加add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)去除，否则会报错

2.gtsam编译

https://github.com/borglab/gtsamhttps://github.com/borglab/gtsam下载完项目，需要需要添加dllexport.h文件，不然编译会报错

git clone https://github.com/borglab/gtsam.git
# 进入 cephes 目录
cd gtsam/gtsam/3rdparty/cephes

# 创建 dllexport.h 文件
tee dllexport.h > /dev/null << 'EOF'
#ifndef DLLEXPORT_H
#define DLLEXPORT_H

/* Define DLLEXPORT for Windows, empty for other platforms */
#ifdef _WIN32
#ifdef GTSAM_SHARED_LIB
#define DLLEXPORT __declspec(dllexport)
#else
#define DLLEXPORT __declspec(dllimport)
#endif
#else
#define DLLEXPORT
#endif

#endif // DLLEXPORT_H
EOF
cd gtsam
#!bash
mkdir build
cd build
cmake ..
make check -j8 #多线程编译
sudo make install

编译有点久,编译完成，查看动态链接库的路径

# 找到 libgtsam.so.4 文件
find ~ -name "libgtsam.so.4" 2>/dev/null

如果后面保存地图出现找不到libgtsam.so.4，可以手动添加到路径到环境变量

# 临时解决方案（在当前终端生效）
export LD_LIBRARY_PATH=/path/to/your/gtsam/install/lib:$LD_LIBRARY_PATH

# 永久解决方案（添加到 ~/.bashrc）
echo 'export LD_LIBRARY_PATH=/path/to/your/gtsam/install/lib:$LD_LIBRARY_PATH' >> ~/.bashrc
source ~/.bashrc

例如

szz@szz:~/ws_livox$ find ~ -name "libgtsam.so.4" 2>/dev/null
/home/szz/gtsam-develop/build/gtsam/libgtsam.so.4
szz@szz:~/ws_livox$ export LD_LIBRARY_PATH=/home/szz/gtsam-develop/build/gtsam/libgtsam.so.4:$LD_LIBRARY_PATH

4.fast-lio2

将fast-lio2里面的包移动到自己src中编译即可使用

5.建图验证

5.1下载验证的pcd文件，也可以用自己的

http://链接: https://pan.baidu.com/s/1rTTUlVwxi1ZNo7ZmcpEZ7A?pwd=t6yb 提取码: t6yb

1.激光惯性里程计

ros2 launch fastlio2 lio_launch.py
ros2 bag play your_bag_file

2.里程计加回环

启动回环节点

ros2 launch pgo pgo_launch.py
ros2 bag play your_bag_file

保存地图

ros2 service call /pgo/save_maps interface/srv/SaveMaps "{file_path: 'your_save_dir', save_patches: true}"

预览pcd地图

sudo apt-get install pcl-tools
pcl_viewer xxx.pcd

3.里程计加重定位

启动重定位节点

ros2 launch localizer localizer_launch.py
ros2 bag play your_bag_file // 可选

设置重定位初始值

ros2 service call /localizer/relocalize interface/srv/Relocalize "{"pcd_path": "your_map.pcd", "x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0, "pitch": 0.0, "roll": 0.0}"

检查重定位结果

ros2 service call /localizer/relocalize_check interface/srv/IsValid "{"code": 0}"

4.一致性地图优化

启动一致性地图优化节点

ros2 launch hba hba_launch.py

调用优化服务

ros2 service call /hba/refine_map interface/srv/RefineMap "{"maps_path": "your maps directory"}"

如果需要调用优化服务，保存地图时需要设置save_patches为true

原作者提到机器性能问题，TIPS：将timerCB改成用用一个单独线程去run就可以了。

优化参考

#include <queue>
#include <mutex>
#include <filesystem>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/pose_stamped.hpp>

#include "localizers/commons.h"
#include "localizers/icp_localizer.h"
#include "interface/srv/relocalize.hpp"
#include "interface/srv/is_valid.hpp"
#include <yaml-cpp/yaml.h>

using namespace std::chrono_literals;

struct NodeConfig
{
std::string cloud_topic = "/fastlio2/body_cloud";
std::string odom_topic = "/fastlio2/lio_odom";
std::string map_frame = "map";
std::string local_frame = "lidar";
double update_hz = 1.0;
};

struct NodeState
{
std::mutex message_mutex;
std::mutex service_mutex;

bool message_received = false;
bool service_received = false;
bool localize_success = false;
rclcpp::Time last_send_tf_time = rclcpp::Clock().now();
builtin_interfaces::msg::Time last_message_time;
CloudType::Ptr last_cloud = std::make_shared<CloudType>();
M3D last_r; // localmap_body_r
V3D last_t; // localmap_body_t
M3D last_offset_r = M3D::Identity(); // map_localmap_r
V3D last_offset_t = V3D::Zero(); // map_localmap_t
M4F initial_guess = M4F::Identity();
};

class LocalizerNode : public rclcpp::Node
{
public:
LocalizerNode() : Node("localizer_node")
{
RCLCPP_INFO(this->get_logger(), "Localizer Node Started");

// 创建回调组
m_timer_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
m_subscriber_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
m_service_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

// 创建执行器选项
rclcpp::SubscriptionOptions subscription_options;
subscription_options.callback_group = m_subscriber_callback_group;

rclcpp::PublisherOptions publisher_options;
publisher_options.callback_group = m_subscriber_callback_group;

loadParameters();
rclcpp::QoS qos = rclcpp::QoS(10);

// 使用选项创建订阅者
m_cloud_sub.subscribe(this, m_config.cloud_topic, qos.get_rmw_qos_profile());
m_odom_sub.subscribe(this, m_config.odom_topic, qos.get_rmw_qos_profile());

m_tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(*this);

m_sync = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, nav_msgs::msg::Odometry>>>(message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, nav_msgs::msg::Odometry>(10), m_cloud_sub, m_odom_sub);
m_sync->setAgePenalty(0.1);
m_sync->registerCallback(std::bind(&LocalizerNode::syncCB, this, std::placeholders::_1, std::placeholders::_2));
m_localizer = std::make_shared<ICPLocalizer>(m_localizer_config);

// 使用回调组创建服务
m_reloc_srv = this->create_service<interface::srv::Relocalize>(
"relocalize",
std::bind(&LocalizerNode::relocCB, this, std::placeholders::_1, std::placeholders::_2),
rmw_qos_profile_services_default,
m_service_callback_group);

m_reloc_check_srv = this->create_service<interface::srv::IsValid>(
"relocalize_check",
std::bind(&LocalizerNode::relocCheckCB, this, std::placeholders::_1, std::placeholders::_2),
rmw_qos_profile_services_default,
m_service_callback_group);

m_map_cloud_pub = this->create_publisher<sensor_msgs::msg::PointCloud2>("map_cloud", 10);

// 使用回调组创建定时器
m_timer = this->create_wall_timer(
10ms,
std::bind(&LocalizerNode::timerCB, this),
m_timer_callback_group);
}

void loadParameters()
{
this->declare_parameter("config_path", "");
std::string config_path;
this->get_parameter<std::string>("config_path", config_path);
YAML::Node config = YAML::LoadFile(config_path);
if (!config)
{
RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!");
return;
}
RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str());

m_config.cloud_topic = config["cloud_topic"].as<std::string>();
m_config.odom_topic = config["odom_topic"].as<std::string>();
m_config.map_frame = config["map_frame"].as<std::string>();
m_config.local_frame = config["local_frame"].as<std::string>();
m_config.update_hz = config["update_hz"].as<double>();

m_localizer_config.rough_scan_resolution = config["rough_scan_resolution"].as<double>();
m_localizer_config.rough_map_resolution = config["rough_map_resolution"].as<double>();
m_localizer_config.rough_max_iteration = config["rough_max_iteration"].as<int>();
m_localizer_config.rough_score_thresh = config["rough_score_thresh"].as<double>();

m_localizer_config.refine_scan_resolution = config["refine_scan_resolution"].as<double>();
m_localizer_config.refine_map_resolution = config["refine_map_resolution"].as<double>();
m_localizer_config.refine_max_iteration = config["refine_max_iteration"].as<int>();
m_localizer_config.refine_score_thresh = config["refine_score_thresh"].as<double>();
}

void timerCB()
{
if (!m_state.message_received)
return;

rclcpp::Duration diff = rclcpp::Clock().now() – m_state.last_send_tf_time;

bool update_tf = diff.seconds() > (1.0 / m_config.update_hz) && m_state.message_received;

if (!update_tf)
{
sendBroadCastTF(m_state.last_message_time);
return;
}

m_state.last_send_tf_time = rclcpp::Clock().now();

M4F initial_guess = M4F::Identity();
if (m_state.service_received)
{
std::lock_guard<std::mutex> lock(m_state.service_mutex);
initial_guess = m_state.initial_guess;
// m_state.service_received = false;
}
else
{
std::lock_guard<std::mutex> lock(m_state.message_mutex);
initial_guess.block<3, 3>(0, 0) = (m_state.last_offset_r * m_state.last_r).cast<float>();
initial_guess.block<3, 1>(0, 3) = (m_state.last_offset_r * m_state.last_t + m_state.last_offset_t).cast<float>();
}

M3D current_local_r;
V3D current_local_t;
builtin_interfaces::msg::Time current_time;
{
std::lock_guard<std::mutex> lock(m_state.message_mutex);
current_local_r = m_state.last_r;
current_local_t = m_state.last_t;
current_time = m_state.last_message_time;
m_localizer->setInput(m_state.last_cloud);
}

bool result = m_localizer->align(initial_guess);
if (result)
{
M3D map_body_r = initial_guess.block<3, 3>(0, 0).cast<double>();
V3D map_body_t = initial_guess.block<3, 1>(0, 3).cast<double>();
m_state.last_offset_r = map_body_r * current_local_r.transpose();
m_state.last_offset_t = -map_body_r * current_local_r.transpose() * current_local_t + map_body_t;
if (!m_state.localize_success && m_state.service_received)
{
std::lock_guard<std::mutex> lock(m_state.service_mutex);
m_state.localize_success = true;
m_state.service_received = false;
}
}
sendBroadCastTF(current_time);
publishMapCloud(current_time);
}

void syncCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud_msg, const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg)
{
std::lock_guard<std::mutex> lock(m_state.message_mutex);

pcl::fromROSMsg(*cloud_msg, *m_state.last_cloud);

m_state.last_r = Eigen::Quaterniond(odom_msg->pose.pose.orientation.w,
odom_msg->pose.pose.orientation.x,
odom_msg->pose.pose.orientation.y,
odom_msg->pose.pose.orientation.z)
.toRotationMatrix();
m_state.last_t = V3D(odom_msg->pose.pose.position.x,
odom_msg->pose.pose.position.y,
odom_msg->pose.pose.position.z);
m_state.last_message_time = cloud_msg->header.stamp;
if (!m_state.message_received)
{
m_state.message_received = true;
m_config.local_frame = odom_msg->header.frame_id;
}
}

void sendBroadCastTF(builtin_interfaces::msg::Time &time)
{
geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.frame_id = m_config.map_frame;
transformStamped.child_frame_id = m_config.local_frame;
transformStamped.header.stamp = time;
Eigen::Quaterniond q(m_state.last_offset_r);
V3D t = m_state.last_offset_t;
transformStamped.transform.translation.x = t.x();
transformStamped.transform.translation.y = t.y();
transformStamped.transform.translation.z = t.z();
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
m_tf_broadcaster->sendTransform(transformStamped);
}

void relocCB(const std::shared_ptr<interface::srv::Relocalize::Request> request, std::shared_ptr<interface::srv::Relocalize::Response> response)
{
std::string pcd_path = request->pcd_path;
float x = request->x;
float y = request->y;
float z = request->z;
float yaw = request->yaw;
float roll = request->roll;
float pitch = request->pitch;

if (!std::filesystem::exists(pcd_path))
{
response->success = false;
response->message = "pcd file not found";
return;
}

Eigen::AngleAxisd yaw_angle = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd roll_angle = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitch_angle = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY());
bool load_flag = m_localizer->loadMap(pcd_path);
if (!load_flag)
{
response->success = false;
response->message = "load map failed";
return;
}
{
std::lock_guard<std::mutex> lock(m_state.service_mutex);
m_state.initial_guess.setIdentity();
m_state.initial_guess.block<3, 3>(0, 0) = (yaw_angle * roll_angle * pitch_angle).toRotationMatrix().cast<float>();
m_state.initial_guess.block<3, 1>(0, 3) = V3F(x, y, z);
m_state.service_received = true;
m_state.localize_success = false;
}

response->success = true;
response->message = "relocalize success";
return;
}

void relocCheckCB(const std::shared_ptr<interface::srv::IsValid::Request> request, std::shared_ptr<interface::srv::IsValid::Response> response)
{
std::lock_guard<std::mutex> lock(m_state.service_mutex);
if (request->code == 1)
response->valid = true;
else
response->valid = m_state.localize_success;
return;
}

void publishMapCloud(builtin_interfaces::msg::Time &time)
{
if (m_map_cloud_pub->get_subscription_count() < 1)
return;
CloudType::Ptr map_cloud = m_localizer->refineMap();
if (map_cloud->size() < 1)
return;
sensor_msgs::msg::PointCloud2 map_cloud_msg;
pcl::toROSMsg(*map_cloud, map_cloud_msg);
map_cloud_msg.header.frame_id = m_config.map_frame;
map_cloud_msg.header.stamp = time;
m_map_cloud_pub->publish(map_cloud_msg);
}

private:
NodeConfig m_config;
NodeState m_state;

ICPConfig m_localizer_config;
std::shared_ptr<ICPLocalizer> m_localizer;
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> m_cloud_sub;
message_filters::Subscriber<nav_msgs::msg::Odometry> m_odom_sub;
rclcpp::TimerBase::SharedPtr m_timer;
std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, nav_msgs::msg::Odometry>>> m_sync;
std::shared_ptr<tf2_ros::TransformBroadcaster> m_tf_broadcaster;
rclcpp::Service<interface::srv::Relocalize>::SharedPtr m_reloc_srv;
rclcpp::Service<interface::srv::IsValid>::SharedPtr m_reloc_check_srv;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_map_cloud_pub;

// 回调组
rclcpp::CallbackGroup::SharedPtr m_timer_callback_group;
rclcpp::CallbackGroup::SharedPtr m_subscriber_callback_group;
rclcpp::CallbackGroup::SharedPtr m_service_callback_group;
};

int main(int argc, char **argv)
{
rclcpp::init(argc, argv);

// 创建多线程执行器
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 3);
auto node = std::make_shared<LocalizerNode>();

// 将节点添加到执行器
executor.add_node(node);

// 启动执行器
executor.spin();

rclcpp::shutdown();
return 0;
}