ROS2终端数据记录：轻量级CSV存储方案实现

1. 项目背景与核心需求

在机器人开发领域，ROS2作为新一代机器人操作系统框架，其节点间通信机制为分布式系统开发提供了极大便利。但在实际调试过程中，我们经常需要将终端交互数据持久化存储，比如记录操作指令、传感器校准参数或调试过程中的关键变量。传统方法要么依赖ros2 bag录制话题（不适合交互式场景），要么需要额外开发数据记录节点（增加系统复杂度）。

这个项目要解决的问题很明确：创建一个轻量级ROS2节点，能够实时监听终端输入（比如通过std::cin获取的用户指令或调试参数），并将这些数据按时间戳追加存储到CSV文件中。这种方案特别适合以下场景：

• 快速记录手动输入的测试参数
• 保存临时调试指令用于后续分析
• 在缺乏可视化工具的环境下记录关键数据

2. 环境准备与工程创建

2.1 基础环境配置

确保已安装ROS2 Humble（推荐）或Foxy版本：

bash复制sudo apt update
sudo apt install ros-humble-desktop

创建功能包（假设命名为terminal_recorder）：

bash复制mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python terminal_recorder --dependencies rclpy std_msgs

注意：如果使用C++实现，需指定--build-type ament_cmake并添加std::chrono和fstream依赖

2.2 文件结构规划

Python实现的标准结构如下：

code复制terminal_recorder/
├── terminal_recorder
│   ├── __init__.py
│   └── terminal_to_csv.py  # 主节点实现
├── resource
├── package.xml
└── setup.py

3. 核心代码实现解析

3.1 节点初始化设计

python复制import rclpy
from rclpy.node import Node
import csv
from datetime import datetime

class TerminalToCsv(Node):
    def __init__(self):
        super().__init__('terminal_recorder')
        self.declare_parameter('output_file', 'terminal_data.csv')
        self.output_path = self.get_parameter('output_file').value
        
        # 初始化CSV文件并写入表头
        with open(self.output_path, 'w', newline='') as f:
            writer = csv.writer(f)
            writer.writerow(['timestamp', 'input_data'])
        
        self.get_logger().info(f"Recording terminal input to {self.output_path}")

关键点说明：

1. 使用declare_parameter实现输出文件路径可配置化
2. 文件打开模式为'w'确保每次运行新建文件（如需追加改用'a'）
3. CSV表头设计为时间戳+输入数据的标准结构

3.2 终端输入监听循环

python复制def start_listening(self):
    try:
        while rclpy.ok():
            user_input = input("Enter data (Ctrl+C to exit): ")
            timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')
            
            # 线程安全地追加写入
            with open(self.output_path, 'a', newline='') as f:
                writer = csv.writer(f)
                writer.writerow([timestamp, user_input])
                
            self.get_logger().debug(f"Recorded: {timestamp} - {user_input}")
            
    except KeyboardInterrupt:
        self.get_logger().info("Shutting down terminal recorder")

注意事项：

• 使用rclpy.ok()作为循环条件确保节点关闭时能退出
• 输入线程会阻塞ROS2回调，建议单独开线程处理（后文详述）
• 时间戳精确到微秒(%f)便于后续数据分析

4. 高级实现技巧

4.1 多线程安全优化

ROS2节点的回调机制是单线程的，直接使用input()会阻塞其他回调。改进方案：

python复制from threading import Thread

def __init__(self):
    # ...原有初始化代码...
    self.input_thread = Thread(target=self.start_listening, daemon=True)
    self.input_thread.start()

重要：必须设置daemon=True，否则节点无法正常关闭

4.2 CSV写入性能优化

高频输入场景下，频繁打开/关闭文件会影响性能。两种优化方案：

方案1：缓冲区批量写入

python复制self.buffer = []
self.buffer_size = 10  # 每10条写入一次

def start_listening(self):
    # ...输入获取逻辑...
    self.buffer.append([timestamp, user_input])
    if len(self.buffer) >= self.buffer_size:
        self.flush_buffer()
        
def flush_buffer(self):
    with open(self.output_path, 'a', newline='') as f:
        writer = csv.writer(f)
        writer.writerows(self.buffer)
    self.buffer.clear()

方案2：保持文件句柄打开

python复制def __init__(self):
    self.csv_file = open(self.output_path, 'a', newline='')
    self.writer = csv.writer(self.csv_file)
    
def destroy_node(self):
    self.csv_file.close()
    super().destroy_node()

5. 部署与使用指南

5.1 编译与运行

bash复制cd ~/ros2_ws
colcon build --packages-select terminal_recorder
source install/setup.bash
ros2 run terminal_recorder terminal_to_csv

5.2 参数配置示例

指定输出路径：

bash复制ros2 run terminal_recorder terminal_to_csv --ros-args -p output_file:=/path/to/special_data.csv

5.3 数据文件示例

生成的CSV文件内容类似：

code复制timestamp,input_data
2023-07-20 14:30:15.123456,start_test
2023-07-20 14:30:18.456789,velocity 0.5
2023-07-20 14:30:22.789123,stop

6. 典型问题排查

6.1 权限问题

错误现象：

code复制PermissionError: [Errno 13] Permission denied: 'terminal_data.csv'

解决方案：

• 检查目标目录写入权限
• 或修改参数指定用户目录路径：

  bash复制-p output_file:=/home/${USER}/ros_data.csv
  

6.2 文件占用锁定

Windows平台下可能出现文件占用错误：

code复制PermissionError: [Errno 13] Permission denied

解决方法：

• 使用try-finally确保文件句柄释放
• 或改用WSL2环境运行

6.3 中文编码问题

当输入包含中文时出现乱码：

code复制UnicodeEncodeError: 'ascii' codec can't encode characters

解决方案：

python复制with open(self.output_path, 'a', newline='', encoding='utf-8-sig') as f:
    writer = csv.writer(f)

7. 功能扩展方向

7.1 集成ROS2话题订阅

同时记录话题数据和终端输入：

python复制self.subscription = self.create_subscription(
    String,
    '/debug_command',
    self.command_callback,
    10)

def command_callback(self, msg):
    timestamp = self.get_clock().now().to_msg()
    with open(self.output_path, 'a') as f:
        writer = csv.writer(f)
        writer.writerow([timestamp, msg.data])

7.2 自动文件分割

按日期或大小分割文件：

python复制from pathlib import Path

def get_new_filename(self):
    date_str = datetime.now().strftime('%Y%m%d')
    counter = 0
    while True:
        path = Path(f"{self.output_dir}/{date_str}_{counter}.csv")
        if not path.exists():
            return str(path)
        counter += 1

7.3 数据校验机制

添加MD5校验防止数据损坏：

python复制import hashlib

def write_with_checksum(self, data):
    row = [datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'), data]
    checksum = hashlib.md5(str(row).encode()).hexdigest()
    row.append(checksum)
    
    with open(self.output_path, 'a') as f:
        writer = csv.writer(f)
        writer.writerow(row)

8. 性能对比测试

在不同硬件环境下测试1000次输入记录耗时：

结论：对于嵌入式设备推荐使用缓冲区方案，PC环境可直接保持文件句柄打开。

9. 实际应用案例

9.1 机械臂校准参数记录

code复制$ ros2 run terminal_recorder terminal_to_csv -p output_file:=arm_calibration.csv
Enter data: joint1_offset 0.02
Enter data: joint2_scale 1.05
...

9.2 无人车测试指令存档

配合脚本自动化测试：

python复制import subprocess
proc = subprocess.Popen(['ros2', 'run', 'terminal_recorder', 'terminal_to_csv'],
                        stdin=subprocess.PIPE)
proc.stdin.write(b"start_mission\n")
proc.stdin.write(b"set_speed 2.0\n")

9.3 实验数据标注

将人工观察结果与传感器数据关联：

code复制2023-07-20 15:00:00.000001,lidar_noise_detected
2023-07-20 15:00:03.123456,object_appeared

10. 工程化建议

1. 日志分级：区分DEBUG/INFO级别日志

   python复制self.declare_parameter('log_level', 'INFO')
   level = self.get_parameter('log_level').value
   self.get_logger().set_level(getattr(rclpy.logging.LoggingSeverity, level))
   

2. 异常恢复：文件写入失败后重试机制

   python复制def safe_write(self, data, max_retries=3):
       for i in range(max_retries):
           try:
               # 写入逻辑
               return True
           except IOError as e:
               self.get_logger().error(f"Write failed (attempt {i+1}): {str(e)}")
               time.sleep(0.1)
       return False
   

3. 启动配置：使用launch文件集成

   xml复制<launch>
       <node pkg="terminal_recorder" exec="terminal_to_csv" name="terminal_recorder">
           <param name="output_file" value="$(find-pkg-share terminal_recorder)/logs/terminal.csv"/>
       </node>
   </launch>