ROS2实战：用rclcpp从零搭建你的第一个机器人节点（附完整代码）

在机器人开发领域，ROS2已经成为事实上的标准框架。与ROS1相比，ROS2在实时性、跨平台支持和分布式系统方面都有了显著提升。而rclcpp作为ROS2的C++客户端库，为开发者提供了强大而灵活的工具集。本文将带你从零开始，一步步构建你的第一个ROS2机器人节点，涵盖环境配置、节点创建、消息发布与订阅等核心功能。

1. 环境准备与基础配置

在开始编写代码之前，我们需要确保开发环境已经正确配置。ROS2支持多种操作系统，包括Ubuntu、Windows和macOS。这里我们以Ubuntu 20.04和ROS2 Foxy Fitzroy为例进行说明。

首先，确保你已经安装了ROS2 Foxy版本。可以通过以下命令验证安装是否成功：

source /opt/ros/foxy/setup.bash
ros2 --version

如果系统正确返回了ROS2的版本信息，说明基础环境已经就绪。接下来，我们需要创建一个工作空间（workspace）来组织我们的代码：

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

提示：每次打开新的终端窗口时，都需要通过source /opt/ros/foxy/setup.bash命令初始化ROS2环境。

为了开发C++节点，我们还需要安装一些必要的编译工具和依赖项：

sudo apt install build-essential cmake git
sudo apt install ros-foxy-rclcpp ros-foxy-std-msgs

2. 创建第一个ROS2节点

现在，让我们开始编写第一个ROS2节点。我们将创建一个简单的节点，它会定期向控制台输出"Hello ROS2"消息。

首先，在工作空间的src目录下创建一个新的包：

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake my_first_robot_node --dependencies rclcpp

这会在src目录下创建一个名为my_first_robot_node的新包。包的结构如下：

my_first_robot_node/
├── CMakeLists.txt
├── include
├── package.xml
└── src

在src目录下创建一个新的C++源文件simple_node.cpp，并添加以下内容：

#include "rclcpp/rclcpp.hpp"

class SimpleNode : public rclcpp::Node {
public:
    SimpleNode() : Node("simple_node") {
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&SimpleNode::timer_callback, this));
    }

private:
    void timer_callback() {
        RCLCPP_INFO(this->get_logger(), "Hello ROS2");
    }
    rclcpp::TimerBase::SharedPtr timer_;
};

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

接下来，我们需要修改CMakeLists.txt文件，添加可执行文件的构建规则：

add_executable(simple_node src/simple_node.cpp)
ament_target_dependencies(simple_node rclcpp)

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

现在，我们可以编译并运行这个节点了：

cd ~/ros2_ws
colcon build --packages-select my_first_robot_node
source install/setup.bash
ros2 run my_first_robot_node simple_node

如果一切正常，你应该会在终端中看到每隔0.5秒输出的"Hello ROS2"消息。

3. 实现节点间通信

ROS2的核心功能之一就是节点间的通信。让我们扩展我们的示例，创建一个发布者和一个订阅者，实现两个节点之间的消息传递。

首先，在同一个包中创建一个新的源文件talker_listener.cpp：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class Talker : public rclcpp::Node {
public:
    Talker() : Node("talker") {
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
        timer_ = this->create_wall_timer(
            500ms, std::bind(&Talker::timer_callback, this));
    }

private:
    void timer_callback() {
        auto message = std_msgs::msg::String();
        message.data = "Hello from talker!";
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

class Listener : public rclcpp::Node {
public:
    Listener() : Node("listener") {
        subscription_ = this->create_subscription<std_msgs::msg::String>(
            "topic", 10, std::bind(&Listener::topic_callback, this, std::placeholders::_1));
    }

private:
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {
        RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::executors::SingleThreadedExecutor executor;
    
    auto talker = std::make_shared<Talker>();
    auto listener = std::make_shared<Listener>();
    
    executor.add_node(talker);
    executor.add_node(listener);
    executor.spin();
    
    rclcpp::shutdown();
    return 0;
}

同样，我们需要在CMakeLists.txt中添加新的构建规则：

add_executable(talker_listener src/talker_listener.cpp)
ament_target_dependencies(talker_listener rclcpp std_msgs)

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

编译并运行这个示例：

cd ~/ros2_ws
colcon build --packages-select my_first_robot_node
source install/setup.bash
ros2 run my_first_robot_node talker_listener

你会看到控制台交替输出发布和接收的消息：

[INFO] [talker]: Publishing: 'Hello from talker!'
[INFO] [listener]: I heard: 'Hello from talker!'

4. 参数与服务的使用

ROS2节点可以暴露参数和服务，使得其他节点可以动态配置和交互。让我们创建一个带有参数和服务的节点示例。

创建一个新的源文件param_service_node.cpp：

#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"

using namespace std::chrono_literals;

class ParamServiceNode : public rclcpp::Node {
public:
    ParamServiceNode() : Node("param_service_node") {
        // 声明参数
        this->declare_parameter("my_parameter", "default_value");
        
        // 获取参数值
        std::string param_value = this->get_parameter("my_parameter").as_string();
        RCLCPP_INFO(this->get_logger(), "Parameter value: %s", param_value.c_str());
        
        // 创建服务
        service_ = this->create_service<example_interfaces::srv::AddTwoInts>(
            "add_two_ints",
            std::bind(&ParamServiceNode::add, this, std::placeholders::_1, std::placeholders::_2));
    }

private:
    void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
             std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) {
        response->sum = request->a + request->b;
        RCLCPP_INFO(this->get_logger(), "Incoming request\na: %ld b: %ld", request->a, request->b);
        RCLCPP_INFO(this->get_logger(), "Sending back response: [%ld]", response->sum);
    }
    rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service_;
};

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

更新CMakeLists.txt，添加新的依赖和可执行文件：

find_package(example_interfaces REQUIRED)

add_executable(param_service_node src/param_service_node.cpp)
ament_target_dependencies(param_service_node rclcpp example_interfaces)

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

编译并运行这个节点：

cd ~/ros2_ws
colcon build --packages-select my_first_robot_node
source install/setup.bash
ros2 run my_first_robot_node param_service_node

在另一个终端中，我们可以测试服务调用：

ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 5, b: 3}"

你应该会看到服务节点输出请求和响应信息，同时服务调用方会收到计算结果。

5. 构建完整的机器人节点

现在，我们将这些概念整合起来，构建一个更完整的机器人节点示例。这个节点将：

1. 定期发布传感器数据
2. 接收控制命令
3. 提供配置服务
4. 使用参数进行初始化

创建一个新的源文件robot_node.cpp：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "robot_interfaces/srv/set_config.hpp"

using namespace std::chrono_literals;

class RobotNode : public rclcpp::Node {
public:
    RobotNode() : Node("robot_node") {
        // 声明和获取参数
        this->declare_parameter("robot_name", "default_robot");
        this->declare_parameter("max_speed", 1.0);
        
        robot_name_ = this->get_parameter("robot_name").as_string();
        max_speed_ = this->get_parameter("max_speed").as_double();
        
        RCLCPP_INFO(this->get_logger(), "Starting %s with max speed: %.2f", 
                   robot_name_.c_str(), max_speed_);
        
        // 创建发布者
        scan_publisher_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
        
        // 创建订阅者
        cmd_vel_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", 10, std::bind(&RobotNode::cmd_vel_callback, this, std::placeholders::_1));
        
        // 创建服务
        config_service_ = this->create_service<robot_interfaces::srv::SetConfig>(
            "set_config",
            std::bind(&RobotNode::set_config, this, std::placeholders::_1, std::placeholders::_2));
        
        // 创建定时器
        timer_ = this->create_wall_timer(
            100ms, std::bind(&RobotNode::timer_callback, this));
    }

private:
    void timer_callback() {
        auto scan_msg = sensor_msgs::msg::LaserScan();
        // 填充模拟的激光扫描数据
        scan_msg.header.stamp = this->now();
        scan_msg.header.frame_id = "laser_frame";
        scan_msg.angle_min = -1.57;
        scan_msg.angle_max = 1.57;
        scan_msg.angle_increment = 0.1;
        scan_msg.time_increment = 0.01;
        scan_msg.scan_time = 0.1;
        scan_msg.range_min = 0.1;
        scan_msg.range_max = 10.0;
        scan_msg.ranges = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0};
        
        scan_publisher_->publish(scan_msg);
    }
    
    void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
        // 根据最大速度限制实际速度
        double linear_x = std::min(std::max(msg->linear.x, -max_speed_), max_speed_);
        double angular_z = std::min(std::max(msg->angular.z, -max_speed_), max_speed_);
        
        RCLCPP_INFO(this->get_logger(), "Received command: linear=%.2f, angular=%.2f", 
                   linear_x, angular_z);
        // 这里可以添加实际控制机器人的代码
    }
    
    void set_config(const std::shared_ptr<robot_interfaces::srv::SetConfig::Request> request,
                    std::shared_ptr<robot_interfaces::srv::SetConfig::Response> response) {
        if (request->max_speed > 0) {
            max_speed_ = request->max_speed;
            response->success = true;
            response->message = "Configuration updated successfully";
            RCLCPP_INFO(this->get_logger(), "New max speed set to: %.2f", max_speed_);
        } else {
            response->success = false;
            response->message = "Invalid max speed value";
        }
    }
    
    std::string robot_name_;
    double max_speed_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_publisher_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
    rclcpp::Service<robot_interfaces::srv::SetConfig>::SharedPtr config_service_;
    rclcpp::TimerBase::SharedPtr timer_;
};

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

这个示例展示了如何构建一个功能相对完整的机器人节点，包含了ROS2中最常用的几种通信模式。在实际项目中，你可以基于这个框架继续扩展功能，如添加更多的传感器接口、实现导航算法或集成机器学习模型等。