RDK X3 + N10激光雷达 SLAM建图完整踩坑记录

前言

最近在做智能车竞赛，需要在地平线RDK X3开发板上实现SLAM建图功能。本来以为是个简单的事情，结果从拿到雷达到成功建图，整整折腾了一个多星期。中间遇到了无数稀奇古怪的问题，官方驱动跑不起来、串口数据读不到、坐标系对不上、地图显示不出来……

这篇文章详细记录了整个过程，包括每一步的操作、遇到的问题、以及最终的解决方案。希望能帮到同样在做智能车竞赛或者ROS2开发的朋友。

硬件环境：

• 主控板：地平线 RDK X3（X3派），ARM架构，4GB内存
• 激光雷达：镭神智能 N10（串口版），360度扫描，10Hz
• 电机驱动：STM32 + origincar底盘（阿克曼转向模型）
• 连接方式：N10接/dev/ttyACM1，STM32接/dev/ttyACM0

软件环境：

• 操作系统：Ubuntu 20.04（X3官方镜像）
• ROS版本：ROS2 Foxy + TogetheROS（地平线定制版）
• SLAM算法：Google Cartographer
• 可视化工具：coStudio（类似Foxglove Studio）
• 雷达驱动：自研Python版（替代官方C++驱动）

---

一、硬件接线

1.1 N10雷达接线

N10雷达通过USB转串口连接到X3。雷达是串口版本，波特率230400，数据格式8N1。

接线很简单，就是把雷达的TX/RX接到USB转串口模块的RX/TX，然后USB插到X3上。插上后系统会自动识别为/dev/ttyACM1。

注意： 如果X3上同时插了STM32和其他USB设备，设备号可能会变。可以用ls -la /dev/ttyACM*查看当前的设备号。

雷达线色	功能	接口
红色	5V电源	USB供电
黑色	GND	USB供电
绿色	TX	USB转串口RX
白色	RX	USB转串口TX

1.2 STM32电机驱动接线

STM32通过USB连接到X3，设备号为/dev/ttyACM0，波特率115200。STM32负责控制电机和发布里程计数据（/odom话题）。

1.3 设备号确认

插好所有USB设备后，用以下命令确认设备号：

ls -la /dev/ttyACM*

正常情况下应该看到：

• /dev/ttyACM0 → STM32电机驱动
• /dev/ttyACM1 → N10激光雷达

如果设备号反了，需要修改驱动的配置文件。

---

二、雷达驱动编译（踩坑重点）

这部分是整个过程中最坑的地方。镭神官方提供了ROS2 Foxy版本的lslidar驱动，但在X3上编译遇到了三个致命问题，每个都花了很长时间才定位到。

2.1 问题一：interface_selection硬编码为"net"

现象： 驱动编译成功，启动后显示"Lidar is N10"、“Initialised lslidar without error”，但就是没有/scan数据发布。用ros2 topic echo /scan什么都看不到。

排查过程：

1. 先检查串口是否有数据：cat /dev/ttyACM1 | xxd，发现有数据（帧头0xA5 0x5A）
2. 再检查驱动是否在读取串口：strace -p <pid> -e trace=read，发现驱动根本没有read系统调用
3. 最后看源码，发现第69行：

// lslidar_driver.cc 第69行
interface_selection = std::string("net");  // 默认网口模式！

原因： 虽然配置文件lsn10.yaml里写了interface_selection: serial，但源码里硬编码为"net"。ROS2的参数机制是先用declare_parameter声明默认值，然后用get_parameter读取配置文件的值。但源码里第69行直接赋值为"net"，覆盖了后面的参数读取。

修复：

// 第69行，改为：
interface_selection = std::string("serial");

// 第97行，改为：
this->declare_parameter<std::string>("interface_selection", "serial");

2.2 问题二：declare_parameter重复声明崩溃

现象： 修复了interface_selection后，驱动启动时直接崩溃，报错"parameter already declared"。

排查过程：

1. 查看崩溃日志，发现是open_serial()函数里的declare_parameter导致的
2. ROS2 Foxy的declare_parameter在参数已声明时会抛出异常
3. 配置文件已经声明了serial_port_参数，open_serial()里又声明了一次

原因： ROS2 Foxy的参数机制是全局的，同一个参数不能声明两次。配置文件通过--params-file传入时已经声明了所有参数，代码里再声明就会冲突。

修复：

// open_serial()函数里，把declare_parameter改为try-catch
void LslidarDriver::open_serial()
{
    diagnostics.setHardwareID("Lslidar");
    int code = 0;
    serial_port_ = std::string("/dev/ttyACM1");  // 直接写死，避免参数冲突
    serial_ = LSIOSR::instance(serial_port_, baud_rate_);
    code = serial_->init();
    if (code != 0)
    {
        printf("open_port %s ERROR !\n", serial_port_.c_str());
        rclcpp::shutdown();
        exit(0);
    }
    printf("open_port %s OK !\n", serial_port_.c_str());
}

2.3 问题三：CRC校验不匹配

现象： 修复了前两个问题后，驱动能打开串口了，但还是没有/scan数据。加了调试日志后发现"CRC failed: expected 0xE3, got 0x08"。

排查过程：

1. 检查串口数据格式：cat /dev/ttyACM1 | xxd，数据帧格式正确（58字节，帧头0xA5 0x5A）
2. 检查CRC算法：N10用的是简单的累加和校验
3. 对比实际数据和计算结果，发现不匹配

原因： N10雷达有多个固件版本，不同版本的CRC算法可能不同。我们拿到的雷达固件和驱动期望的CRC算法不一致。

临时修复：禁用CRC校验

// 第616-619行，改为：
if (lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P")
{
    // CRC check disabled for firmware compatibility
    if (false)
        return 0;
}

注意： 禁用CRC校验后，如果有数据损坏，驱动会尝试解析错误数据。但在实际测试中，数据质量还是很好的，没有出现问题。

2.4 Python驱动替代方案

由于官方C++驱动问题太多，我决定写一个Python版本的N10驱动。Python驱动的好处是：

1. 不需要编译，改代码马上生效
2. 串口读取更简单，不需要复杂的线程管理
3. 可以直接用serial库，兼容性更好

完整代码：

#!/usr/bin/env python3
"""
N10 LSLidar Python驱动
读取/dev/ttyACM1串口数据，发布/scan话题
数据帧格式：58字节，帧头0xA5 0x5A，每帧16个点
每个点3字节：2字节距离(mm) + 1字节强度
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from sensor_msgs.msg import LaserScan
import serial
import math

class N10Lidar(Node):
    def __init__(self):
        super().__init__('n10_lidar')
        
        # N10参数
        self.serial_port = '/dev/ttyACM1'
        self.baud_rate = 230400
        self.packet_size = 58      # 每帧58字节
        self.points_per_packet = 16 # 每帧16个点
        self.total_points = 2000    # 一圈总共2000个点
        
        # 发布者（QoS: RELIABLE，这是关键！）
        qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.VOLATILE,
            depth=10
        )
        self.pub = self.create_publisher(LaserScan, '/scan', qos)
        
        # 打开串口
        self.get_logger().info(f'Opening {self.serial_port} at {self.baud_rate}')
        self.ser = serial.Serial(
            port=self.serial_port,
            baudrate=self.baud_rate,
            bytesize=serial.EIGHTBITS,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            timeout=1.0
        )
        self.get_logger().info(f'Serial port opened: {self.ser.name}')
        
        # 数据存储
        self.points = []
        
        # 定时读取（100Hz）
        self.timer = self.create_timer(0.01, self.read_serial)
    
    def read_serial(self):
        try:
            # 读取一帧数据（58字节）
            data = self.ser.read(self.packet_size)
            if len(data) < self.packet_size:
                return
            
            # 检查帧头（0xA5 0x5A）
            if data[0] != 0xA5 or data[1] != 0x5A:
                self.ser.read(1)  # 重新同步
                return
            
            # 解析角度（字节5-6是起始角度，字节55-56是结束角度）
            angle_start = (data[5] * 256 + data[6]) / 100.0
            angle_end = (data[55] * 256 + data[56]) / 100.0
            
            # 解析16个点（每个点3字节：2字节距离+1字节强度）
            for i in range(self.points_per_packet):
                offset = 7 + i * 3
                distance = (data[offset] * 256 + data[offset + 1]) / 1000.0  # mm转m
                intensity = data[offset + 2]
                
                # 计算这个点的角度（线性插值）
                if self.points_per_packet > 1:
                    angle = angle_start + (angle_end - angle_start) * i / self.points_per_packet
                else:
                    angle = angle_start
                
                # 归一化角度到0-360度
                if angle < 0:
                    angle += 360.0
                elif angle >= 360.0:
                    angle -= 360.0
                
                self.points.append((math.radians(angle), distance, intensity))
            
            # 收集够一帧（2000个点）就发布
            if len(self.points) >= self.total_points:
                self.publish_scan()
                self.points = []
                
        except Exception as e:
            self.get_logger().error(f'Serial error: {e}')
    
    def publish_scan(self):
        if not self.points:
            return
        
        scan = LaserScan()
        scan.header.stamp = self.get_clock().now().to_msg()
        scan.header.frame_id = 'laser'  # 坐标系名称
        
        scan.angle_min = 0.0
        scan.angle_max = 2.0 * math.pi
        scan.angle_increment = 2.0 * math.pi / self.total_points
        scan.time_increment = 0.0
        scan.scan_time = 0.1  # 10Hz
        scan.range_min = 0.15  # 最小测距15cm
        scan.range_max = 100.0 # 最大测距100m
        
        # 初始化数组
        ranges = [float('inf')] * self.total_points
        intensities = [0.0] * self.total_points
        
        # 填充数据
        for angle, distance, intensity in self.points:
            idx = int(angle / scan.angle_increment) % self.total_points
            ranges[idx] = distance
            intensities[idx] = float(intensity)
        
        scan.ranges = ranges
        scan.intensities = intensities
        
        self.pub.publish(scan)
        self.get_logger().info(f'Published scan with {len(self.points)} points')

def main():
    rclpy.init()
    node = N10Lidar()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

关键点说明：

1. QoS设置：必须用RELIABLE，不能用BEST_EFFORT。cartographer默认用SensorDataQoS（BEST_EFFORT），但实测发现用RELIABLE更稳定。

2. 帧头同步：N10的数据帧头是0xA5 0x5A，如果读取位置不对，需要跳过一个字节重新同步。

3. 角度计算：每帧有起始角度和结束角度，16个点在之间线性插值。

4. 数据积累：N10一圈大约需要125帧（2000/16），收集够2000个点后才发布一次完整的扫描。

---

三、Cartographer配置

3.1 配置文件（lslidar_2d.lua）

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  
  -- 坐标系配置
  map_frame = "map",                    -- 地图坐标系
  tracking_frame = "base_footprint",    -- 跟踪小车底盘
  published_frame = "odom_combined",    -- 发布的坐标系
  odom_frame = "odom_combined",         -- 里程计坐标系
  
  -- 关键配置！必须为true才发布map帧
  provide_odom_frame = true,
  
  -- 其他配置
  publish_frame_projected_to_2d = false,
  use_odometry = false,                 -- 不使用里程计（我们的odom不准）
  use_nav_sat = false,
  use_landmarks = false,
  
  -- 雷达配置
  num_laser_scans = 1,                  -- 一个激光雷达
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  
  -- 时间配置
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 1.0,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  
  -- 采样率
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

-- 使用2D建图
MAP_BUILDER.use_trajectory_builder_2d = true

-- 2D建图参数
TRAJECTORY_BUILDER_2D.min_range = 0.15      -- 最小测距
TRAJECTORY_BUILDER_2D.max_range = 10.0      -- 最大测距
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false  -- 不使用IMU

-- 位姿图优化
POSE_GRAPH.optimization_problem.huber_scale = 1e1
POSE_GRAPH.optimize_every_n_nodes = 50
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

关键配置说明：

1. provide_odom_frame = true：这个配置非常重要！如果设为false，cartographer不会发布map → odom_combined的tf变换，导致coStudio里看不到map参考系。

2. tracking_frame = "base_footprint"：cartographer会跟踪这个坐标系。必须确保tf树里有这个坐标系。

3. use_odometry = false：我们的STM32里程计不太准，所以不使用。如果里程计准的话，设为true可以提高建图精度。

4. use_imu_data = false：我们的小车没有IMU，所以关闭。

3.2 坐标系关系

整个系统的坐标系关系如下：

map → odom_combined → base_footprint → laser

• map：地图坐标系，由cartographer发布，是全局固定坐标系
• odom_combined：里程计坐标系，由STM32发布，会随时间漂移
• base_footprint：小车底盘坐标系，是odom_combined的子坐标系
• laser：雷达坐标系，通过静态变换连接到base_footprint

tf变换来源：

• map → odom_combined：cartographer发布
• odom_combined → base_footprint：odom_to_tf.py发布（读取/odom话题）
• base_footprint → laser：static_transform_publisher发布（静态变换）

---

四、其他Python脚本

4.1 odom_to_tf.py（里程计转tf）

这个脚本读取STM32发布的/odom话题，转换成tf变换：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped

class OdomToTf(Node):
    def __init__(self):
        super().__init__("odom_to_tf")
        self.br = TransformBroadcaster(self)
        self.sub = self.create_subscription(Odometry, "/odom", self.cb, 10)
    
    def cb(self, msg):
        t = TransformStamped()
        t.header.stamp = msg.header.stamp
        t.header.frame_id = msg.header.frame_id      # odom_combined
        t.child_frame_id = msg.child_frame_id        # base_footprint
        t.transform.translation.x = msg.pose.pose.position.x
        t.transform.translation.y = msg.pose.pose.position.y
        t.transform.translation.z = msg.pose.pose.position.z
        t.transform.rotation = msg.pose.pose.orientation
        self.br.sendTransform(t)

rclpy.init()
node = OdomToTf()
rclpy.spin(node)

4.2 stm32_odom.py（STM32里程计读取）

这个脚本直接读取STM32的串口数据，发布/odom话题（替代官方的origincar_base驱动）：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
import serial
import math

class STM32Odom(Node):
    def __init__(self):
        super().__init__('stm32_odom')
        
        self.serial_port = '/dev/ttyACM0'
        self.baud_rate = 115200
        self.frame_size = 24
        self.frame_header = 0x7B
        self.frame_tail = 0x7D
        
        # 发布者
        self.odom_pub = self.create_publisher(Odometry, '/odom', 10)
        
        # 打开串口
        self.ser = serial.Serial(
            port=self.serial_port,
            baudrate=self.baud_rate,
            bytesize=serial.EIGHTBITS,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            timeout=0.1
        )
        self.get_logger().info(f'Serial port opened: {self.ser.name}')
        
        # 位置和速度
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
        self.vx = 0.0
        self.vy = 0.0
        self.vz = 0.0
        
        # 定时读取
        self.timer = self.create_timer(0.01, self.read_serial)
        self.last_time = self.get_clock().now()
    
    def read_serial(self):
        try:
            data = self.ser.read(self.frame_size)
            if len(data) < self.frame_size:
                return
            
            # 检查帧头帧尾
            if data[0] != self.frame_header or data[23] != self.frame_tail:
                self.ser.read(1)
                return
            
            # 解析速度（字节2-7，大端序有符号整数）
            import struct
            vx_raw = struct.unpack('>h', bytes([data[2], data[3]]))[0]
            vy_raw = struct.unpack('>h', bytes([data[4], data[5]]))[0]
            vz_raw = struct.unpack('>h', bytes([data[6], data[7]]))[0]
            
            # 转换为m/s
            self.vx = vx_raw / 1000.0
            self.vy = vy_raw / 1000.0
            self.vz = vz_raw / 1000.0
            
            # 积分计算位置
            current_time = self.get_clock().now()
            dt = (current_time - self.last_time).nanoseconds / 1e9
            self.last_time = current_time
            
            self.theta += self.vz * dt
            self.x += (self.vx * math.cos(self.theta) - self.vy * math.sin(self.theta)) * dt
            self.y += (self.vx * math.sin(self.theta) + self.vy * math.cos(self.theta)) * dt
            
            # 发布odom
            self.publish_odom()
            
        except Exception as e:
            self.get_logger().error(f'Serial error: {e}')
    
    def publish_odom(self):
        odom = Odometry()
        odom.header.stamp = self.get_clock().now().to_msg()
        odom.header.frame_id = 'odom_combined'
        odom.child_frame_id = 'base_footprint'
        
        # 位置
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0.0
        
        # 朝向（四元数）
        q = Quaternion()
        q.z = math.sin(self.theta / 2.0)
        q.w = math.cos(self.theta / 2.0)
        odom.pose.pose.orientation = q
        
        # 速度
        odom.twist.twist.linear.x = self.vx
        odom.twist.twist.linear.y = self.vy
        odom.twist.twist.angular.z = self.vz
        
        self.odom_pub.publish(odom)

rclpy.init()
node = STM32Odom()
rclpy.spin(node)

4.3 scan_to_map.py（扫描数据转占用栅格地图）

由于cartographer的occupancy_grid_node在X3上崩溃（共享库问题），我写了一个Python版本的占用栅格地图生成器：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import OccupancyGrid, MapMetaData
import numpy as np
import math

class ScanToMap(Node):
    def __init__(self):
        super().__init__('scan_to_map')
        
        # 地图参数
        self.resolution = 0.05  # 5cm/格
        self.width = 600       # 600格 = 30m
        self.height = 600
        self.origin_x = -self.width * self.resolution / 2
        self.origin_y = -self.height * self.resolution / 2
        
        # 地图数据：-1=未知, 0=空闲, 100=占用
        self.map_data = np.full(self.width * self.height, -1, dtype=np.int8)
        
        # QoS设置
        qos_reliable = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            depth=1
        )
        qos_sensor = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            depth=10
        )
        
        # 发布者和订阅者
        self.map_pub = self.create_publisher(OccupancyGrid, '/map', qos_reliable)
        self.scan_sub = self.create_subscription(LaserScan, '/scan', self.scan_cb, qos_sensor)
        
        # 定时发布地图（2秒一次）
        self.timer = self.create_timer(2.0, self.publish_map)
        
        self.get_logger().info(f'ScanToMap started: {self.width}x{self.height} grid')
    
    def world_to_grid(self, x, y):
        """世界坐标转网格坐标"""
        gx = int((x - self.origin_x) / self.resolution)
        gy = int((y - self.origin_y) / self.resolution)
        return gx, gy
    
    def scan_cb(self, msg):
        """处理激光扫描数据"""
        cx, cy = self.world_to_grid(0, 0)  # 机器人在原点
        
        for i, r in enumerate(msg.ranges):
            if r < msg.range_min or r > msg.range_max:
                continue
            if math.isinf(r) or math.isnan(r):
                continue
            
            # 计算终点坐标
            angle = msg.angle_min + i * msg.angle_increment
            ex = r * math.cos(angle)
            ey = r * math.sin(angle)
            
            # 转换为网格坐标
            gx, gy = self.world_to_grid(ex, ey)
            
            # 标记终点为占用
            if 0 <= gx < self.width and 0 <= gy < self.height:
                self.map_data[gy * self.width + gx] = 100
            
            # 用Bresenham算法标记射线经过的格子为空闲
            self.bresenham_free(cx, cy, gx, gy)
    
    def bresenham_free(self, x0, y0, x1, y1):
        """Bresenham直线算法，标记射线经过的格子为空闲"""
        dx = abs(x1 - x0)
        dy = abs(y1 - y0)
        sx = 1 if x0 < x1 else -1
        sy = 1 if y0 < y1 else -1
        err = dx - dy
        
        x, y = x0, y0
        while True:
            if x == x1 and y == y1:
                break
            if 0 <= x < self.width and 0 <= y < self.height:
                idx = y * self.width + x
                if self.map_data[idx] != 100:  # 不覆盖占用格子
                    self.map_data[idx] = 0
            
            e2 = 2 * err
            if e2 > -dy:
                err -= dy
                x += sx
            if e2 < dx:
                err += dx
                y += sy
    
    def publish_map(self):
        """发布占用栅格地图"""
        grid = OccupancyGrid()
        grid.header.stamp = self.get_clock().now().to_msg()
        grid.header.frame_id = 'map'
        
        grid.info = MapMetaData()
        grid.info.resolution = self.resolution
        grid.info.width = self.width
        grid.info.height = self.height
        grid.info.origin.position.x = self.origin_x
        grid.info.origin.position.y = self.origin_y
        grid.info.origin.orientation.w = 1.0
        
        grid.data = self.map_data.tolist()
        self.map_pub.publish(grid)
        
        occupied = np.sum(self.map_data == 100)
        free = np.sum(self.map_data == 0)
        self.get_logger().info(f'Map published: occupied={occupied}, free={free}')

rclpy.init()
node = ScanToMap()
rclpy.spin(node)

---

五、一键启动脚本

由于X3的/tmp目录重启后会清空，所有脚本都放在/root/scripts/目录下。

#!/bin/bash
# start_slam.sh - X3 SLAM一键启动脚本
# 使用方法：bash /root/scripts/start_slam.sh

# 设置环境变量（非常重要！）
source /opt/ros/foxy/setup.bash
source /root/dev_ws/install/setup.bash
export PYTHONPATH=/opt/ros/foxy/lib/python3.8/site-packages:/opt/tros/lib/python3.8/site-packages:$PYTHONPATH
export LD_LIBRARY_PATH=/opt/ros/foxy/lib:/opt/ros/foxy/lib/aarch64-linux-gnu:/opt/tros/lib:$LD_LIBRARY_PATH
export AMENT_PREFIX_PATH=/opt/ros/foxy:/opt/tros:$AMENT_PREFIX_PATH
export CMAKE_PREFIX_PATH=/opt/ros/foxy:/opt/tros:$CMAKE_PREFIX_PATH

echo "=== Starting X3 SLAM System ==="

# 1. 电机驱动
echo "1. Motor driver..."
nohup ros2 launch origincar_base base_serial.launch.py > /tmp/motor.log 2>&1 &
sleep 3

# 2. 雷达驱动（Python版）
echo "2. Lidar..."
nohup python3 /root/scripts/n10_lidar.py > /tmp/n10.log 2>&1 &
sleep 2

# 3. 里程计读取
echo "3. Odom..."
nohup python3 /root/scripts/stm32_odom.py > /tmp/stm32.log 2>&1 &
sleep 1

# 4. odom转tf
echo "4. Odom to TF..."
nohup python3 /root/scripts/odom_to_tf.py > /tmp/odom_tf.log 2>&1 &
sleep 1

# 5. 静态变换（base_footprint → laser）
echo "5. Static TF..."
nohup /opt/ros/foxy/lib/tf2_ros/static_transform_publisher 0 0 0 0 0 0 base_footprint laser > /tmp/stf.log 2>&1 &
sleep 1

# 6. Cartographer SLAM
echo "6. Cartographer..."
nohup /opt/ros/foxy/lib/cartographer_ros/cartographer_node \
    -configuration_directory /root/lslidar_ws/cartographer_config/ \
    -configuration_basename lslidar_2d.lua > /tmp/cart.log 2>&1 &
sleep 2

# 7. 占用栅格地图生成
echo "7. Scan to Map..."
nohup python3 /root/scripts/scan_to_map.py > /tmp/scan_map.log 2>&1 &
sleep 1

# 8. Rosbridge（用于coStudio连接）
echo "8. Rosbridge..."
nohup /opt/ros/foxy/lib/rosapi/rosapi_node > /tmp/rosapi.log 2>&1 &
nohup /opt/ros/foxy/lib/rosbridge_server/rosbridge_websocket --ros-args -p port:=9090 > /tmp/rosbridge.log 2>&1 &

echo "=== All started ==="
echo "coStudio: ws://$(hostname -I | awk '{print $1}'):9090"
echo ""
echo "To check status: tail -f /tmp/slam.log"
echo "To stop all: kill -9 \$(pgrep -f 'n10_lidar\|stm32_odom\|odom_to_tf\|cartographer\|scan_to_map\|rosbridge\|rosapi')"
echo ""
echo "Press Ctrl+C to stop all"
wait

---

六、coStudio可视化配置

6.1 连接设置

1. 打开coStudio（Windows上安装）
2. 点击左上角 + 按钮，选择 WebSocket 连接
3. 输入地址：ws://10.146.125.155:9090（X3的IP地址）
4. 点击连接

6.2 3D面板配置

1. 连接成功后，默认会显示3D面板
2. 在左侧面板设置里：
   • 展示参考系：选择map
   • 跟踪模式：选择固定（这样视角不会跟着小车移动）
3. 在话题列表里，启用/map和/scan（点击眼睛图标）

6.3 添加地图面板

如果3D面板里看不到地图，需要手动添加地图面板：

1. 点击左上角 + 按钮
2. 选择 地图 面板
3. 话题选择 /map

6.4 可视化效果

正常情况下，你应该能看到：

• 白色区域：空闲空间（机器人可以通行）
• 黑色区域：障碍物（墙壁、家具等）
• 灰色区域：未知区域（还没扫描到）
• 彩色点云：激光雷达的实时扫描数据

---

七、保存地图

建图完成后，保存地图用于导航：

# 设置环境
source /opt/ros/foxy/setup.bash
export PYTHONPATH=/opt/ros/foxy/lib/python3.8/site-packages:/opt/tros/lib/python3.8/site-packages:$PYTHONPATH

# 保存地图
ros2 run nav2_map_server map_saver_cli -f /root/map

会生成两个文件：

• map.pgm：地图图片（黑白灰的PNG格式）
• map.yaml：地图配置文件（包含分辨率、原点等信息）

这两个文件可以用于后续的导航功能。

---

八、踩坑总结

问题	现象	原因	解决方案
雷达不发数据	/scan话题为空	interface_selection硬编码为"net"	改为"serial"
驱动崩溃	启动时直接退出	declare_parameter重复声明	用try-catch包裹或直接写死参数
数据被丢弃	有串口数据但不处理	CRC校验不匹配	禁用CRC校验
没有map帧	coStudio看不到map参考系	provide_odom_frame=false	改为true
找不到rclpy	Python脚本报错	PYTHONPATH没设置	启动脚本里export
共享库缺失	C++节点崩溃	LD_LIBRARY_PATH没设置	启动脚本里export
/tmp脚本丢失	重启后脚本没了	系统重启清空/tmp	脚本放/root/scripts/
地图不更新	地图一直不变	scan_to_map.py没运行	重启启动脚本
coStudio连不上	WebSocket连接失败	rosbridge没启动	检查rosbridge进程

---

九、总结

在RDK X3上做SLAM建图，最大的坑是驱动兼容性。镭神官方的C++驱动在X3上有多个bug，包括参数硬编码、重复声明、CRC校验不匹配等。最终用Python重写驱动才解决。

关键经验：

1. 不要相信官方驱动：一定要测试串口是否有数据，用cat /dev/ttyACM1 | xxd确认
2. 环境变量很重要：ROS2 Foxy需要正确设置PYTHONPATH和LD_LIBRARY_PATH，否则Python脚本找不到rclpy，C++节点找不到共享库
3. cartographer的provide_odom_frame必须为true：否则没有map坐标系，coStudio里看不到地图
4. 脚本不要放/tmp：X3的/tmp目录重启后会清空，重要脚本要放在/root/scripts/等持久目录
5. QoS设置要匹配：雷达驱动和cartographer的QoS要一致，建议都用RELIABLE

后续优化方向：

• 集成YOLOv8目标检测，实现动态避障
• 优化cartographer参数，提高建图精度
• 实现自主导航和路径规划

希望这篇文章能帮到同样在做智能车竞赛的朋友！如果有问题，欢迎在评论区交流。

---

项目代码： [待补充]

参考链接：

• 镭神智能N10雷达官方文档
• 地平线RDK X3官方文档
• Cartographer ROS2官方文档
• ROS2 Foxy官方文档