3. 第一个机械臂控制节点：创建ROS2功能包、编写Publisher与Subscriber节点、使用rqt_graph可视化节点通信、launch文件启动多节点

好，到了真正动手写代码的时候了。

前面两章我们把ROS2的环境搭好，也搞清楚了机械臂的URDF模型长什么样。但机器人要动起来，光有模型可不行——你得有“大脑”去指挥它。这个大脑，就是ROS2里的节点。

这一章，我们就从零开始，创建你的第一个机械臂控制节点。我会带你走完整个流程：建功能包、写发布者、写订阅者、用rqt_graph看通信，最后用launch文件一键启动。每一步我都会穿插一些实际项目里踩过的坑，希望能帮你少走弯路。

本章核心目标：掌握ROS2节点开发的标准流程，理解发布-订阅模型，学会用launch文件管理多节点。

3.1 创建ROS2功能包

在ROS2里，代码不是随便扔在文件夹里的。你得先创建一个功能包（package）。它就像是一个项目文件夹，里面包含了代码、配置文件、依赖声明等等。

我个人习惯用ament_cmake作为构建类型，因为后面要跟C++写的运动学库打交道，CMake更灵活。当然，纯Python项目也可以用ament_python，看个人喜好。

3.1.1 创建工作空间

先确保你有一个工作空间。如果没有，按下面的命令创建：

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
colcon build

3.1.2 创建功能包

进入src目录，用ros2 pkg create命令创建包：

cd ~/ros2_ws/src
ros2 pkg create robot_arm_control \
  --build-type ament_cmake \
  --dependencies rclcpp std_msgs geometry_msgs

这里我指定了三个依赖：

  • rclcpp：ROS2的C++客户端库，写节点必须的。
  • std_msgs：标准消息类型，比如字符串、整数。
  • geometry_msgs：几何消息类型，里面有点、姿态、位姿等，控制机械臂关节位置时会用到。

小技巧：如果你不确定需要哪些依赖，可以先不写--dependencies，后面在package.xml和CMakeLists.txt里手动添加也行。但我建议一开始就列清楚，省得后面漏掉。

3.2 编写Publisher节点：发布关节角度指令

第一个节点，我们写一个发布者（Publisher）。它的任务很简单：每隔一段时间，发布一组关节角度值。你可以把它想象成“指挥官”，不断给机械臂下达位置指令。

3.2.1 创建源文件

在robot_arm_control/src目录下，新建一个文件joint_command_publisher.cpp：

#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

using namespace std::chrono_literals;

class JointCommandPublisher : public rclcpp::Node
{
public:
  JointCommandPublisher() : Node("joint_command_publisher")
  {
    publisher_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_commands", 10);
    timer_ = this->create_wall_timer(100ms, std::bind(&JointCommandPublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto message = sensor_msgs::msg::JointState();
    message.header.stamp = this->now();
    message.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"};
    message.position = {0.0, 0.5, -0.3, 0.0, 0.2, 0.0};

    RCLCPP_INFO(this->get_logger(), "Publishing joint angles: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]",
                message.position[0], message.position[1], message.position[2],
                message.position[3], message.position[4], message.position[5]);

    publisher_->publish(message);
  }

  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

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

这段代码的核心逻辑：

  • 创建一个名为joint_command_publisher的节点。
  • 在joint_commands话题上发布JointState消息。
  • 每100毫秒触发一次回调，发布6个关节的角度值。

注意：JointState消息里的name数组顺序，必须和你的机械臂URDF里定义的关节顺序一致。我曾经在这个地方吃过亏——顺序搞反了，结果机械臂扭成了麻花。嗯，调试了整整一个下午才发现。

3.3 编写Subscriber节点：接收并打印关节状态

有发布者，就得有订阅者（Subscriber）。这个节点负责接收关节指令，然后打印出来。在实际项目中，订阅者可能是真正的硬件驱动，负责把指令发给电机。这里我们先做模拟，看看通信是否正常。

3.3.1 创建订阅者源文件

在同一个src目录下，新建joint_state_subscriber.cpp：

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

class JointStateSubscriber : public rclcpp::Node
{
public:
  JointStateSubscriber() : Node("joint_state_subscriber")
  {
    subscription_ = this->create_subscription<sensor_msgs::msg::JointState>(
      "joint_commands", 10, std::bind(&JointStateSubscriber::topic_callback, this, std::placeholders::_1));
  }

private:
  void topic_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "Received joint angles:");
    for (size_t i = 0; i < msg->name.size(); ++i)
    {
      RCLCPP_INFO(this->get_logger(), "  %s: %.2f", msg->name[i].c_str(), msg->position[i]);
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
};

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

这个节点订阅了同一个话题joint_commands。每当发布者发来消息，它就会把每个关节的名字和角度打印出来。

3.4 配置CMakeLists.txt并编译

代码写完了，得告诉CMake怎么编译。打开CMakeLists.txt，在底部添加：

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

add_executable(joint_command_publisher src/joint_command_publisher.cpp)
ament_target_dependencies(joint_command_publisher rclcpp std_msgs sensor_msgs)

add_executable(joint_state_subscriber src/joint_state_subscriber.cpp)
ament_target_dependencies(joint_state_subscriber rclcpp std_msgs sensor_msgs)

install(TARGETS
  joint_command_publisher
  joint_state_subscriber
  DESTINATION lib/${PROJECT_NAME}
)

然后编译：

cd ~/ros2_ws
colcon build --packages-select robot_arm_control

避坑指南：编译时如果报错找不到sensor_msgs，别忘了在package.xml里加上依赖声明：<depend>sensor_msgs</depend>。我刚开始学ROS2时经常漏这一步，编译报错一脸懵。

3.5 运行节点并使用rqt_graph可视化

编译成功后，先打开一个终端，source环境：

cd ~/ros2_ws
source install/setup.bash
ros2 run robot_arm_control joint_command_publisher

再开第二个终端，运行订阅者：

source ~/ros2_ws/install/setup.bash
ros2 run robot_arm_control joint_state_subscriber

你应该能看到订阅者终端里不断打印出关节角度信息。这说明两个节点已经通过话题成功通信了。

但光看日志还不够，我们得用rqt_graph来可视化一下节点间的通信关系。打开第三个终端：

rqt_graph

你会看到一个清晰的图：一个椭圆节点（发布者）指向一个椭圆节点（订阅者），中间连着一条线，线上标着话题名/joint_commands。

为什么会这样？说白了，rqt_graph就是ROS2的“通信地图”。它能帮你快速定位：哪个节点在发消息？哪个节点在收消息？话题名对不对？

实际项目经验：我在调试多机器人协作系统时，经常同时打开rqt_graph和rqt_console。一个看拓扑结构，一个看日志输出。这两个工具配合使用，能解决90%的通信问题。

3.6 使用launch文件启动多节点

每次开三个终端，source三次，太麻烦了。ROS2提供了launch文件，可以一键启动多个节点。

3.6.1 创建launch文件

在robot_arm_control包目录下，新建launch文件夹，然后创建arm_control.launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='robot_arm_control',
            executable='joint_command_publisher',
            name='joint_command_publisher',
            output='screen'
        ),
        Node(
            package='robot_arm_control',
            executable='joint_state_subscriber',
            name='joint_state_subscriber',
            output='screen'
        ),
    ])

这个launch文件定义了两个节点，并指定了它们的可执行文件、节点名和输出方式。

3.6.2 配置CMakeLists.txt安装launch文件

在CMakeLists.txt末尾添加：

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

3.6.3 一键启动

重新编译后，运行：

ros2 launch robot_arm_control arm_control.launch.py

两个节点同时启动，日志一起打印出来。是不是清爽多了？

注意：launch文件里的executable名称，必须和CMakeLists.txt里add_executable定义的名字一致。大小写敏感，别写错了。我曾经因为把joint_command_publisher写成了joint_command_Publisher，折腾了半小时没启动起来。

3.7 本章知识体系总览

下面这张图，帮你把本章的核心逻辑串起来：

ROS2节点通信与启动流程 robot_arm_control joint_command_publisher /joint_commands joint_state_subscriber rqt_graph 可视化 arm_control.launch.py 💡 核心流程说明 1. 功能包包含两个节点：发布者与订阅者 2. 发布者通过话题 /joint_commands 发送关节角度指令 3. launch文件可一键启动所有节点，rqt_graph可实时查看通信拓扑

3.8 本章小结

这一章我们走完了ROS2节点开发的完整闭环：

  • 创建功能包：用ros2 pkg create搭建项目骨架。
  • 编写发布者：定时发布JointState消息，模拟关节指令。
  • 编写订阅者：接收并打印关节角度，验证通信是否正常。
  • rqt_graph可视化：直观看到节点和话题的拓扑关系。
  • launch文件：一键启动多节点，告别手动开终端。
← 上一章 📖 返回目录 下一章 →