从 0 跑通一个最小 ROS2 + C++ + CMake + Eigen 机器人项目项目简介：在 Windows +

项目简介：在 Windows + WSL2 + Ubuntu + ROS2 环境中，创建并运行一个最小的 C++ / CMake / Eigen 机器人节点。

文章重点不放在“经验感受”，而放在三件事上：

这个最小项目是什么
它是怎么搭起来并跑起来的
里面的关键概念为什么这么设计

1. 项目结构

这个项目只有三个核心文件：

embodied_minimal_cpp/
├── CMakeLists.txt
├── package.xml
└── src/
    └── minimal_robot_node.cpp

它们分别承担不同职责：

minimal_robot_node.cpp：实现 ROS2 节点与 Eigen 数学计算
CMakeLists.txt：描述如何构建这个 C++ 节点
package.xml：描述这个包在 ROS2 生态中的元信息和依赖

虽然规模很小，但已经覆盖了机器人工程中几层最基本的内容：

ROS2 工作区与 package
C++ 节点
CMake 构建
Eigen 矩阵计算
最基本的机器人运动学与控制计算

2. 环境与执行边界

项目使用 Windows + WSL2 + Ubuntu + ROS2 环境，这样的好处是实施成本较低，相比安装ubuntu双系统，能省去不少精力和踩坑，适合暂时还没有调试真实机器人需求的初学者。

环境搭建过程略过，直接问AI即可

执行边界

Windows 侧

安装 VS Code
打开 WSL 终端
作为宿主系统

Ubuntu（WSL2 内）

安装 ROS2
安装 colcon、cmake、g++、Eigen
编译和运行 ROS2 工程

依赖安装

sudo apt update
sudo apt install -y \
  build-essential \
  cmake \
  python3-colcon-common-extensions \
  libeigen3-dev

3. 项目构建

3.1 创建文件

先在 Ubuntu 中创建 ROS2 工作区和 package 目录：

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
mkdir -p embodied_minimal_cpp/src

然后在 embodied_minimal_cpp 下创建三个文件: minimal_robot_node.cpp、CMakeLists.txt、 package.xml

`minimal_robot_node.cpp`

下面是最小节点代码：

#include <chrono>
#include <cmath>
#include <memory>
#include <sstream>

#include "rclcpp/rclcpp.hpp"
#include <Eigen/Dense>

using namespace std::chrono_literals;

class MinimalRobotNode : public rclcpp::Node
{
public:
  MinimalRobotNode() : Node("minimal_robot_node")
  {
    timer_ = this->create_wall_timer(
      1000ms, std::bind(&MinimalRobotNode::on_timer, this));
  }

private:
  void on_timer()
  {
    const double l1 = 1.0;
    const double l2 = 0.8;

    Eigen::Vector2d q;
    q << 0.5, -0.3;

    Eigen::Vector2d x_desired;
    x_desired << 1.5, 0.2;

    const double q1 = q(0);
    const double q2 = q(1);

    Eigen::Vector2d x;
    x(0) = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
    x(1) = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);

    Eigen::Matrix2d J;
    J(0, 0) = -l1 * std::sin(q1) - l2 * std::sin(q1 + q2);
    J(0, 1) = -l2 * std::sin(q1 + q2);
    J(1, 0) =  l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
    J(1, 1) =  l2 * std::cos(q1 + q2);

    Eigen::Vector2d e = x_desired - x;

    Eigen::Matrix2d K = 0.8 * Eigen::Matrix2d::Identity();
    Eigen::Vector2d qdot = J.inverse() * (K * e);

    std::ostringstream oss;
    oss << "\nq = [" << q.transpose() << "]"
        << "\nx = [" << x.transpose() << "]"
        << "\nx_desired = [" << x_desired.transpose() << "]"
        << "\ne = [" << e.transpose() << "]"
        << "\nJ = \n" << J
        << "\nqdot = [" << qdot.transpose() << "]";

    RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str());
  }

  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalRobotNode>());
  rclcpp::shutdown();
  return 0;
}

这段代码可以按功能拆成四部分：

ROS2 节点框架

class MinimalRobotNode : public rclcpp::Node

这表示创建了一个继承自 rclcpp::Node 的 ROS2 节点。

构造函数中：

timer_ = this->create_wall_timer(
  1000ms, std::bind(&MinimalRobotNode::on_timer, this));

作用是：每隔 1 秒执行一次 on_timer()。

机器人参数与状态

const double l1 = 1.0;
const double l2 = 0.8;

Eigen::Vector2d q;
q << 0.5, -0.3;

这里定义了：

两根连杆长度 l1、l2
两个关节角 q1、q2

运动学与雅可比矩阵计算

x(0) = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
x(1) = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);

这是两连杆末端位置的正运动学。

J(0, 0) = -l1 * std::sin(q1) - l2 * std::sin(q1 + q2);
J(0, 1) = -l2 * std::sin(q1 + q2);
J(1, 0) =  l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
J(1, 1) =  l2 * std::cos(q1 + q2);

这是对末端位置对关节角求偏导得到的 Jacobian。

最小控制律

Eigen::Vector2d e = x_desired - x;
Eigen::Matrix2d K = 0.8 * Eigen::Matrix2d::Identity();
Eigen::Vector2d qdot = J.inverse() * (K * e);

这里实现的是一个最小任务空间误差反馈：

先计算末端误差 e
通过增益矩阵 K 给出任务空间速度方向
再用 J.inverse() 将任务空间速度映射为关节速度命令

`CMakeLists.txt`

cmake_minimum_required(VERSION 3.8)
project(embodied_minimal_cpp)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(Eigen3 REQUIRED)

add_executable(minimal_robot_node src/minimal_robot_node.cpp)

target_compile_features(minimal_robot_node PUBLIC cxx_std_17)

target_link_libraries(minimal_robot_node Eigen3::Eigen)
ament_target_dependencies(minimal_robot_node rclcpp)

install(TARGETS
  minimal_robot_node
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

文件作用

这个文件告诉构建系统：

工程名字是什么
依赖哪些包
需要编译哪个源文件
生成的可执行文件叫什么
安装到哪里

重点

`find_package(ament_cmake REQUIRED)`

表示这个包使用 ROS2 的 ament_cmake 构建方式。

`find_package(rclcpp REQUIRED)`

表示依赖 ROS2 的 C++ 客户端库。

`find_package(Eigen3 REQUIRED)`

表示依赖 Eigen。

`add_executable(...)`

定义一个可执行文件。

`ament_target_dependencies(...)`

为这个目标声明 ROS2 依赖。

`ament_package()`

告诉 ROS2 这是一个标准的 ament 包，便于被工作区和其他包识别。

`package.xml`

<?xml version="1.0"?>
<package format="3">
  <name>embodied_minimal_cpp</name>
  <version>0.0.1</version>
  <description>Minimal ROS2 + CMake + Eigen robot project</description>

  <maintainer email="you@example.com">you</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

这个文件负责描述：

包名
版本
维护者信息
构建工具依赖
运行依赖
构建类型

重点

初学时容易觉得 CMakeLists.txt 已经写了依赖，为什么还要有 package.xml。

原因是：

CMakeLists.txt 更偏构建规则
package.xml 更偏 ROS2 包元信息和生态兼容

可以理解为：

一个文件面向“怎么编”
一个文件面向“这个包是什么、依赖什么、如何被生态识别”

3.2 编译

source /opt/ros/jazzy/setup.bash
cd ~/ros2_ws
colcon build --packages-select embodied_minimal_cpp
source ~/ros2_ws/install/setup.bash

3.3 运行

source /opt/ros/jazzy/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run embodied_minimal_cpp minimal_robot_node

运行结果

[INFO] [1774526815.073422502] [minimal_robot_node]: 
q = [ 0.5 -0.3]
x = [ 1.66164 0.638361]
x_desired = [1.5 0.2]
e = [-0.161636 -0.438361]
J = 
-0.638361 -0.158935
  1.66164  0.784053
qdot = [0.664598 -1.85575]
[INFO] [1774526816.073502706] [minimal_robot_node]: 
q = [ 0.5 -0.3]
x = [ 1.66164 0.638361]
x_desired = [1.5 0.2]
e = [-0.161636 -0.438361]
J = 
-0.638361 -0.158935
  1.66164  0.784053
qdot = [0.664598 -1.85575]
...

这样，一个最小机器人节点项目就跑通了

4. 问题

4.1 `colcon` 是什么，为什么用它来构建

在 ROS2 中，通常不是直接手写一长串 CMake 命令，而是使用：

colcon build --packages-select embodied_minimal_cpp

4.1.1 `colcon` 的作用

colcon 是 ROS2 工作区的构建调度工具。它负责：

扫描工作区中的 package
识别每个 package 的构建类型
调用对应构建系统
管理构建顺序与输出目录

4.1.2 它和 CMake 的关系

关系可以理解成：

colcon
  -> ament_cmake
      -> CMake
          -> make/ninja
              -> g++

也就是说：

colcon 不直接编译 C++ 源码
它负责调度整个 ROS2 workspace 的构建流程

重点

容易混淆的一点是：

VS Code 中的 task 名称只是一个标签
终端里真正能执行的是 colcon build --packages-select 包名

例如：

colcon build current pkg

这不是有效命令；它只是一个任务名风格的字符串。

4.2 `ament_cmake` 和 CMake 的区别

这两个概念很容易混在一起。

4.2.1 CMake 是什么

CMake 是通用构建系统，用于描述：

编译哪些源文件
链接哪些库
生成哪些目标
如何安装

4.2.2 `ament_cmake` 是什么

ament_cmake 是 ROS2 对 CMake 的一层封装和规范化扩展。

它解决的问题不是“C++ 能不能编出来”，而是：

这个项目如何成为 ROS2 里的标准 package
如何与 package.xml 协同
如何被 colcon 正确识别
如何导出 ROS2 包依赖和安装信息

4.2.3 可以这样记

ament_cmake = CMake + ROS2 包规范 + 辅助宏

难点

初学时常见误解是把 ament_cmake 当成“另一个完全不同的构建工具”。

更准确的理解是：

底层仍然是 CMake
ament_cmake 是 ROS2 场景下的工程适配层

4.3 为什么两连杆末端位置会出现 `q1 + q2`

两连杆正运动学公式如下：

[
x = l_1 \cos q_1 + l_2 \cos(q_1 + q_2)
]

[
y = l_1 \sin q_1 + l_2 \sin(q_1 + q_2)
]

4.3.1 先区分：角度不是坐标分量

这里最容易出错的点是把 q1、q2 看成 x/y 方向的分量。

实际上：

q1、q2 是 关节角
x、y 才是 末端坐标分量

4.3.2 第二根杆为什么是 `q1 + q2`

对于平面串联两连杆：

第一根杆相对基坐标系转了 q1
第二根杆不是直接相对基坐标系定义，而是相对第一根杆再转了 q2

所以第二根杆在世界坐标系中的绝对方向是：

[ q_1 + q_2 ]

重点

初学两连杆运动学时，最核心的澄清是：

每根连杆都像一个“有长度、有方向”的向量
末端位置是所有连杆向量首尾相接后的结果
q1 + q2 不是坐标相加，而是串联关节角度累加

4.4 雅可比矩阵是什么

对于两连杆，末端位置是关节角的函数：

[
x = f(q)
]

其中：

[
q = [q_1, q_2]^T
]

雅可比矩阵定义为：

[
J(q) = \frac{\partial x}{\partial q}
]

对应的速度关系为：

[
\dot{x} = J(q)\dot{q}
]

4.4.1 这条式子表达什么

它表示：

关节速度 \dot{q}
会通过 J(q)
映射为末端速度 \dot{x}

也就是说，雅可比矩阵是连接：

关节空间
任务空间

之间的局部线性桥梁。

4.4.2 两连杆 Jacobian 的具体形式

对末端位置分别对 q1、q2 求偏导，可以得到：

[
J =
\begin{bmatrix}

l_1 \sin q_1 - l_2 \sin(q_1 + q_2) & - l_2 \sin(q_1 + q_2) \
l_1 \cos q_1 + l_2 \cos(q_1 + q_2) & l_2 \cos(q_1 + q_2)
\end{bmatrix}
]

这就是代码中的 J。

重点

记住最关键的一条式子即可：

[
\dot{x} = J(q)\dot{q}
]

它在机器人中的用途非常广，后续会继续出现在：

逆运动学
力控制
阻抗控制
优化控制

4.5 控制律是什么

控制律可以理解为：

根据当前状态和目标状态，生成控制输入的规则。

4.5.1 这个最小项目中的控制律

先定义末端位置误差：

[
e = x_d - x
]

再给出一个简单的任务空间速度命令：

[
\dot{x}_{cmd} = K e
]

然后通过 Jacobian 逆映射到关节空间：

[
\dot{q} = J^{-1}\dot{x}_{cmd}
]

合起来就是：

[
\dot{q} = J^{-1}K(x_d - x)
]

4.5.2 它在做什么

可以按顺序理解：

末端当前在 x
目标末端位置是 x_d
两者的差值是误差 e
用 K 把误差变成一个期望运动方向和速度量级
再用 J^{-1} 把末端运动需求换算成关节速度命令

难点

这个例子里直接用了 J.inverse()，这样做只适合演示最小原理。

在真实工程中，需要注意：

Jacobian 可能不可逆
接近奇异位形时数值不稳定
更常见的做法是伪逆或阻尼伪逆

但在入门阶段，这个最小形式足够帮助建立：

误差
反馈
控制命令

三者之间的关系。

4.6 VS Code 打开Ubuntu工程

使用 cat 创建的文件，和用 VS Code 图形界面新建的文件没有区别。只要文件已经存在，就可以直接在 VS Code 中打开。

例如在 Ubuntu 中执行：

cd ~/ros2_ws
code .

然后在 VS Code 左侧打开：

src/embodied_minimal_cpp/src/minimal_robot_node.cpp
src/embodied_minimal_cpp/CMakeLists.txt
src/embodied_minimal_cpp/package.xml

重点

这里真正重要的不是文件“怎么创建”，而是 VS Code 是否以 WSL 模式打开工程。

应确认左下角显示：

WSL: Ubuntu

这样终端、依赖和路径才与 Ubuntu 中的 ROS2 工作区保持一致。

4.7 整个最小链路如何串起来

把工程层、数学层和控制层串起来，可以得到一条很清晰的最小闭环：

ROS2 workspace
  -> colcon build
    -> ament_cmake
      -> CMake
        -> g++ 编译 C++ 节点
          -> 节点运行
            -> 用 Eigen 表达向量/矩阵
              -> 计算两连杆正运动学
              -> 计算 Jacobian
              -> 根据误差计算控制输出

这条链路说明，机器人项目并不是只有算法公式，也不是只有环境配置。一个最小可运行系统通常至少包含三层：

工程构建层：工作区、构建系统、依赖管理
数学表示层：向量、矩阵、运动学关系
控制计算层：误差、映射、控制输出

5. 重点与难点整理

重点 1：区分各层工具的角色

colcon：工作区构建调度器
ament_cmake：ROS2 对 CMake 的构建规范层
CMake：通用构建系统
g++：真正执行编译的编译器
Eigen：数学表达与计算库

重点 2：区分变量类型

q1、q2：关节角
x、y：末端坐标
J：关节速度到末端速度的局部线性映射
qdot：控制输出的关节速度命令

难点 1：`q1 + q2` 的几何意义

这里最容易出错的是把角度和坐标混淆。需要明确：

第二根杆的方向是串联角度累加的结果
不是单独的 q2

难点 2：`ament_cmake` 和 CMake 的关系

容易把它们当成两套平行系统。更准确的理解是：

CMake 是底层通用构建系统
ament_cmake 是 ROS2 场景下的适配层

难点 3：控制律中的 `J.inverse()`

它适合用来解释基本思路，但不代表真实工程中总能直接用逆矩阵。

6. 拓展

在这个最小项目跑通并理解之后，可以继续沿着下面几步推进：

第一步：把代码和公式一一对应

对照代码，逐行回答：

这一行对应哪个数学对象
这一行在工程上做了什么
这一行输出了什么信息

第二步：让关节角随时间变化

把固定的 q1、q2 改成变化量，观察：

x 怎么变化
J 怎么变化
qdot 怎么变化

这样可以建立对“构型变化”与 Jacobian 变化的直观认识。

第三步：把常量改成 ROS 参数

例如把：

l1
l2
x_desired

改成可配置参数，开始接近真实工程中的节点结构。

第四步：接入消息输入输出

进一步升级为：

订阅关节状态
发布控制命令

这会让项目从“打印计算结果”走向“具备输入输出接口的最小机器人节点”。

7. 总结

这个最小项目的价值在于它把机器人开发中最关键的几层东西以最小形式连了起来：

用 colcon 组织 ROS2 工作区构建
用 ament_cmake 将 CMake 项目接入 ROS2 生态
用 Eigen 表达向量和矩阵
用两连杆模型理解最基本的正运动学
用 Jacobian 连接关节空间和任务空间
用最小控制律说明误差如何转成控制命令

如果把它当作入门模板，最重要的不是一次记住所有名词，而是沿着这条链路反复验证：

每个文件是干什么的
每条命令在调用哪一层工具
每个公式在代码中如何落地
每个变量在物理上代表什么

一旦这些问题能够逐步回答清楚，后续继续学习更复杂的机械臂控制、规划或具身智能系统时，就会更容易把新知识放到已有框架中。

从 0 跑通一个最小 ROS2 + C++ + CMake + Eigen 机器人项目