ROS 1到ROS 2的机器人底盘控制系统迁移实战

1. 项目背景与核心挑战

去年接手了一个机器人底盘控制系统的升级项目，需要将原本基于ROS 1的底盘驱动代码迁移到ROS 2环境。这个看似简单的版本升级，在实际操作中却遇到了各种意想不到的"坑"。作为从ROS 1时代走过来的开发者，我深刻体会到两个版本间的差异远比官方文档描述的更复杂。

底盘控制作为机器人运动的核心模块，其稳定性和实时性要求极高。我们原有的代码base经过多年迭代，包含了里程计计算、电机控制、PID调节、串口通信等核心功能，代码量约1.5万行，主要采用C++03风格编写。迁移过程中不仅要保证功能一致性，还要利用ROS 2的新特性进行现代化改造。

2. ROS 1与ROS 2架构差异解析

2.1 通信机制的本质区别

ROS 1采用的TCPROS/UDPROS通信在ROS 2中被DDS取代。这个变化直接影响到底盘驱动中最关键的几个话题：

• /cmd_vel：运动控制指令
• /odom：里程计数据
• /battery：电源状态

在迁移过程中，我们发现ROS 2的QoS配置对底盘控制尤为关键。例如：

cpp复制auto qos = rclcpp::QoS(
  rclcpp::KeepLast(10),
  rmw_qos_profile_sensor_data
);

必须显式设置历史深度和可靠性策略，否则在无线网络不稳定的场地下会出现指令丢失。

2.2 生命周期管理的变化

ROS 1的ros::init在ROS 2中变为rclcpp::init，看似简单的API变化背后是全新的节点生命周期模型。底盘驱动需要特别注意：

1. 节点创建时机
2. 参数服务器访问顺序
3. 定时器回调的激活条件

我们采用了LifecycleNode来实现安全的状态切换：

cpp复制class ChassisDriver : public rclcpp_lifecycle::LifecycleNode {
  //...
  CallbackReturn on_configure(const State &) override;
  CallbackReturn on_activate(const State &) override;
};

3. 代码迁移实战步骤

3.1 基础框架搭建

1. 创建功能包：

   bash复制ros2 pkg create chassis_driver --build-type ament_cmake --dependencies rclcpp sensor_msgs geometry_msgs
   

2. CMakeLists.txt改造：

   cmake复制find_package(ament_cmake REQUIRED)
   find_package(rclcpp REQUIRED)
   
   add_executable(chassis_node src/chassis_node.cpp)
   ament_target_dependencies(chassis_node rclcpp)
   
   install(TARGETS chassis_node
     DESTINATION lib/${PROJECT_NAME})
   

3. package.xml更新：
   必须显式声明所有依赖，ROS 2不再有roscpp这样的元包。

3.2 核心功能迁移

3.2.1 话题订阅改造

原ROS 1代码：

cpp复制ros::Subscriber cmd_sub = nh.subscribe("cmd_vel", 10, cmdCallback);

ROS 2等效实现：

cpp复制auto cmd_sub = create_subscription<geometry_msgs::msg::Twist>(
  "cmd_vel", 
  10,
  [this](const geometry_msgs::msg::Twist::SharedPtr msg) {
    // 回调处理
  });

关键变化：

• 使用lambda替代自由函数
• 消息类型变为模板参数
• SharedPtr智能指针管理生命周期

3.2.2 服务端改造

底盘控制需要提供急停服务，ROS 1实现：

cpp复制ros::ServiceServer estop_srv = nh.advertiseService("emergency_stop", estopHandler);

ROS 2版本：

cpp复制auto estop_srv = create_service<std_srvs::srv::Empty>(
  "emergency_stop",
  [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
         std::shared_ptr<std_srvs::srv::Empty::Response> response) {
    // 处理逻辑
  });

3.3 参数系统升级

ROS 1的参数获取方式：

cpp复制ros::param::param<double>("max_speed", max_speed_, 0.5);

ROS 2提供了更安全的声明式参数：

cpp复制// 声明参数
this->declare_parameter<double>("max_speed", 0.5);
// 获取参数
this->get_parameter("max_speed", max_speed_);

建议为关键参数添加描述：

cpp复制auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
param_desc.description = "Maximum linear velocity (m/s)";
this->declare_parameter("max_speed", 0.5, param_desc);

4. 现代C++改造实践

4.1 智能指针应用

原代码中大量使用裸指针管理硬件资源，容易导致内存泄漏。改造后：

cpp复制class SerialPort {
public:
  using SharedPtr = std::shared_ptr<SerialPort>;
  static SharedPtr create(const std::string &port) {
    return std::make_shared<SerialPort>(port);
  }
  //...
private:
  explicit SerialPort(const std::string &port);
};

4.2 线程安全改进

底盘驱动涉及多线程操作，采用现代同步原语：

cpp复制class ChassisController {
public:
  void updateOdometry(const Odometry &odom) {
    std::lock_guard<std::mutex> lock(odom_mutex_);
    odometry_ = odom;
  }
private:
  Odometry odometry_;
  mutable std::mutex odom_mutex_;
};

4.3 异步编程模型

利用std::async处理耗时操作：

cpp复制auto future = std::async(std::launch::async, [this]() {
  return readEncoderData();
});
// ...其他操作
auto encoders = future.get();

5. 典型问题与解决方案

5.1 串口通信异常

现象：ROS 2下串口数据接收不完整
原因：DDS占用CPU资源导致串口中断响应延迟
解决：

1. 调整线程优先级：

   cpp复制sched_param sch;
   sch.sched_priority = 50;
   pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch);
   

2. 使用专用IO线程池

5.2 实时性下降

现象：控制指令响应延迟增加
优化方案：

1. 启用实时时钟：

   cpp复制rcl_clock_t clock;
   rcl_clock_init(RCL_STEADY_TIME, &clock);
   

2. 使用零拷贝发布：

   cpp复制auto msg = std::make_unique<nav_msgs::msg::Odometry>();
   // 填充数据
   odom_pub_->publish(std::move(msg));
   

5.3 参数动态配置

需求：运行时调整PID参数
实现：

cpp复制// 声明参数回调
auto param_callback = [this](std::vector<rclcpp::Parameter> params) {
  auto result = rcl_interfaces::msg::SetParametersResult();
  result.successful = true;
  for (auto ¶m : params) {
    if (param.get_name() == "p_gain") {
      p_gain_ = param.as_double();
    }
    // 其他参数处理
  }
  return result;
};
param_handler_ = add_on_set_parameters_callback(param_callback);

6. 性能优化技巧

6.1 消息序列化优化

对于高频发布的里程计数据，使用固定内存分配：

cpp复制class OdometryPublisher {
public:
  OdometryPublisher() {
    message_ = std::make_shared<nav_msgs::msg::Odometry>();
    // 预分配所有字段内存
    message_->header.frame_id.reserve(32);
    message_->child_frame_id.reserve(32);
    // ...
  }
  void publish() {
    // 重用message_对象
    pub_->publish(*message_);
  }
private:
  nav_msgs::msg::Odometry::SharedPtr message_;
};

6.2 零拷贝数据传输

对于大尺寸消息如点云，使用共享内存：

cpp复制auto loaned_msg = pub_->borrow_loaned_message();
// 直接操作loaned_msg.get()
pub_->publish(std::move(loaned_msg));

6.3 执行器配置优化

针对底盘控制的高实时性需求，定制执行器：

cpp复制rclcpp::ExecutorOptions options;
options.context = context_;
auto executor = std::make_shared<rclcpp::executors::StaticSingleThreadedExecutor>(options);
executor->add_node(node);
executor->spin();

7. 测试验证策略

7.1 单元测试框架

使用ROS 2内置测试工具：

cmake复制if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
  
  add_rostest(test/test_chassis.test)
endif()

7.2 硬件在环测试

搭建测试脚手架：

cpp复制class TestChassis : public ::testing::Test {
protected:
  void SetUp() override {
    rclcpp::init(0, nullptr);
    node = std::make_shared<rclcpp::Node>("test_node");
  }
  rclcpp::Node::SharedPtr node;
};

7.3 性能基准测试

测量关键指标：

cpp复制auto start = std::chrono::high_resolution_clock::now();
// 被测代码
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> elapsed = end - start;
RCLCPP_INFO(logger_, "Latency: %.3f ms", elapsed.count() * 1000);

8. 持续集成方案

8.1 GitHub Actions配置

yaml复制name: ROS 2 CI

on: [push, pull_request]

jobs:
  build:
    runs-on: ubuntu-latest
    container: ros:galactic
    steps:
    - uses: actions/checkout@v2
    - run: |
        mkdir -p ~/ros2_ws/src
        ln -s $PWD ~/ros2_ws/src/chassis_driver
        cd ~/ros2_ws
        rosdep install -y --from-paths src --ignore-src
        colcon build --packages-select chassis_driver

8.2 静态代码分析

集成clang-tidy：

cmake复制set(CMAKE_CXX_CLANG_TIDY "clang-tidy;-checks=*")

9. 部署最佳实践

9.1 容器化部署

Dockerfile示例：

dockerfile复制FROM ros:galactic

# 安装依赖
RUN apt-get update && \
    apt-get install -y ros-galactic-robot-localization

# 复制功能包
COPY chassis_driver /ros_ws/src/chassis_driver

# 构建
RUN . /opt/ros/galactic/setup.sh && \
    cd /ros_ws && \
    colcon build

# 启动命令
CMD ["ros2", "launch", "chassis_driver", "chassis.launch.py"]

9.2 系统服务集成

创建systemd服务：

ini复制[Unit]
Description=Chassis Driver
After=network.target

[Service]
Type=simple
ExecStart=/usr/bin/ros2 launch chassis_driver chassis.launch.py
Restart=always
User=robot

[Install]
WantedBy=multi-user.target

10. 项目成果与经验总结

经过三个月的迁移改造，新系统在以下方面获得显著提升：

1. 性能指标：

   • 控制指令延迟从15ms降至8ms
   • CPU占用率降低40%
   • 内存使用减少25%

2. 代码质量：

   • 编译警告清零
   • 代码复用率提高30%
   • 单元测试覆盖率从60%提升至85%

3. 可维护性：

   • 参数配置全部可视化
   • 日志系统标准化
   • 故障诊断时间缩短70%

关键经验教训：

• ROS 2的QoS配置对实时系统至关重要，需要根据具体场景精细调整
• 生命周期管理是迁移过程中最容易出错的部分
• 现代C++特性可以显著提升代码安全性和性能，但需要团队统一规范