ROS2 Jazzy C++自定义库开发指南-代码聚汇网

ROS2 Jazzy C++自定义库开发指南

赵泠

1. ROS2 Jazzy与C++自定义库开发概述

在机器人操作系统(ROS)生态中，ROS2 Jazzy作为2024年发布的长期支持版本(LTS)，为开发者提供了更稳定的API和更完善的工具链支持。与Python相比，C++在性能敏感型机器人应用中仍占据主导地位，特别是在实时控制、传感器数据处理等场景。本指南将完整演示从零构建ROS2 C++自定义库到实际应用集成的全流程。

为什么选择C++进行ROS2开发？实测数据显示，相同算法逻辑下，C++实现的节点CPU占用率通常比Python低30-50%，这对于资源受限的嵌入式机器人平台尤为关键。而自定义库的封装，则能实现算法模块的跨项目复用，避免"重复造轮子"。

典型应用场景包括：

机器人运动控制算法的封装与迭代优化
传感器驱动(SDK)的二次封装
计算机视觉处理流水线的模块化设计
多机器人系统的通信中间件

提示：虽然本文以Jazzy为例，但所述方法同样适用于Humble等ROS2版本，只需注意个别API的差异。

2. 开发环境配置

2.1 基础环境准备

以Ubuntu 24.04为例，官方推荐使用以下方式安装ROS2 Jazzy：

bash复制sudo apt update && sudo apt install curl gnupg
curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
sudo apt install ros-jazzy-desktop

关键开发工具链安装：

bash复制sudo apt install -y \
    build-essential \
    cmake \
    git \
    python3-colcon-common-extensions \
    python3-rosdep \
    python3-vcstool \
    ros-dev-tools

2.2 VSCode环境配置

对于C++开发，推荐使用VSCode配合以下插件：

C/C++ (Microsoft)
CMake Tools
ROS
clangd

配置示例.vscode/c_cpp_properties.json：

json复制{
    "configurations": [
        {
            "name": "Linux",
            "includePath": [
                "${workspaceFolder}/**",
                "/opt/ros/jazzy/include/**"
            ],
            "defines": [],
            "compilerPath": "/usr/bin/g++",
            "cStandard": "c17",
            "cppStandard": "c++17",
            "intelliSenseMode": "linux-gcc-x64"
        }
    ],
    "version": 4
}

注意：务必在workspace根目录执行source /opt/ros/jazzy/setup.bash，否则会出现头文件找不到的问题。

3. 自定义C++库开发实战

3.1 创建库项目结构

建议采用以下目录结构：

code复制my_robot_libs/
├── CMakeLists.txt
├── include
│   └── my_robot_libs  # 公共头文件
├── src                # 私有实现
└── test               # 单元测试

关键CMake配置示例：

cmake复制cmake_minimum_required(VERSION 3.8)
project(my_robot_libs)

# 默认使用C++17标准
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)

# 添加库目标
add_library(trajectory_planner
  src/trajectory_planner.cpp
)

target_include_directories(trajectory_planner PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)

# 链接依赖
ament_target_dependencies(trajectory_planner
  rclcpp
  Eigen3
)

# 安装规则
install(
  TARGETS trajectory_planner
  EXPORT export_${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION include
)

ament_export_targets(export_${PROJECT_NAME})
ament_package()

3.2 实现运动规划库示例

以机器人轨迹规划为例，include/my_robot_libs/trajectory_planner.hpp：

cpp复制#pragma once

#include <vector>
#include <Eigen/Dense>

namespace my_robot_libs {

class TrajectoryPlanner {
public:
  struct Waypoint {
    Eigen::Vector3d position;
    Eigen::Quaterniond orientation;
    double time_from_start;
  };

  explicit TrajectoryPlanner(double max_accel = 1.0);

  std::vector<Waypoint> planLinearTrajectory(
    const Eigen::Vector3d& start,
    const Eigen::Vector3d& end,
    double duration);

  std::vector<Waypoint> planCircularTrajectory(
    const Eigen::Vector3d& center,
    double radius,
    double duration);

private:
  double max_accel_;
  // 其他私有成员...
};

}  // namespace my_robot_libs

对应的实现文件src/trajectory_planner.cpp需要实现这些接口。特别注意：

使用Eigen进行矩阵运算而非原生数组
时间参数建议使用ROS2的builtin_interfaces::msg::Duration
内存分配尽量使用智能指针

4. 应用节点集成

4.1 创建依赖库的ROS2节点

在另一个package中，CMakeLists.txt需添加：

cmake复制find_package(my_robot_libs REQUIRED)

add_executable(robot_controller src/robot_controller.cpp)
target_link_libraries(robot_controller
  trajectory_planner
  other_dependencies
)

典型节点实现结构：

cpp复制#include "my_robot_libs/trajectory_planner.hpp"
#include "rclcpp/rclcpp.hpp"

class RobotController : public rclcpp::Node {
public:
  RobotController() : Node("robot_controller") {
    planner_ = std::make_unique<my_robot_libs::TrajectoryPlanner>(1.5);
    // 其他初始化...
  }

private:
  std::unique_ptr<my_robot_libs::TrajectoryPlanner> planner_;
  // 其他成员...
};

4.2 参数配置与动态加载

推荐使用ROS2的参数声明：

cpp复制declare_parameter<double>("max_accel", 1.0);
auto max_accel = get_parameter("max_accel").as_double();
planner_ = std::make_unique<my_robot_libs::TrajectoryPlanner>(max_accel);

对于需要动态更新的参数，可添加参数回调：

cpp复制param_callback_ = add_on_set_parameters_callback(
  [this](const std::vector<rclcpp::Parameter> &params) {
    auto result = rcl_interfaces::msg::SetParametersResult();
    result.successful = true;
    for (const auto ¶m : params) {
      if (param.get_name() == "max_accel") {
        planner_->setMaxAccel(param.as_double());
      }
    }
    return result;
  });

5. 调试与性能优化

5.1 GDB调试技巧

在launch文件中启用调试：

xml复制<executable cmd_prefix="gdb -ex run --args" 
           name="robot_controller" 
           output="screen" 
           pkg="my_robot_pkg" />

常用调试场景：

段错误定位：bt full查看完整调用栈
内存泄漏检测：搭配Valgrind使用
死锁检测：thread apply all bt

5.2 性能分析工具

使用perf进行热点分析：

bash复制perf record -g ./install/my_robot_pkg/lib/my_robot_pkg/robot_controller
perf report -g "graph,0.5,caller"

关键优化方向：

减少Eigen矩阵的临时对象创建
使用move语义传递大型数据结构
避免在实时循环中进行动态内存分配

6. 进阶主题：模板库与插件架构

6.1 模板化设计

对于通用算法，可采用模板设计：

cpp复制template <typename Scalar>
class KalmanFilter {
public:
  using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
  // 接口定义...
};

6.2 插件实现

参考ROS2的pluginlib机制：

定义基类接口
注册插件类
使用pluginlib加载

示例CMake配置：

cmake复制pluginlib_export_plugin_description_file(my_robot_libs plugin_description.xml)

在代码中动态加载：

cpp复制auto plugin_loader = std::make_unique<pluginlib::ClassLoader<ControllerInterface>>(
  "my_robot_libs", "my_robot_libs::ControllerInterface");
auto controller = plugin_loader->createSharedInstance("pid_controller");

7. 测试与持续集成

7.1 单元测试框架

使用ament_cmake添加gtest：

cmake复制if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_add_gtest(test_trajectory_planner
    test/test_trajectory_planner.cpp)
  target_link_libraries(test_trajectory_planner
    trajectory_planner
    gtest_main)
endif()

测试用例示例：

cpp复制TEST(TestTrajectoryPlanner, LinearTrajectory) {
  TrajectoryPlanner planner;
  auto waypoints = planner.planLinearTrajectory(
    {0,0,0}, {1,1,1}, 1.0);
  EXPECT_GT(waypoints.size(), 0);
  // 更多断言...
}

7.2 CI/CD配置

GitLab CI示例：

yaml复制test_job:
  image: ros:jazzy
  script:
    - apt-get update && apt-get install -y ros-jazzy-ament-lint
    - mkdir -p src && cd src
    - git clone https://gitlab.com/my_project/my_robot_libs.git
    - cd ..
    - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug
    - colcon test
    - colcon test-result --verbose