在ROS2 humble中实现Livox mid-360双雷达合成点云
主播本人用的系统是ubuntu 22.04 humble
一.实现目标
要做的是两个livox mid-360雷达点云融合 接受到的数据类型是PiontCloud2 最后发布的格式也是PointCloud2
数据类型是
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
float32 intensity # the value is reflectivity, 0.0~255.0
uint8 tag # livox tag
uint8 line # laser number in lidar
float64 timestamp # Timestamp of point
二.过程实现
1.安装livox SDK2和livox_diver2和livox_viewer2
从一开始csdn看安装SDK2和diver2 这个过程可以参考livox的官方github很详细 在这里就不赘述了
livox_viewer2也是使用官方的软件源 这个在调试中有很大用处
到后面构建包 发现包中的CMakeLists.txt 和conda中的python解释器冲突了 因此在ubuntu中删除conda 重新构建包
（吐槽 ~~：使用ubuntu还是不需要使用conda了 一系列删除conda与本文无关 不需要详细描述 ~~)
再下载并学会调试viewer2 从viewer2的对单一点云的可视化出发 学习雷达点云调试和雷达基础知识的学习
调试端口的可以参考别的帖子 （别忘了修改端口！！！
2.实现不同topic下的点云显示 important
修改后的MID360_config.json:
{
"lidar_summary_info" : {
"lidar_type": 8
},
"MID360": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.50", //这里是修改后的端口
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.50",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.50",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.50",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.xx", //雷达编号
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
},
{
"ip" : "192.168.1.xx", //雷达编号
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw":0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
通过修改MID360_config.json实现同一端口输入两个点云 没有对发布的topic进行区分 这时不能进行点云的区分 就做不到融合
再从csdn的两个雷达点云显示 到rviz的实现 再到发布不同的点云
这是是阅读官方的livox_ros_driver2文档 通过修改MID360_config.json和rviz_MID360_launch.py实现的
rviz_MID360_launch.py;
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import launch
################### user configure parameters for ros2 start ###################
xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
#需要将这里的multi_topic 改为1 可以将雷达topic进行分离
multi_topic = 1 # 0-All LiDARs share the same topic, 1-One LiDAR one topic
data_src = 0 # 0-lidar, others-Invalid data src
publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
output_type = 0
frame_id = 'livox_frame'
lvx_file_path = '/home/livox/livox_test.lvx'
cmdline_bd_code = 'livox0000000001'
cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
cur_config_path = cur_path + '../config'
rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
################### user configure parameters for ros2 end #####################
#将用户配置参数打包成一个字典列表，作为 LiDAR 驱动节点的参数
livox_ros2_params = [
{"xfer_format": xfer_format},
{"multi_topic": multi_topic},
{"data_src": data_src},
{"publish_freq": publish_freq},
{"output_data_type": output_type},
{"frame_id": frame_id},
{"lvx_file_path": lvx_file_path},
{"user_config_path": user_config_path},
{"cmdline_input_bd_code": cmdline_bd_code}
]
def generate_launch_description():
livox_driver = Node(
package='livox_ros_driver2',
executable='livox_ros_driver2_node',
name='livox_lidar_publisher',
output='screen',
parameters=livox_ros2_params
)
#Rviz节点-配置rviz的启动文件
livox_rviz = Node(
package='rviz2',
executable='rviz2',
output='screen',
arguments=['--display-config', rviz_config_path]
)
return LaunchDescription([
livox_driver,
livox_rviz,
# launch.actions.RegisterEventHandler(
# event_handler=launch.event_handlers.OnProcessExit(
# target_action=livox_rviz,
# on_exit=[
# launch.actions.EmitEvent(event=launch.events.Shutdown()),
# ]
# )
# )
])
3.外参标定
外参标定也看了很多资料：这里就不给出相应的资料了
从使用livox_viewer2进行标定
到livox官方自动标定 这里可以参考dji有官方的标定 不过是四年以前了 太老了
git clone后 自动标定的时候发现官方指令不对 colcon build这个包时 有报错
然后又想用rviz标定 但是这个还值得探索 主播认为这个方法对于任何的点云位置调整 是万能的 毕竟对rviz的认识也有限
最后主播想到的办法是直接用物理方法 因为雷达坐标系与基准坐标系的相对位置确定 使用一点小学二年级的坐标变换知
识计算坐标变换矩阵出就可以了（使用matlab会比较方便） 得到欧拉角和不同坐标系坐标原点平移距离六个参数
坐标系变换肯定是先旋转再平移得到的
4.核心部分：订阅不同的topic进行点云融合
I.使用python的open3d过程实现
现在想实现功能第一想法都是python 因为python的代码易懂且好写
(1)数据采集和同步
import rospy
import message_filters
from sensor_msgs.msg import PointCloud2
def callback(lidar1_data, lidar2_data):
# 处理同步后的数据
pass
# 订阅两个 LiDAR 的 Topic
lidar1_sub = message_filters.Subscriber("/lidar1/point_cloud", PointCloud2)
lidar2_sub = message_filters.Subscriber("/lidar2/point_cloud", PointCloud2)
# 使用 ApproximateTimeSynchronizer 进行时间同步
ts = message_filters.ApproximateTimeSynchronizer([lidar1_sub, lidar2_sub], queue_size=10, slop=0.1)
ts.registerCallback(callback)
rospy.init_node("lidar_sync_node")
rospy.spin()
(2)坐标变换
import rospy
import tf2_ros
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
def transform_point_cloud(point_cloud, target_frame):
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
try:
transform = tf_buffer.lookup_transform(target_frame, point_cloud.header.frame_id, rospy.Time(0))
transformed_cloud = do_transform_cloud(point_cloud, transform)
return transformed_cloud
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
rospy.logerr("TF transform failed: %s", str(e))
return None
(3)点云合成
import open3d as o3d
# 加载两个点云
cloud1 = o3d.io.read_point_cloud("lidar1.pcd")
cloud2 = o3d.io.read_point_cloud("lidar2.pcd")
# 合并点云
merged_cloud = cloud1 + cloud2
# 保存合并后的点云
o3d.io.write_point_cloud("merged_cloud.pcd", merged_cloud)
好的 结果是发现报错有存储空间不够 Not enough memory in the buffer stream
在网上没找到解决办法后放弃使用python这一想法 转为功能强大cpp的PCL
II.使用PCL库
后面就用cpp的PCL 发现能行的通 不会报缓存不够的问题 这时就决定用cpp写
从cpp的订阅点云出发到简单的点云直接相加合成 发布在rviz中看效果
接着是加入时间戳 同步时间问题
再在cpp中加入坐标变换矩阵 最后发布
这里直接给出最后代码
merge_cloud_code.cpp:
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <Eigen/Dense> // 用于构建变换矩阵
class MergeCloudNode : public rclcpp::Node
{
public:
MergeCloudNode() : Node("merge_cloud_node")
{
// 订阅两个 Livox 点云话题
cloud1_sub_.subscribe(this, "/livox/lidar_192_168_1_xxx");
cloud2_sub_.subscribe(this, "/livox/lidar_192_168_1_xxx");
// 使用 ApproximateTime 同步器
sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(
SyncPolicy(10), cloud1_sub_, cloud2_sub_);
sync_->registerCallback(
std::bind(&MergeCloudNode::syncCallback, this, std::placeholders::_1, std::placeholders::_2));
// 发布合并后的点云
merged_cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("merged_cloud", 10);
}
private:
void syncCallback(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud1_msg,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud2_msg)
{
// 将 ROS2 PointCloud2消息转换为 PCL 点云
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*cloud1_msg, *cloud1);
pcl::fromROSMsg(*cloud2_msg, *cloud2);
// 定义变换矩阵
Eigen::Matrix4f transform1 = Eigen::Matrix4f::Identity(); // cloud1 的变换矩阵
Eigen::Matrix4f transform2 = Eigen::Matrix4f::Identity(); // cloud2 的变换矩阵
// 设置 cloud1 的变换矩阵（欧拉角 + 平移）
setTransformMatrix(transform1, roll1_, pitch1_, yaw1_, tx1_, ty1_, tz1_);
// 设置 cloud2 的变换矩阵（欧拉角 + 平移）
setTransformMatrix(transform2, roll2_, pitch2_, yaw2_, tx2_, ty2_, tz2_);
// 对点云进行坐标变换
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1_transformed(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2_transformed(new pcl::PointCloud<pcl::PointXYZI>);
pcl::transformPointCloud(*cloud1, *cloud1_transformed, transform1);
pcl::transformPointCloud(*cloud2, *cloud2_transformed, transform2);
// 合并点云
pcl::PointCloud<pcl::PointXYZI>::Ptr merged_cloud(new pcl::PointCloud<pcl::PointXYZI>);
*merged_cloud = *cloud1_transformed + *cloud2_transformed;
// 将 PCL 点云转换为 ROS 2 消息
sensor_msgs::msg::PointCloud2 merged_msg;
pcl::toROSMsg(*merged_cloud, merged_msg);
merged_msg.header.frame_id = "map"; // 设置坐标系
merged_msg.header.stamp = this->now();
// 发布合并后的点云
merged_cloud_pub_->publish(merged_msg);
RCLCPP_INFO(this->get_logger(), "Published merged cloud with %zu points.", merged_cloud->size());
}
// 设置变换矩阵（欧拉角 + 平移）
void setTransformMatrix(Eigen::Matrix4f& transform, float roll, float pitch, float yaw, float tx, float ty, float tz)
{
// 计算旋转矩阵
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
Eigen::Quaternion<float> q = yawAngle * pitchAngle * rollAngle;
Eigen::Matrix3f rotationMatrix = q.matrix();
// 设置变换矩阵的旋转部分
transform.block<3, 3>(0, 0) = rotationMatrix;
// 设置变换矩阵的平移部分
transform(0, 3) = tx;
transform(1, 3) = ty;
transform(2, 3) = tz;
}
// 定义同步策略
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> SyncPolicy;
// 订阅器
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloud1_sub_;
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloud2_sub_;
// 同步器
std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
// 发布器
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr merged_cloud_pub_;
// 变换参数（欧拉角 + 平移
// 注意两个topic对应的变换矩阵的不同
float roll1_ = 0.0f, pitch1_ = 0.0f, yaw1_ = 0.0f; // cloud1 的欧拉角（弧度）
float tx1_ = 0.0f, ty1_ = 0.0f, tz1_ = 0.0f; // cloud1 的平移（米）
float roll2_ = 0.0f, pitch2_ = 0.0f, yaw2_ = 0.0f; // cloud2 的欧拉角（弧度）
float tx2_ = 0.0f, ty2_ = 0.0f, tz2_ = 0.0f; // cloud2 的平移（米）
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MergeCloudNode>());
rclcpp::shutdown();
return 0;
}
CMakeLists.txt:
cmake_minimum_required(VERSION 3.8)
project(merge_cloud)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(message_filters REQUIRED)
find_package(Eigen3 REQUIRED) # 用于构建变换矩阵
# 添加可执行文件
add_executable(merge_cloud_node src/merge_cloud_code.cpp)
# 链接依赖库
ament_target_dependencies(merge_cloud_node rclcpp pcl_conversions pcl_ros sensor_msgs message_filters Eigen3)
# 安装可执行文件
install(TARGETS
merge_cloud_node
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
package.xml:
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>merge_cloud</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ricardo@todo.todo">ricardo</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<depend>sensor_msgs</depend>
<depend>message_filters</depend>
<depend>eigen</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
三.实现背后 也是写在最后
最后加起来其实没有500行代码
但是其实看了很多代码 也想到了很多不同的解决办法 写了几天 中间破防了几次
但是写在最后 还是又想起了那段话（请允许我加粗：
如果你觉得这件事值得做，那么多花一些时间好好地做，并不会耽误这件事。
扫一扫
1542
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
