【ROS2多激光雷达融合】基于ROS2的双2D激光雷达点云融合与遮挡剔除方案

zh路西法2026-04-26 9:36

前言

  • 最近手头一个项目，在一个中心有机械臂的麦克纳姆结构的机器人小车上配置导航，由于中心机械臂的阻挡，单一激光雷达难免会由于中心机械臂的阻碍导致实际的数据错误(读到的雷达数据是到机械臂的数据)

  • 尽管雷达的启动文件是可以设置数据的裁切角的，但是大面积的数据缺少必定会导致导航订阅错乱，因此单一雷达的方案并不适配此项目。

  • 因此本文提出一个全新的方案，通过在车头车尾对角线的位置安放两个激光雷达，通过坐标变换 + 点云融合 + ROI过滤 + 反投影生成LaserScan，最后达到如下的效果。

  • 本文使用的环境与硬件为:

    • Ubuntu22.04LTS ROS2
    • 雷达：镭神智能 N10 2D激光雷达 X 2

1 多雷达启动与安装

1-1 什么串口重命名
  • 在 Linux 中，USB设备默认映射为：
bash 复制代码 
/dev/ttyUSB0  
/dev/ttyUSB1  
/dev/ttyACM0
  • 这种方式存在两个典型问题：
    • 设备重启后编号变化
    • 更换USB口后设备名漂移
  • 因此工程上必须使用 udev 规则进行固定映射，例如：
bash 复制代码 
/dev/ttyACM0  → /dev/lidar_front  
/dev/ttyACM1  → /dev/lidar_back
  • 本质：通过设备硬件信息生成稳定软链接。
1-2 多雷达串口规则制定
  • 官方一般会提供单雷达绑定规则，但多雷达场景直接复用会冲突，因此需要重构逻辑。
  • 例如下面是官方提供的脚本
bash 复制代码 
#CP2102 串口号0001 设置别名为wheeltec_lidar
echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar.rules
#CH9102，同时系统安装了对应驱动 串口号0001 设置别名为wheeltec_lidar
echo  'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar2.rules
#CH9102，同时系统没有安装对应驱动 串口号0001 设置别名为wheeltec_lidar
echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar3.rules

service udev reload
sleep 2
service udev restart
  • 但是上述脚本仅适用单一雷达的暴力重命名，对于多激光雷达是存在问题的，因此我这里我们重新为其编写逻辑，一般情况下，雷达对应的设备名和USB拓扑会是如下格式
设备 USB拓扑 结论
ttyACM0 1-1.2.3:1.0 雷达A
ttyACM1 1-1.1:1.4 ️ 不是雷达（可能是控制/虚拟口）
ttyACM2 1-1.2.2:1.0 雷达B
  • 核心规则：

只有以 :1.0 结尾的USB设备才认为是雷达主接口

1-2-1 自动识别脚本实现
bash 复制代码 
#!/bin/bash
# 此脚本用于自动检测多雷达设备，并给雷达进行/dev/ttyACM*的重映射
set -e

RULE_FILE="/etc/udev/rules.d/99-dual-lidar.rules"

echo "[INFO] Scanning ttyACM devices..."

ACM_LIST=($(ls /dev/ttyACM* 2>/dev/null || true))

if [ ${#ACM_LIST[@]} -lt 2 ]; then
    echo "[ERROR] Not enough ACM devices"
    exit 1
fi

echo "[INFO] Raw devices:"
printf '%s\n' "${ACM_LIST[@]}"

# -----------------------------
# 判断是否雷达
# -----------------------------
is_lidar() {
    local dev=$1
    local kernel
    kernel=$(udevadm info -a -n "$dev" | grep KERNELS | head -n 1 | awk -F'"' '{print $2}')

    # 必须是 :1.0 结尾（雷达常见）
    if [[ "$kernel" == *":1.0"* ]]; then
        echo "$kernel"
        return 0
    fi

    return 1
}

LIDAR_DEVICES=()
LIDAR_KERNELS=()

# -----------------------------
# 筛选雷达
# -----------------------------
for dev in "${ACM_LIST[@]}"; do
    kernel=$(udevadm info -a -n "$dev" | grep KERNELS | head -n 1 | awk -F'"' '{print $2}')

    echo "[CHECK] $dev -> $kernel"

    if [[ "$kernel" == *":1.0"* ]]; then
        LIDAR_DEVICES+=("$dev")
        LIDAR_KERNELS+=("$kernel")
        echo "[OK] LiDAR detected: $dev"
    else
        echo "[SKIP] Not lidar: $dev"
    fi
done

# -----------------------------
# 检查是否找到2个雷达
# -----------------------------
if [ ${#LIDAR_KERNELS[@]} -lt 2 ]; then
    echo "[ERROR] Less than 2 lidar devices found"
    exit 1
fi

FRONT=${LIDAR_KERNELS[0]}
BACK=${LIDAR_KERNELS[1]}

echo "[INFO] FRONT = $FRONT"
echo "[INFO] BACK  = $BACK"

# -----------------------------
# 写 udev
# -----------------------------
cat <<EOF > $RULE_FILE
# Auto dual lidar mapping (rule-based detection)

KERNELS=="$FRONT", SYMLINK+="lidar_front", MODE="0777", GROUP="dialout"
KERNELS=="$BACK",  SYMLINK+="lidar_back",  MODE="0777", GROUP="dialout"

EOF

# -----------------------------
# reload
# -----------------------------
udevadm control --reload-rules
udevadm trigger

sleep 2

echo "[RESULT]"
ls -l /dev/lidar* 2>/dev/null || echo "no symlinks yet"

echo "[DONE]"
  • 需要注意的是上述雷达重命名会随机分配lidar_front和lidar_back，因此具体哪个放前面哪个放后面需要根据分配后的脚本进行测试
  • 执行上述脚本后，以后我们就可以通过/dev/lidar_front和/dev/lidar_back，即便是更换了不同USB接口，这个规则仍然生效。
1-3 启动文件配置
  • 在配置上述udev规则后，我们就可以更改官方雷达启动节点的配置文件，就可以顺利启动雷达 了
  • 以镭神N10雷达启动的配置脚本为例子,我们只需要修改serial_port分别为/dev/lidar_front和/dev/lidar_back即可，同时我们需要分别给不同的雷达配置不同的坐标系和话题名称，以便后面我们使用
yaml 复制代码 
x10:  
  lslidar_driver_node:
    ros__parameters:
      lidar_type: "X10"           # 雷达类型
      lidar_model: "N10"          # 雷达型号 M10 M10GPS M10P N10 N10Plus N301
      # serial_port: ""             # 雷达连接串口名称 使用网口请将值改为空
      serial_port: "/dev/lidar_front"             # 雷达连接串口名称 使用网口请将值改为空
      device_ip: "192.168.1.200"  # 雷达IP地址
      msop_port: 2368             # 雷达目的数据端口
      difop_port: 2369            # 雷达目的设备端口
      packet_rate: 188.0          # PCAP文件回放速率，离线解析PCAP数据时使用

      # 点云处理参数
      frame_id: "laser_front"      # 坐标系名称
      pointcloud_topic: "lslidar_point_cloud_front"  # 点云话题名
      laserscan_topic: "/scan_front"     # 发布 LaserScan 数据话题名称
1-4 tf2静态坐标变换
  • 双雷达安装在车体对角，因此必须建立统一坐标系，结构如下：
bash 复制代码 
base_link
  └── laser_link
        ├── laser_front
        └── laser_back
  • 需要注意的是，为了节省算力这里我们发布的是静态坐标变换，因此要保证两个对角激光雷达的安装都是正面超前的！！否则如果两个雷达一正一反，使用静态坐标变换后有一个雷达生成的数据会跟着坐标轴一起旋转（当然你可以使用动态坐标变换来解决这个问题
  • 为了在双雷达节点发布的同时发布tf2坐标变换，我们可以把双雷达的启动和tf2坐标变换绑在一起，写成一个launch.py节点
python 复制代码 
Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['0', '0', '0', '0', '0', '0',
               'base_link', 'laser_link']
),

Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['0.10', '0.11', '0', '0', '0', '0',
               'laser_link', 'laser_back']
),

Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['-0.10', '-0.11', '0', '0', '0', '0',
               'laser_link', 'laser_front']
),
  • 完整launch文件见后面
  • 相关的坐标参数请根据实际的相对安装位置设置，这里提供注释
python 复制代码 
arguments=[
                '-0.10', '-0.11', '0',
                '0', '0', '0',
                'laser_link', 'laser_front'
            ]
  • tf2静态坐表变换的参数分别是平移和旋转x y z roll pitch yaw
  • 然后后面两个参数是： 父坐标系（parent frame），子坐标系（child frame）
1-5 启动测试
  • -RViz2中设置 global frame = odom
  • 可观察到：
    • 前雷达遮挡区域
    • 后雷达盲区补全
    • 中心机械臂干扰明显存在

2 多雷达融合实现

2-1 整体任务分析
  • 我们简单分析一下我们接下来需要做的内容
    1. 订阅双雷达数据
    2. 把双雷达数据从各自的坐标系上变换回车辆中心的laser_link
    3. 将双雷达数据进行融合即可
  • 看起来很简单，这是却存在一个问题，我们来看一下基础雷达的数据结构，在终端输入ros2 interface show sensor_msgs/msg/LaserScan查询基础的雷达信息：
bash 复制代码 
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

std_msgs/Header header # timestamp in the header is the acquisition time of
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id
                             # the first ray in the scan.
                             #
                             # in frame frame_id, angles are measured around
                             # the positive Z axis (counterclockwise, if Z is up)
                             # with zero angle being forward along the x axis

float32 angle_min            # start angle of the scan [rad]
float32 angle_max            # end angle of the scan [rad]
float32 angle_increment      # angular distance between measurements [rad]

float32 time_increment       # time between measurements [seconds] - if your scanner
                             # is moving, this will be used in interpolating position
                             # of 3d points
float32 scan_time            # time between scans [seconds]

float32 range_min            # minimum range value [m]
float32 range_max            # maximum range value [m]

float32[] ranges             # range data [m]
                             # (Note: values < range_min or > range_max should be discarded)
float32[] intensities        # intensity data [device-specific units].  If your
                             # device does not provide intensities, please leave
                             # the array empty.
  • 可以看到雷达的基础数据存储是一个循环的数组，简单的进行范围截取组合必然会出现大量的问题，因此我们不能直接进行合并。
  • 因此本文提供的方案是，将双雷达的数据转为2维世界的点云数据，这时候，数据合并和范围截取的事情都变得非常方便，最后在将合并后的点云数据转为雷达数据发布即可。
2-2 完整代码实现
  • 我们先看完整实现再仔细讲解
cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <mutex>
#include <cmath>
#include <limits>
#include <laser_geometry/laser_geometry.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
class ScanFusionNode : public rclcpp::Node
{
public:
    ScanFusionNode() : Node("scan_fusion_node")
    {
        front_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan_front", 10,
            std::bind(&ScanFusionNode::frontCallback, this, std::placeholders::_1));

        back_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan_back", 10,
            std::bind(&ScanFusionNode::backCallback, this, std::placeholders::_1));

        scan_pub_ = create_publisher<sensor_msgs::msg::LaserScan>("/scan", 10);

       
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&ScanFusionNode::timerCallback, this));
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        cloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("/scan_fused_cloud", 10);
        std::cout<<"ScanFusionNode start!"<<std::endl;
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr front_sub_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr back_sub_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
    rclcpp::TimerBase::SharedPtr timer_;

    sensor_msgs::msg::LaserScan front_scan_;
    sensor_msgs::msg::LaserScan back_scan_;

    laser_geometry::LaserProjection projector_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    sensor_msgs::msg::PointCloud2 cloud_front_, cloud_back_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
    const float x_min = -0.3f;
    const float x_max =  0.3f;
    const float y_min = -0.3f;
    const float y_max =  0.3f;

private:
    void frontCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        try {
            projector_.projectLaser(*msg, cloud_front_);
        } catch (...) {
            RCLCPP_WARN(this->get_logger(), "Front projection failed");
        }
    }

    void backCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        try {
            projector_.projectLaser(*msg, cloud_back_);
        } catch (...) {
            RCLCPP_WARN(this->get_logger(), "Back projection failed");
        }
    }
   void timerCallback()
    {
        sensor_msgs::msg::PointCloud2 cloud_front_tf, cloud_back_tf;
      

        try {
            tf_buffer_->transform(cloud_front_, cloud_front_tf, "laser_link");
            tf_buffer_->transform(cloud_back_, cloud_back_tf, "laser_link");
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "TF failed: %s", ex.what());
            return;
        }

        
        sensor_msgs::msg::PointCloud2 fused_cloud = cloud_front_tf;

        fused_cloud.data.insert(
            fused_cloud.data.end(),
            cloud_back_tf.data.begin(),
            cloud_back_tf.data.end()
        );

        fused_cloud.width += cloud_back_tf.width;
        filterCloud(fused_cloud);
        fused_cloud.header.frame_id = "laser_link";
        fused_cloud.header.stamp = this->now();

        
        cloud_pub_->publish(fused_cloud);
        sensor_msgs::msg::LaserScan scan_out =CloudToScan(fused_cloud, "laser_link");

        scan_pub_->publish(scan_out);
    }
    sensor_msgs::msg::LaserScan CloudToScan(
    const sensor_msgs::msg::PointCloud2 &cloud_msg,
    const std::string &frame_id,
    float angle_min = -M_PI,
    float angle_max = M_PI,
    float angle_increment = 0.01f,
    float range_min = 0.1f,
    float range_max = 10.0f)
    {
        // ===== 1. 转 PCL =====
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(cloud_msg, cloud);

        // ===== 2. 初始化 LaserScan =====
        sensor_msgs::msg::LaserScan scan;
        scan.header.frame_id = frame_id;

        scan.angle_min = angle_min;
        scan.angle_max = angle_max;
        scan.angle_increment = angle_increment;

        scan.range_min = range_min;
        scan.range_max = range_max;

        int size = static_cast<int>((angle_max - angle_min) / angle_increment);
        scan.ranges.assign(size, std::numeric_limits<float>::infinity());

        // ===== 3. 点云 → scan =====
        for (const auto &p : cloud.points)
        {
            if (!std::isfinite(p.x) || !std::isfinite(p.y))
                continue;

            float range = std::sqrt(p.x * p.x + p.y * p.y);
            if (range < range_min || range > range_max)
                continue;

            float angle = std::atan2(p.y, p.x);

            int idx = static_cast<int>((angle - angle_min) / angle_increment);

            if (idx >= 0 && idx < size)
            {
                scan.ranges[idx] = std::min(scan.ranges[idx], range);
            }
        }

        scan.header.stamp = rclcpp::Clock().now();
        return scan;
    }
    void filterCloud(sensor_msgs::msg::PointCloud2 &cloud)
    {

        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(cloud, *pcl_cloud);
        size_t before_size = pcl_cloud->size(); 
        if(before_size==0)
             RCLCPP_WARN(this->get_logger(),"[Filter] No points recieve!");
        // CropBox
        pcl::CropBox<pcl::PointXYZ> crop;
        crop.setInputCloud(pcl_cloud);

        crop.setMin(Eigen::Vector4f(x_min, y_min, -1.0, 1.0));
        crop.setMax(Eigen::Vector4f(x_max, y_max,  1.0, 1.0));

        //关键：删除框内点
        crop.setNegative(true);

        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
        crop.filter(*filtered);

        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*filtered, *filtered, indices);
        size_t after_size = filtered->size(); 
        // 因为二者之间有车的框体在，所有一定会有点云被过滤，反之如果没有，那就是炸了
        if (after_size >= before_size)
        {
            RCLCPP_WARN(this->get_logger(),
                "[Filter] No points removed! before=%ld after=%ld",
                before_size, after_size);
        }

        pcl::toROSMsg(*filtered, cloud);
    }

};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ScanFusionNode>());
    rclcpp::shutdown();
    return 0;
}
  • 然后我们分别看一下每一个区块的实现逻辑
2-3 雷达订阅与点云转换
cpp 复制代码 
void frontCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
	try {
		projector_.projectLaser(*msg, cloud_front_);
	} catch (...) {
		RCLCPP_WARN(this->get_logger(), "Front projection failed");
	}
}

void backCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
	try {
		projector_.projectLaser(*msg, cloud_back_);
	} catch (...) {
		RCLCPP_WARN(this->get_logger(), "Back projection failed");
	}
}
  • 在两个雷达订阅回调函数中，我们直接调用LaserProjection的projectLaser函数，此函数会将二维雷达转换为三维的点云数据
2-4 坐标变换
cpp 复制代码 
sensor_msgs::msg::PointCloud2 cloud_front_tf, cloud_back_tf;
    
try {
	tf_buffer_->transform(cloud_front_, cloud_front_tf, "laser_link");
	tf_buffer_->transform(cloud_back_, cloud_back_tf, "laser_link");
} catch (tf2::TransformException &ex) {
	RCLCPP_WARN(this->get_logger(), "TF failed: %s", ex.what());
	return;
}
  • 这里调用了tf2进行坐标变换，把各自雷达的点云信息从各自的坐标系转换到中心雷达坐标系laser_link
  • 这一步的转换非常关键，否则后续点云会无法正确拼接
2-5 点云融合（核心操作）
cpp 复制代码 
sensor_msgs::msg::PointCloud2 fused_cloud = cloud_front_tf;
fused_cloud.data.insert(
    fused_cloud.data.end(),
    cloud_back_tf.data.begin(),
    cloud_back_tf.data.end()
);
fused_cloud.width += cloud_back_tf.width;
  • 这里我们直接进行二进制data拼接，将前后两个雷达数据进行拼接，同时我们需要更新新的雷达点数width
  • 这里拼接的前提必须保证前后点云内存结构完全一致，由于我们使用的是完全相同的两个雷达，因此这里直接拼接即可
2-6 点云过滤
  • 我们开头的前言提到过，本项目中两个对角激光雷达的中心连线处是一个机械臂，因此我们需要过滤掉中心的点云数据
  • 相比与使用雷达数据进行角度剔除可能造成的数据丢失，点云范围剔除就可以在不丢失其他关键点的前提下，精准删除特定范围的点云数据
  • 这里我们使用到了点云处理库来辅助我们进行裁切
cpp 复制代码 
void filterCloud(sensor_msgs::msg::PointCloud2 &cloud)
{

	pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromROSMsg(cloud, *pcl_cloud);
	size_t before_size = pcl_cloud->size(); 
	if(before_size==0)
		 RCLCPP_WARN(this->get_logger(),"[Filter] No points recieve!");
	// CropBox
	pcl::CropBox<pcl::PointXYZ> crop;
	crop.setInputCloud(pcl_cloud);

	crop.setMin(Eigen::Vector4f(x_min, y_min, -1.0, 1.0));
	crop.setMax(Eigen::Vector4f(x_max, y_max,  1.0, 1.0));

	//关键：删除框内点
	crop.setNegative(true);

	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
	crop.filter(*filtered);

	std::vector<int> indices;
	pcl::removeNaNFromPointCloud(*filtered, *filtered, indices);
	size_t after_size = filtered->size(); 
	// 因为二者之间有车的框体在，所有一定会有点云被过滤，反之如果没有，那就是炸了
	if (after_size >= before_size)
	{
		RCLCPP_WARN(this->get_logger(),
			"[Filter] No points removed! before=%ld after=%ld",
			before_size, after_size);
	}

	pcl::toROSMsg(*filtered, cloud);
}
  • 通过上述函数，我们将构建一个ROI盒子,这个盒子的范围我们设置为车体的大小
bash 复制代码 
x: [-0.3, 0.3]
y: [-0.3, 0.3]
  • 通过filterCloud函数后，我们将删除以laser_link为中心ROI大小的所有点云信息
2-7 点云转雷达

  • 通过点云裁切函数，我们已经可以发布合并后的点云数据了

  • 可以看到，两个雷达的数据被融合，切车辆中心的点云数据已经被清理(上图车辆右上角的点云是障碍物)

  • 那么我们最后只需要把这个点云数据转换为雷达数据即可

  • 这里提一嘴，ROS2是有点云转雷达的库pointcloud_to_laserscan，但是这里我们直接自己实现即可

cpp 复制代码 
sensor_msgs::msg::LaserScan CloudToScan(
const sensor_msgs::msg::PointCloud2 &cloud_msg,
const std::string &frame_id,
float angle_min = -M_PI,
float angle_max = M_PI,
float angle_increment = 0.01f,
float range_min = 0.1f,
float range_max = 10.0f)
{
	// ===== 1. 转 PCL =====
	pcl::PointCloud<pcl::PointXYZ> cloud;
	pcl::fromROSMsg(cloud_msg, cloud);

	// ===== 2. 初始化 LaserScan =====
	sensor_msgs::msg::LaserScan scan;
	scan.header.frame_id = frame_id;

	scan.angle_min = angle_min;
	scan.angle_max = angle_max;
	scan.angle_increment = angle_increment;

	scan.range_min = range_min;
	scan.range_max = range_max;

	int size = static_cast<int>((angle_max - angle_min) / angle_increment);
	scan.ranges.assign(size, std::numeric_limits<float>::infinity());

	// ===== 3. 点云 → scan =====
	for (const auto &p : cloud.points)
	{
		if (!std::isfinite(p.x) || !std::isfinite(p.y))
			continue;

		float range = std::sqrt(p.x * p.x + p.y * p.y);
		if (range < range_min || range > range_max)
			continue;

		float angle = std::atan2(p.y, p.x);

		int idx = static_cast<int>((angle - angle_min) / angle_increment);

		if (idx >= 0 && idx < size)
		{
			scan.ranges[idx] = std::min(scan.ranges[idx], range);
		}
	}

	scan.header.stamp = rclcpp::Clock().now();
	return scan;
}
  • 这里我们进行了初始化 scan，分辨率设置，range数组初始化，点云投影回2D以及索引映射
  • 核心内容是保留最近点（标准激光模拟逻辑）
cpp 复制代码 
scan.ranges[idx] = min(existing, range);
2-8 最终效果
  • 我们可以把雷达融合节点加入一开始的launch.py，一并启动
python 复制代码 
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

import os
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():

    lslidar_pkg = get_package_share_directory('lslidar_driver')
    fusion_pkg = get_package_share_directory('dual_lidar_fusion')

    return LaunchDescription([

        # ========================
        # front 雷达
        # ========================
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(lslidar_pkg, 'launch', 'lsn10_launch_front.py')
            )
        ),

        # ========================
        # back 雷达
        # ========================
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(lslidar_pkg, 'launch', 'lsn10_launch_back.py')
            )
        ),

        # ========================
        # base_link -> laser_link (0 0 0)
        # ========================
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=[
                '0', '0', '0',
                '0', '0', '0',
                'base_link', 'laser_link'
            ]
        ),

        # ========================
        # laser_link -> 
        # =======================
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=[
                '0.10', '0.11', '0',
                '0', '0', '0',
                'laser_link', 'laser_back'
            ]
        ),

        # ========================
        # laser_link -> 
        # ========================
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=[
                '-0.10', '-0.11', '0',
                '0', '0', '0',
                'laser_link', 'laser_front'
            ]
        ),
         # ========================
        # 1. 融合点云节点
        # ========================
        Node(
            package='dual_lidar_fusion',
            executable='scan_fusion_node',
            name='scan_fusion_node',
            output='screen'
        ),

    ])
  • 可以看到，我们的雷达数据正常发布

完整效果图展示

  • 机器人坐标展示

  • 前雷达

  • 后雷达

  • 融合后的点云

  • 融合后的雷达

小结

  • 本文针对带中心机械臂的麦克纳姆底盘提出了一种双2D激光雷达融合方案，通过 udev 实现设备稳定映射，结合 tf2 构建统一坐标系，并将 LaserScan 转换为 PointCloud2 后进行空间融合与ROI过滤，最终再反投影生成新的 LaserScan 数据流，从而有效解决单雷达遮挡导致的感知缺失问题，实现了稳定可靠的全局环境感知结构。
  • 如有错误，欢迎支持!
  • 感谢支持!
相关推荐
qq_413847401 小时前
开发者工具怎么看HTML_Elements面板使用指南【操作】
jvm·数据库·python
qq_372906931 小时前
mysql如何设置密码过期策略_mysql default_password_lifetime
jvm·数据库·python
七颗糖很甜1 小时前
开源雷达NEXRAD Level 3 数据完整获取与 Python 处理教程
大数据·python·算法
SuAluvfy1 小时前
PyTorch 基础：数据操作与数据预处理
人工智能·pytorch·python
楼田莉子2 小时前
CMake学习：动态库场景下的应用
c++·后端·学习·软件构建
jingshaoqi_ccc2 小时前
使用QT6创建一个可编辑的表格并导出和载入
c++·qt·表格
ydmy2 小时前
Embedding层（个人理解）
python·深度学习·embedding
qq_330037992 小时前
mysql在高并发下如何优化索引更新_mysql锁策略与调整
jvm·数据库·python
u0109147602 小时前
如何排查SQL存储过程内存溢出_优化大数据量临时表使用
jvm·数据库·python
热门推荐
01GitHub 镜像站点022026年4月技术前沿：AI大模型爆发、智能体革命与量子安全新纪元03近期有什么ai的新消息，新动态？ 2026.4月042026年4月AI大事件深度解读：大模型竞争进入“深水区“052026年AI前瞻：量子AI、具身智能与科学发现的新纪元06AI Weekly | 2026年4月第二周 · GitHub热门项目与AI发展趋势深度解析07codex app每次打开重连5次Reconnecting问题解决082026 年 AI 辅助编程工具全景对比：Copilot、Cursor、Claude Code 与 Codex 深度解析09CC-Switch & Claude 基于 Linux 服务器安装使用指南102026 年 AI 编程助手全面对比评测：Cursor vs Copilot vs Claude Code vs GitHub Copilot Free