ROS2核心基础

节点

什么是节点（What is Node）

节点的定义和概念

• 节点： ROS2系统中的基本执行单元，是一个独立的进程
• 功能职责： 每个节点负责特定的功能，如传感器处理、运动控制、决策等
• 通信机制： 节点间通过话题、服务、动作进行通信

节点的特性

节点特性：
├── 独立性 # 每个节点是独立的进程，崩溃不影响其他节点
├── 可重用性 # 同一节点可以启动多个实例
├── 分布式 # 节点可以运行在不同的机器上
├── 命名空间 # 支持命名空间隔离
└── 生命周期 # 具有完整的生命周期管理

节点的组成要素

• 基础标识
  • 节点名称和命名空间-确保节点在系统中的唯一识别和组织管理
• 唯一标识符-防止节点名称冲突，支持分布式部署
• 通信接口
• **发布者：**异步发送数据到话题，实现一对多广播通信
• **订阅者：**异步接收话题数据，支持多个订阅者同时监听
• **服务服务器：**提供同步请求-响应服务，处理客户端调用
• **服务客户端：**发起同步请求，等地啊服务器响应
• 动作服务器：处理长时间运行的任务，提供进度反馈
• 动作客户端：发起异步任务请求，接收执行状态和结果
• 配置管理
  • 参数声明和验证：定义节点配置项，确保参数类型和范围正确性
• 参数回调处理：监听参数变化。实现动态配置响应
• 动态参数更新：运行时修改节点行为，无需重启节点
• 时间管理
  • 周期性定时器：按固定间隔执行任务，如传感器数据采集
• 一次性定时器：延迟执行单次任务，用于超时处理
• 时间戳处理：记录和同步事件和时间，确保数据正确
• 日志系统
  • 多级别日志记录：支持DEBUG/INFO/WARN/ERROR等不同重要性级别
• 格式化输出：统一日志格式，便于分析和调试
• 调试信息：提供运行时状态信息，帮助问题定位
• 执行控制
  • 回调组管理：组织和调度不同类型的回调函数
• 优先级控制：确保重要人物优先执行
• 并发处理：支持多线程执行，提高系统响应性能
• 生命周期
  • 状态管理：跟踪节点当前运行状态
• 配置/激活/停用/清理：标准化节点启动、运行、停止流程
• 状态转换：确保节点安全可靠的在不同状态间切换

节点编程方法

C++节点编程基础

• 创建C++功能包

# 进入工作空间src目录
cd ~/ros2_ws/src

# 创建简单的C++功能包
ros2 pkg create --build-type ament_cmake hello_cpp_node --dependencies rclcpp

命令解析：

• ros2 pkg create
  • ROS2的包创建工具
• 用于自动生成标准ROS2包结构
• –build-type ament_cmake
• 指定构建系统类型为ament_cmake
• ament_cmake是ROS2推荐的C++包构建系统
• 基于CMake，提供ROS2特点的构建功能
• hello_cpp_node
• 包名称
• 将创建名为hello_cpp_node的ROS2包
• 包名应遵循小写字母和下划线的命名规范
• –dependencies rclcpp
• 声明包依赖项
• rclcpp是ROS2的C++客户端库
• 自动在package.xml和CMakeLists.txt中添加此依赖项

**执行结果：**该命令将创建以下目录结构：

hello_cpp_node/
├── CMakeLists.txt          # CMake构建配置文件
├── package.xml             # 包元数据和依赖声明
├── src/                    # 源代码目录
└── include/hello_cpp_node/ # 头文件目录

编写节点代码

src/hello_cpp_node/src/hello_node.cpp
这个范例启动一个定时器，间隔一秒打印信息。

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <string>

// chrono字面量，方便时间单位标识（1s表示1秒）
using namespace std::chrono_literals;

class HelloNode : public rclcpp::Node {

public:
    HelloNode() : Node("hello_node"), count_(0) {
        // 创建墙时钟定时器，每1秒执行一次timer_callback函数
        // create_wall_timer使用系统时钟，不受ROS时钟影响
        timer_ = this->create_wall_timer(
            1s, std::bind(&HelloNode::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "Hello ROS2 node setup!");
    }

private:

    void timer_callback() {
        RCLCPP_INFO(this->get_logger(), "Hello ROS2! count_: %ld", count_++);
    }

    rclcpp::TimerBase::SharedPtr timer_;
    uint32_t count_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    auto node = std::make_shared<HelloNode>();

    // 进入ROS2事件循环
    // 这个函数会被阻塞直到节点关闭
    rclcpp::spin(node);

    // 清理ros2资源，关闭通信中间件
    rclcpp::shutdown();

    return 0;
}

设置编译选项

src/hello_cpp_node/CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(hello_cpp_node)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(hello_node src/hello_cpp_node.cpp)
ament_target_dependencies(hello_node rclcpp)

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

ament_package()

编译运行

# --run--
cd ~/ros2_ws
# 编译指定包
colcon build --packages-select hello_cpp_node

source install/setup.bash
# 运行 [节点] [节点中的二进制文件]
ros2 run hello_cpp_node hello_node

节点命令行操作

在hello_node运行时的操作命令

节点信息查看

# 查看所有运行的节点
ros2 node list

# 查看节点详细信息
ros2 node info /my_node
# 比如
ros2 node info /hello_node

# 实时查看节点信息
watch -n 1 "ros2 node list"

# 可以停止hello_node，然后启动hello_node，观察watch信息

通信机制-话题

什么是话题

定义和概念

• **话题：**ROS2中节点间进行异步数据传输的命名通道
• **作用：**作为数据传递的桥梁，连接发布者和订阅者
• **特点：**基于发布-订阅模式，支持一对多、多对一、多对多通信

话题组成要素

话题系统：
├── 话题名称 # 唯一标识符，如 "/sensor_data"
├── 消息类型 # 数据格式定义，如 std_msgs/String
├── 发布者 # 发送数据的节点
└── 订阅者 # 接收数据的节点

话题vs其他通信方式

• **话题：**异步、多对多、持续数据流
• **服务：**同步、一对一、请求-响应
• **动作：**异步、长时间任务、带反馈

话题通信模型

多对多通信

通信模式示例：
发布者1 ──┐
发布者2 ──┼──► 话题 "/data" ──┼──► 订阅者1
发布者3 ──┘                   ├──► 订阅者2
                              └──► 订阅者3

异步通信特点

• 发布者： 发送消息后立刻返回，不等待接收确认
• 订阅者： 通过回调函数异步处理接收数据
• 解耦性： 发布审核和订阅者不需要知道对方的存在

消息接口

• 标准消息类型：std_msgs，geometry_msgs，sensor_msgs等
  • std_msgs：基础数据类型

//基本数据类型
std_msgs::msg::String text_msg;
text_msg.data = "Hallo ROS2"

std_msgs::msg::Int32 number_msg;
number_msg.data = 42;

// 数组类型
std_msgs::msg::Float64MultiArray array_msg;
array_msg.data = {1.0, 2.0, 3.0, 4.0};
array_msg.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
array_msg.layout.dim[0].size = 4;
array_msg.layout.dim[0].stride = 4;
array_msg.layout.dim[0].label = "values;

- geometry_msgs-集合和空间信息
    * 位置信息 geometry_msgs::msg::Point
    * 四元数标识旋转 geometry_msgs::msg::Quaternion
    * 位姿信息（位置+旋转） geometry_msgs::msg::Pose
    * 带时间戳的位姿 geometry_msgs::msg::PoseStamped
    * 速度信息 geometry_msgs::msgs::msg::Twist
    * 路径信息 nav_msgs::msg::Path
- sensor_msgs-传感器数据
    * 图像数据 sensor_msgs::msg::Image
    * 激光扫描数据 sensor_msgs::msg::LaserScan
    * 点云数据 sensor_msgs::msg::PointCloud2
    * IMU数据 sensor_msgs::msg::Imu

• 自定义消息： 用户可以定义特定的消息格式
  • 消息定义文件（.msg） 定义数据结构

# 在msg/目录下创建 RobotStatus.msg
# msg/RobotStatus.msg
string robot_name
geometry_msgs/Pose current_pose
float64 battery_level
bool is_moving
int32[] sensor_readings
RobotMode mode # 自定义枚举

# 常量定义
int32 STATUS_IDLE = 0
int32 STATUS_MOVING = 0
int32 STATUS_CHARGING = 2

• QoS（服务质量）： 控制星系传输的可靠性和延迟
  • QoS配置参数节选

#include <rclcpp/qos.hpp>

class QosExampleNode: public rclcpp::Node {
public:
    QosExampleNode() : Node("qos_example") {
        // 可靠性设置
        auto reliable_qos = rclcpp::QoS(10)
            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) // 可靠传输
            .durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); // 持久化

        auto best_effort_qos = rclcpp::Qos(10)
            .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        
        ...
    }
}

C++话题编程示例

创建话题通讯功能包

# 创建功能包
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake topic_example \
    --dependencies rclcpp std_msgs

命令解析：

• ros2 pkg create
  • ros2：ROS2命令行工具
• pkg：包管理子命令
• create： 创建新包的操作
• –build-type ament_cmake
  • –build-type：指定构建系统类型
• ament_cmake：使用CMake构建系统，这是C++包的标准构建方式
• 其他选项还有：ament_python
• topic_example
  • 这是想要创建的包名
• 包名必须符合ROS2的命名约定
• –dependencies rclcpp std_msgs
• –dependencies：指定包的依赖
• rclcpp：ROS2的C++客户端库，C++节点开发的核心库
• std_msgs：标准消息类型库，包含基本的消息类型如String，Int32等

简单的发布者示例

源文件
src/topic_example/src/simple_publisher.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

using namespace std::chrono_literals;

class SimplePublisher : public rclcpp::Node {

public:
    SimplePublisher() : Node("simple_publisher"), count_(0) {
        publisher_ = this->create_publisher<std_msgs::msg::String>("hello_topic", 10);

        timer_ = this->create_wall_timer(
            2s, std::bind(&SimplePublisher::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "simple_publisher node setup success");
    }

private:

    void timer_callback() {
        auto message = std_msgs::msg::String();

        message.data = "Hello ROS2! message id:" + std::to_string(count_);

        RCLCPP_INFO(this->get_logger(), "publish topic: '%s'", message.data.c_str());

        publisher_->publish(message);

        count_++;
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    uint32_t count_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<SimplePublisher>());

    rclcpp::shutdown();
    
    return 0;
}

简单的订阅者示例

源文件
src/topic_example/src/simple_subscriber.cpp

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <chrono>

class SimpleSubscriber : public rclcpp::Node {

public:
    SimpleSubscriber() : Node("simple_subscriber") {
        subscription_ = this->create_subscription<std_msgs::msg::String>("hello_topic", 10,
        std::bind(&SimpleSubscriber::topic_callback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "simple_subscriber setup success");
    }

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

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<SimpleSubscriber>());

    rclcpp::shutdown();

    return 0;
}

稍微复杂的发布者示例

源文件
src/topic_example/src/number_publisher

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32.hpp>
#include <chrono>
using namespace std::chrono_literals;

class NumberPublisher : public rclcpp::Node {
public:
    NumberPublisher() : Node("number_publisher") {
        publisher_ = this->create_publisher<std_msgs::msg::Int32>("numbers", 10);
        timer_ = this->create_wall_timer(1s, std::bind(&NumberPublisher::publisher_number_callback, this));

        RCLCPP_INFO(this->get_logger(), "number publisher setup success");
    }

private:
    void publisher_number_callback() {
        auto msg = std_msgs::msg::Int32();

        msg.data = number_;

        RCLCPP_INFO(this->get_logger(), "publish number: %ld", number_);

        number_++;

        publisher_->publish(msg);
    }
    uint32_t number_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<NumberPublisher>());

    rclcpp::shutdown();

    return 0;
}

稍微复杂的订阅者示例

源文件
src/topic_example/src/number_subscriber.cpp

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32.hpp>


class NumberSubscriber : public rclcpp::Node {

public:

    NumberSubscriber() : Node("number_subscriber"), sum_(0) {
        subscription_ = this->create_subscription<std_msgs::msg::Int32>(
            "numbers", 10,
            std::bind(&NumberSubscriber::number_callback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "number_subscriber setup success");
    }
    

private:
    void number_callback(const std_msgs::msg::Int32::SharedPtr msg) {
        sum_ += msg->data;

        RCLCPP_INFO(this->get_logger(), "recv num: %d, sum: %d", msg->data, sum_);
    }
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_;

    uint32_t sum_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<NumberSubscriber>());

    rclcpp::shutdown();

    return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(topic_example)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(simple_publisher src/simple_publisher.cpp)
add_executable(simple_subscriber src/simple_subscriber)
add_executable(number_publisher src/number_publisher)
add_executable(number_subscriber src/number_subscriber)
ament_target_dependencies(simple_publisher rclcpp std_msgs)
ament_target_dependencies(simple_subscriber rclcpp std_msgs)
ament_target_dependencies(number_publisher rclcpp std_msgs)
ament_target_dependencies(number_subscriber rclcpp std_msgs)

install(
  TARGETS
  simple_publisher
  simple_subscriber
  number_publisher
  number_subscriber
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

编译运行

程序运行后，可以使用`watch -n 1 "ros2 node list"实时查看话题

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

source install/setup.bash

ros2 run topic_example simple_publisher

ros2 run topic_example simple_subscriber

命令行查看话题操作示例

# 查看所有话题
ros2 topic list

# 查看话题详细信息
ros2 topic info /hello_topic

# 查看话题消息类型
ros2 topic type /hello_topic

# 实时监听话题消息
ros2 topic echo /hello_topic

# 查看话题发布频率
ros2 topic hz /hello_topic

# 查看话题带宽使用
ros2 topic bw /hello_topic

命令行发布消息

# 发布字符串消息
ros2 topic pub /hello_topic std_msgs/msg/String '{data: "命令行发布的消息"}'

# 持续发布消息
ros2 topic pub /hello_topic std_msgs/msg/Stirng '{data: "持续消息"}' --rate 0.5

# 发布数值消息
ros2 topic pub /numbers std_msgs/msg/Int32 '{data: 42}'

# 发布一次后退出
ros2 topic pub --once /hello_topic std_msgs/msg/String '{data: "单次消息"}'

多节点通信测试

# 同时运行多个发布者和订阅者
# 终端1：发布者1
ros2 run topic_example simple_publisher
# 终端2：发布者2（重映射话题名）
ros2 run topic_example simple_publisher --ros-args --remap hello_topic:=hello_topic
# 终端3：订阅者1
ros2 run topic_example simple_subscriber
# 终端4：订阅者2
ros2 run topic_example simple_subscriber
# 终端5：查看话题连接
ros2 topic info /hello_topic

通信机制-服务

什么是服务

服务通信图示

服务通信模型

同步通信特点

异步通信特点

一对多通信模式

• **一个服务端：**提供特定功能的节点
• **多个客户端：**可以同时向同一个服务端发送请求
• 队列处理：服务端按顺序处理客户端请求

服务接口组成

这里的.srv 是重要技术点

服务接口文件 (.srv):
├── 请求部分 (Request)
│ ├── 输入参数1
│ ├── 输入参数2
│ └── ...
├── 分隔符 (---)
└── 响应部分 (Response)
    ├── 输出参数1
    ├── 输出参数2
    └── ...

服务通信 C++编程示例

创建服务通信功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake service_example \
  --dependencies rclcpp std_srvs

命令解析：

• std_srvs：标准服务类型库

与 topic_example 的区别：

方面	topic_example	service_example
通信模式	发布-订阅	请求-响应
消息库	std_msgs	std_srvs
通信特点	异步、持续	同步、临时

简单的服务端示例

这里的请求响应格式：

Request:
  bool data
Response:
  bool success,
  string message

源文件

src/service_example/src/simple_service_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/set_bool.hpp>
#include <memory>

class SimpleServiceServer : public rclcpp::Node {

public:
    SimpleServiceServer() : Node("simple_serive_server"), robot_enabled_(false) {
        service_ = this->create_service<std_srvs::srv::SetBool>(
            "robot_control",
            std::bind(&SimpleServiceServer::handle_servive, this,
            std::placeholders::_1, std::placeholders::_2));

        RCLCPP_INFO(this->get_logger(), "simple_service_server setup success");
    }

private:

    void handle_servive(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
        std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
            robot_enabled_ = request->data;

            response->success = true;
            
            if (robot_enabled_) {
                response->message = "robot setup success";
                RCLCPP_INFO(this->get_logger(), "get request, robot setup success");
            }
            else {
                response->message = "robot down success";
                RCLCPP_INFO(this->get_logger(), "get request, robot down success");
            }

            RCLCPP_INFO(this->get_logger(), "server response send");
        }

    rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_;

    bool robot_enabled_;
};


int  main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<SimpleServiceServer>());

    rclcpp::shutdown();

    return 0;
}

简单的客户端示例

源文件

src/service_example/src/simple_service_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/set_bool.hpp>

#include <chrono>
#include <memory>

using namespace std::chrono_literals;

class SimpleServieClient : public rclcpp::Node {

public:
    SimpleServieClient() : Node("simple_service_client") {
        client_ = create_client<std_srvs::srv::SetBool>("robot_control");

        RCLCPP_INFO(this->get_logger(), "simple_service_client setup success");
        // 等待服务变为可用状态
        // 这是一个阻塞循环，直到服务上线

        while (!client_->wait_for_service(1s)) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "waiting service break...");
                return;
            }

            RCLCPP_INFO(this->get_logger(), "waiting service online...");
        }

        send_request(true);

        std::this_thread::sleep_for(2s);

        send_request(false);
    }

private:

    void send_request(bool enable) {
        auto request = std::make_shared<std_srvs::srv::SetBool::Request>();

        request->data = enable;

        RCLCPP_INFO(this->get_logger(), "send request: %s robot", enable ? "enable" : "disable");
        
        auto future = client_->async_send_request(request);

        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) {
            auto response = future.get();

            RCLCPP_INFO(this->get_logger(), "get response: success=%s, message='%s'",
                response->success ? "ture" : "false", response->message.c_str());
        }
        else {
            RCLCPP_ERROR(this->get_logger(), "request server error");
        }
    }

    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
};


int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<SimpleServieClient>());

    rclcpp::shutdown();
    
    return 0;
}

自定义服务 接口示例

首先创建自定义服务接口：

src/service_example/srv/AddTwoInts.srv

int64 a
int64 b
---
int64 sum
string message

自定义服务 服务端示例

src/service_example/src/add_service_server.cpp

#include <iostream>
#include <service_example/srv/add_two_ints.hpp>
#include <rclcpp/rclcpp.hpp>
#include <memory>

class AddServiceServer : public rclcpp::Node {
public:
    AddServiceServer() : Node("add_service_server") {
        service_ = create_service<service_example::srv::AddTwoInts>(
            "add_two_ints",
            std::bind(&AddServiceServer::add_callback, this,
                std::placeholders::_1, std::placeholders::_2));

        RCLCPP_INFO(this->get_logger(), "add service setup success");
    }
private:
    void add_callback(
        const std::shared_ptr<service_example::srv::AddTwoInts::Request> request,
        std::shared_ptr<service_example::srv::AddTwoInts::Response> response
    ) {
        response->sum = request->a + request->b;

        response->message = "calc finished";

        RCLCPP_INFO(this->get_logger(), "get req: %d + %d = %d", request->a, request->b, response->sum);
    }

    rclcpp::Service<service_example::srv::AddTwoInts>::SharedPtr service_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<AddServiceServer>());

    rclcpp::shutdown();
    
    return 0;
}

自定义服务 客户端示例

src/service_example/src/add_service_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <service_example/srv/add_two_ints.hpp>
#include <chrono>

using namespace std::chrono_literals;

class AddServiceClient : public rclcpp::Node {
public:

    AddServiceClient() : Node("add_service_client") {
        client_ = create_client<service_example::srv::AddTwoInts>("add_two_ints");

        while (!client_->wait_for_service(1s))
        {
            /* code */
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "wait for service break...");
                return;
            }

            RCLCPP_INFO(this->get_logger(), "wait for add service...");
        }
        
    }
    void send_request(int a, int b) {
        auto request = std::make_shared<service_example::srv::AddTwoInts::Request>();

        request->a = a;
        request->b = b;

        RCLCPP_INFO(this->get_logger(), "send request: %d + %d", a, b);

        auto future = client_->async_send_request(request);

        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(),
            future) == rclcpp::FutureReturnCode::SUCCESS) {
            auto response = future.get();

            RCLCPP_INFO(this->get_logger(), "result: %ld, message: %s", response->sum, response->message.c_str());
        }
        else {
            RCLCPP_ERROR(this->get_logger(), "service error");
        }
    }

private:

    rclcpp::Client<service_example::srv::AddTwoInts>::SharedPtr client_;
};


int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    auto node = std::make_shared<AddServiceClient>();

    node->send_request(2, 3);
    node->send_request(5, -5);
    node->send_request(0, 10);

    rclcpp::shutdown();


    return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(service_example)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_srvs REQUIRED)
# 自定义接口依赖包
find_package(rosidl_default_generators REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# 生成自定义服务接口
rosidl_generate_interfaces(
  ${PROJECT_NAME}
  "srv/AddTwoInts.srv"
)

add_executable(
  simple_service_server
  src/simple_service_server.cpp
)
add_executable(
  simple_service_client
  src/simple_service_client.cpp
)
add_executable(
  add_service_client
  src/add_service_client.cpp
)
add_executable(
  add_service_server
  src/add_service_server.cpp
)

ament_target_dependencies(simple_service_server rclcpp std_srvs)
ament_target_dependencies(simple_service_client rclcpp std_srvs)
ament_target_dependencies(add_service_server rclcpp)
ament_target_dependencies(add_service_client rclcpp)

# 链接自定义接口
rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(add_service_server "${cpp_typesupport_target}")
target_link_libraries(add_service_client "${cpp_typesupport_target}")
# rosidl_target_interfaces(add_service_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
# rosidl_target_interfaces(add_service_client ${PROJECT_NAME} "rosidl_typesupport_cpp")

install(
  TARGETS
  simple_service_server
  simple_service_client
  add_service_server
  add_service_client
  DESTINATION lib/${PROJECT_NAME}
)

# 导出依赖
ament_export_dependencies(rosidl_default_runtime)

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>service_example</name>

  <version>0.0.0</version>

  <description>TODO: Package description</description>

  <maintainer email="lenn@todo.todo">lenn</maintainer>

  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>

  <depend>std_srvs</depend>

  <!-- 自定义接口依赖 -->
  <build_depend>rosidl_default_generators</build_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <test_depend>ament_lint_auto</test_depend>

  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>

  </export>

</package>

编译运行

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

source install/setup.bash

ros2 run service_example simple_servie_server

ros2 run service_example simple_service_client

ros2 run service_example add_service_server

ros2 run service_example add_service_client

运行结果

客户端

服务端

服务命令行操作

查看服务信息

# 查看所有可用服务
ros2 service list

# 查看服务类型
ros2 service type /simple_service_server

# 查看服务接口定义
ros2 interface show std_srvs/srv/SetBool

# 查看自定义服务接口
ros2 interface show service_example/srv/AddTwoInts

调用服务

# 调用简单服务 启动机器人
ros2 service call /robot_control std_srvs/srv/SetBool '{data:true}'

# 调用简单服务 停止机器人
ros2 service call /robot_control std_srvs/srv/SetBool '{data:false}'

# 调用加法服务
ros2 service call /add_two_ints service_example/srv/AddTwoInts '{a: 5, b: 3}'

服务调试和监控

# 查找提供特定服务的节点
ros2 service find std_srvs/srv/SetBool

# 实时监控服务调用
ros2 service echo /simple_service_server

# 查看服务统计信息
ros2 service info /simple_service_server

话题和服务对比

发布-订阅（Topic）

• 通信模式： 发布者-订阅者模式
• 数据流向： 单向数据流
• 通信方式： 异步
• 连接关系： 多对多

服务（Service）

• 通信模式： 请求-响应模式
• 数据流向： 双向数据交换
• 通信方式： 同步通信
• 连接关系： 多对一（多个客户端，一个服务端）

特别注意：这里的异步是：发送端只管发送数据；这里讲的同步是：发送请求后需要处理响应（这个响应可以是同步也可以是异步）

通信模式图示对比

话题 Topic 通信模式

Topic 通信核心特征：

• 一对多：一个发布者，多个订阅者
• 异步：发布者不等订阅者处理
• 解耦：发布者不需要知道订阅者的存在
• 广播：所有订阅者都能接收到相同的数据

服务 Service 通信模式

Service 通信的核心特征：

• 客户端发送请求后等待响应
• 服务端处理完成返回结果
• 一对一的请求-响应关系
• 同步处理，意思是需要返回结果

选择适合的通信方式是 ROS2 系统设计的关键，应该根据数据特性、频率要求和应用场景来决定。

通信机制-动作

定义和概念

• 动作： ROS2中用于管理长时间运行任务的通信机制
• 特点： 支持目标设置、实时反馈、结果返回和任务取消
• 应用场景： 导航、抓取、充电等需要长时间完成的任务

ROS2动作概念图

动作vs其他通信方式对比

动作通信时序图

C++动作通信接口示例

多动作通信机制数据交互示意图

flowchart LR
subgraph 数据流示意图
A[目标：厨房位置<br/>x=5.0,y=3.0]
B[反馈：距离剩余2.1米<br/>预计时间30s]
C[结果：导航成功<br/>最终误差0.05m]
end

flowchart LR
subgraph 多动作通信系统架构
A[导航服务<br/>Action Server] --> |反馈：当前位置|B[导航控制器<br/>Action Client]
B --> |NavigateToGoal|A

C[机械臂控制器<br/>Action Client] --> |PickAndPlace|D[抓取服务<br/>Action Server]
D --> |反馈：抓取进度|C

E[任务调度器<br/>Action Client] --> |GoToCharging|F[充电服务<br/>Action Server]
F --> |反馈：充电状态|E
end

动作接口定义的方法

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake action_interfaces \
    --dependencies rosidl_default_generators rosidl_default_runtime \
     geometry_msgs builtin_interfaces

参数说明：

• 包名：action_interfaces
  • 约定命名：以_interfaces 结尾，表明这是接口定义包
  • 用途：专门用于定义动作接口
  • 动作：ROS2中用于长时间运行任务的通信模式
• 核心依赖：接口生成工具
  • rosidl_default_generators
• 作用：编译时代码生成器
• 功能：将. action 文件转化为 C++/Python 代码
• 类型：build_depend（构建依赖）
• rosidl_default_runtime
• 作用：运行时支持库
• 功能：提供生成接口的运行时基础设施
• 类型：exec_depend（执行依赖）
• 消息类型依赖：
• geometry_msgs
  • 作用：集合相关的标准消息类型包含：
• Point ,Pose, Twist（位置、姿态、速度）
• Vector3, Quaternion（向量、四元数）
• 用途：机器人导航、定位、运动控制
• builtin_interfaces
• 作用：ROS2内置的基础接口类型，包含：
• Time, Duration （时间相关）
• 基础数据类型支持
• 用途：时间戳、持续时间等

基础动作接口定义

使用 — 区分 目标定义 和 结果定义

action/NavigateToGoal.action

# ============================================================================
# 目标定义 (Goal) - 客户端发送给服务器的任务参数
# ============================================================================

# 目标位置和姿态
# 包含坐标系信息、3D位置坐标和四元数姿态
geometry_msgs/PoseStamped target_pose

# 最大移动速度限制（单位：米/秒）
# 用于控制机器人的移动速度，确保安全性
float64 max_speed

# 是否使用路径规划算法
# true：使用高级路径规划避障算法
# false：使用简单的直线导航
bool use_path_planning

# 路径规划算法名称
# 可选值：A_star RRT Dijkstra direct
# 当use_path_planning为true时生效
string planning_algorithm

---

# ============================================================================
# 结果定义 (Result) - 服务器返回给客户端的最终结果
# ============================================================================

# 导航任务是否成功完成
# true：成功到达目标位置
# false：任务失败或被取消
bool success

# 详细的结果描述消息
# 例如：导航成功完成 目标不可达 导航被用户取消
string result_message

# 最终到目标位置的距离误差（单位：米）
# 用于评估导航精度，越小越精确
builtin_interfaces/Duration total_time

# 机器人的最终位置和姿态
# 任务结束时机器人的实际位置
geometry_msgs/PoseStamped final_pose

---

# ============================================================================
# 反馈定义 (Feedback) - 服务器定期发送给客户端的进度信息
# ============================================================================

# 机器人当前位置和姿态
# 实时更新的机器人位置信息
geometry_msgs/PoseStamped current_pose

# 到目标位置的剩余距离（单位：米）
# 动态计算的直线距离，用于显示进度
float64 distance_remaining

# 机器人当前移动速度（单位：米/秒）
# 实时监测的实际移动速度
float64 current_speed

# 任务完成百分比（0-100）
# 基于已完成距离与总距离的比值计算
float64 completion_percentage

# 当前状态描述消息
# 例如： 导航中... 避障中... 重新规划路径...
string status_message

action/ChargeBattery.action

# ============================================================================
# 目标定义 (Goal) - 客户端发送给服务器的任务参数
# ============================================================================

# 目标位置和姿态
# 包含坐标系信息、3D位置坐标和四元数姿态
geometry_msgs/PoseStamped target_pose

# 最大移动速度限制（单位：米/秒）
# 用于控制机器人的移动速度，确保安全性
float64 max_speed

# 是否使用路径规划算法
# true：使用高级路径规划避障算法
# false：使用简单的直线导航
bool use_path_planning

# 路径规划算法名称
# 可选值：A_star RRT Dijkstra direct
# 当use_path_planning为true时生效
string planning_algorithm

---

# ============================================================================
# 结果定义 (Result) - 服务器返回给客户端的最终结果
# ============================================================================

# 导航任务是否成功完成
# true：成功到达目标位置
# false：任务失败或被取消
bool success

# 详细的结果描述消息
# 例如：导航成功完成 目标不可达 导航被用户取消
string result_message

# 最终到目标位置的距离误差（单位：米）
# 用于评估导航精度，越小越精确
builtin_interfaces/Duration total_time

# 机器人的最终位置和姿态
# 任务结束时机器人的实际位置
geometry_msgs/PoseStamped final_pose

---

# ============================================================================
# 反馈定义 (Feedback) - 服务器定期发送给客户端的进度信息
# ============================================================================

# 机器人当前位置和姿态
# 实时更新的机器人位置信息
geometry_msgs/PoseStamped current_pose

# 到目标位置的剩余距离（单位：米）
# 动态计算的直线距离，用于显示进度
float64 distance_remaining

# 机器人当前移动速度（单位：米/秒）
# 实时监测的实际移动速度
float64 current_speed

# 任务完成百分比（0-100）
# 基于已完成距离与总距离的比值计算
float64 completion_percentage

# 当前状态描述消息
# 例如： 导航中... 避障中... 重新规划路径...
string status_message

action/PickAndPlace.action

# @file action_interfaces/action/PickAndPlace.action
# @brief ROS2机械臂抓取放置动作接口定义
# @description 定义了机械臂执行抓取和放置操作的完整动作接口，包括目标参数、执行结果和实时反馈

# ============================================================================
# 目标定义 (Goal) - 客户端发送给服务器的抓取任务参数
# ============================================================================

# 抓取位置和姿态
# 包含目标物体的空间位置坐标和抓取姿态（四元数表示）
geometry_msgs/PoseStamped pick_pose

# 放置位置和姿态
# 机械臂完成抓取后将物体放置到的目标位置和姿态
geometry_msgs/PoseStamped place_pose

# 目标物体标识符
# 用于识别和跟踪特定物体的唯一ID字符串
string object_id

# 夹爪抓取力度 (单位: 牛顿)
# 控制夹爪的抓取力度，避免损坏脆弱物体或抓取力不足
float64 gripper_force

# 接近距离 (单位: 米)
# 机械臂在实际抓取前的预接近距离，用于精确定位
float64 approach_distance

# 是否使用视觉引导
# true: 启用视觉系统进行实时位置修正
# false: 仅使用预设的位置坐标
bool use_vision_guidance

---

# ============================================================================
# 结果定义 (Result) - 服务器返回给客户端的最终执行结果
# ============================================================================

# 抓取放置任务是否成功完成
# true: 完整完成抓取和放置操作
# false: 任务失败或被中断
bool success

# 详细的结果描述消息
# 例如: "抓取放置成功完成", "物体检测失败", "抓取力度不足"
string result_message

# 失败原因详细说明
# 当success为false时，提供具体的失败原因分析
string failure_reason

# 实际使用的夹爪力度 (单位: 牛顿)
# 机械臂实际执行时的夹爪力度反馈值
float64 actual_gripper_force

# 实际抓取位置和姿态
# 机械臂实际执行抓取时的末端执行器位置（可能与目标位置有偏差）
geometry_msgs/PoseStamped actual_pick_pose

# 实际放置位置和姿态
# 机械臂实际放置物体时的末端执行器位置
geometry_msgs/PoseStamped actual_place_pose

# 整个任务的执行时间
# 从开始接近物体到完成放置的总耗时
builtin_interfaces/Duration execution_time

---

# ============================================================================
# 反馈定义 (Feedback) - 服务器定期发送给客户端的执行进度信息
# ============================================================================

# 当前执行阶段标识符
# 可能的值: "approaching"(接近中), "grasping"(抓取中), "lifting"(提升中), 
#          "moving"(移动中), "placing"(放置中)
string current_phase

# 当前阶段完成百分比 (0-100)
# 表示当前执行阶段的完成程度
float64 phase_completion

# 机械臂末端执行器当前位置和姿态
# 实时更新的机械臂末端坐标和姿态信息
geometry_msgs/PoseStamped current_end_effector_pose

# 夹爪当前开合位置 (单位: 米或度)
# 夹爪的实时开合状态，用于监控抓取过程
float64 current_gripper_position

# 是否检测到目标物体
# true: 传感器成功检测到目标物体
# false: 未检测到物体或物体丢失
bool object_detected

# 夹爪力度反馈 (单位: 牛顿)
# 夹爪传感器实时反馈的抓取力度值
float64 grip_force_feedback

# 当前状态描述消息
# 例如: "正在接近物体...", "调整抓取姿态...", "物体抓取成功..."
string status_message

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(action_interfaces)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rosidl_default_runtime REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(std_msgs REQUIRED)

# if(BUILD_TESTING)
#   find_package(ament_lint_auto REQUIRED)
#   # the following line skips the linter which checks for copyrights
#   # comment the line when a copyright and license is added to all source files
#   set(ament_cmake_copyright_FOUND TRUE)
#   # the following line skips cpplint (only works in a git repo)
#   # comment the line when this package is in a git repo and when
#   # a copyright and license is added to all source files
#   set(ament_cmake_cpplint_FOUND TRUE)
#   ament_lint_auto_find_test_dependencies()
# endif()

# 生成动作接口
rosidl_generate_interfaces(
  ${PROJECT_NAME}
  "action/NavigateToGoal.action"
  "action/PickAndPlace.action"
  "action/ChargeBattery.action"
  DEPENDENCIES
  geometry_msgs
  builtin_interfaces
  std_msgs
)

ament_export_dependencies(rosidl_default_runtime)
ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>action_interfaces</name>

  <version>0.0.0</version>

  <description>TODO: Package description</description>

  <maintainer email="lennlouis@163.com">lenn</maintainer>

  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <!-- 接口生成依赖 -->
  <build_depend>rosidl_default_generators</build_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>

  <!-- 接口依赖 不能重复添加-->
  <!-- <depend>rosidl_default_generators</depend>

  <depend>rosidl_default_runtime</depend> -->
  <depend>geometry_msgs</depend>

  <depend>builtin_interfaces</depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <test_depend>ament_lint_auto</test_depend>

  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>

  </export>

</package>

编译和验证接口

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

# 设置环境
source install/setup.bash

# 验证接口生成
ros2 interface list | grep action_interfaces

执行后显示：

后续需要使用：action_interfaces/action/NavigateToGoal

C++编程方法（使用 NavigateToGoal）

交互逻辑

动作生命周期

创建动作演示功能包

cd ~/ros2_ws/src

# 创建action_demo功能包
ros2 pkg create --build-type ament_cmake action_demo \
  --dependencies rclcpp rclcpp_action action_interfaces geometry_msgs tf2 tf2_geometry_msgs

命令解析：

• action_demo
  • 作用：新创建的 ROS2 包名称
  • 生成：会创建名为 action_demo 的目录
• –dependencies 依赖：
  • rclcpp_action
• 用途：ROS2 C++动作系统库
• 提供：动作客户端和服务器的实现
• 功能：处理长时间运行的任务，支持取消和进度反馈
• action_interfaces （即是我们自己创建的动作接口）
• 用途：自定义动作接口包
• 包含：项目特定的.action 文件定义
• 动作：提供 NavigateToGoal、PickAndPlace 等动作接口
• geometry_msgs
• 用途：几何消息类型标准库
• 提供：PoseStamped、Twist、Point、Quaternion 等
• 用途：处理位置、姿态、速度等几何数据
• tf2
• 用途：坐标变换库核心
• 功能：坐标系变换计算和管理
• 作用：处理机器人各部件之间的空间关系
• tf2_geometry_msgs
• 用途：TF2 与 geometry_msgs 的集成
• 功能：为 geometry_msgs 提供坐标变换功能
• 例如：将 PoseStamped 在不同坐标系间转换

动作服务端编程方法

action_demo/src/navigation_action_server.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
// 包含ROS2动作系统的头文件
#include <rclcpp_action/rclcpp_action.hpp>
// 包含自定义导航动作接口的头文件
#include <action_interfaces/action/navigate_to_goal.hpp>
// 包含集合消息类型，用于位置和姿态的表示
#include <geometry_msgs/msg/pose_stamped.hpp>
// 包含TF2四元数类，用于姿态计算
#include <tf2/LinearMath/Quaternion.hpp>
// 包含TF2集合消息转换工具
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// 包含数学计算函数
#include <cmath>
// 多线程支持
#include <thread>

using NavigateToGoal = action_interfaces::action::NavigateToGoal;
using GoalHandleNavigate = rclcpp_action::ServerGoalHandle<NavigateToGoal>;

class NavigationActionServer : public rclcpp::Node {
public:
    NavigationActionServer() : Node("navigation_action_server") {
        // 创建动作服务器对象
        // 模版参数指定动作类型NavigateToGoal
        // navigate_to_goal是动作名称，客户端通过此名称调用动作
        // 绑定三个回调函数：目标处理、取消处理、接受处理
        action_server_ = rclcpp_action::create_server<NavigateToGoal>(
            this,
            "navigate_to_goal",
            std::bind(&NavigationActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&NavigationActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&NavigationActionServer::handle_accept, this, std::placeholders::_1)
        );

        // 设置坐标系为map，这是ROS导航中的标准全局坐标系
        current_pose_.header.frame_id = "map";
        current_pose_.pose.position.x = 0.0;
        current_pose_.pose.position.y = 0.0;
        current_pose_.pose.position.z = 0.0;
        current_pose_.pose.orientation.w = 1.0; // 四元数w分量，表示无旋转

        RCLCPP_INFO(get_logger(), "navigation service setup");
    }

private:
    // 动作服务器对象的智能指针，管理导航动作的生命周期
    rclcpp_action::Server<NavigateToGoal>::SharedPtr action_server_;

    // 当前机器人位置
    geometry_msgs::msg::PoseStamped current_pose_;

    // 是否接收或拒绝该目标
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID& uuid, // 目标唯一标识符
        std::shared_ptr<const NavigateToGoal::Goal> goal // 客户端发送的导航目标 包含目标位置和最大速度
    ) {
        // 避免未使用变量警告
        (void)uuid;

        RCLCPP_INFO(get_logger(),
            "get navigation target:(%.2f, %.2f), max_speed: %.2f m/s",
            goal->target_pose.pose.position.x,
            goal->target_pose.pose.position.y,
            goal->max_speed);

        if (goal->max_speed <= 0 || goal->max_speed > 5.0) {
            return rclcpp_action::GoalResponse::REJECT;
        }

        double distance = calculate_distance(current_pose_, goal->target_pose);
        if (distance > 100.0) { // 距离太远
            return rclcpp_action::GoalResponse::REJECT;
        }

        RCLCPP_INFO(get_logger(), "target accepted, begin navigation");

        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;  // 触发handle_accepted的调用
    }

    // 客户端请求取消正在进行的导航任务时，此函数被调用
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleNavigate> goal_handle // 要取消的目标句柄
    ) {
        (void)goal_handle;

        RCLCPP_INFO(get_logger(), "get cancel request");

        // 接受取消请求（在实际中需要更复杂的逻辑）
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accept(
        const std::shared_ptr<GoalHandleNavigate> goal_handle // 目标句柄，用于发布反馈和设置结果
    ) {
        RCLCPP_INFO(get_logger(), "get accepted request");

        // 新线程中执行导航任务，避免阻塞主线程
        // 使用detach()让线程独立运行
        std::thread(std::bind(&NavigationActionServer::execute_navigation, this, goal_handle)).detach();
    }

    void execute_navigation(const std::shared_ptr<GoalHandleNavigate> goal_handle) {
        RCLCPP_INFO(get_logger(), "navigation begin");

        const auto goal = goal_handle->get_goal();

        auto feedback = std::make_shared<NavigateToGoal::Feedback>();
        auto result = std::make_shared<NavigateToGoal::Result>();

        // 记录任务开始时间
        auto start_time = this->now();

        // 计算到目标的总距离
        double total_distance = calculate_distance(current_pose_, goal->target_pose);

        const double update_rate = 10.0;
        const double time_step = 1.0 / update_rate; // 时间步长
        rclcpp::Rate rate(update_rate);

        // 主要导航执行循环
        while (rclcpp::ok()) {
            // 是否收到取消请求
            if (goal_handle->is_canceling()) {
                result->success = false;
                result->result_message = "navigation canceled";
                result->final_distance_to_goal = calculate_distance(current_pose_, goal->target_pose);
                result->total_time = this->now() - start_time;
                result->final_pose = current_pose_;

                // 标记目标取消
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "navigation canceled");
                return;
            }

            // 模拟机器人向目标移动
            move_towards_goal(goal->target_pose, goal->max_speed, time_step);

            double remaining_distance = calculate_distance(current_pose_, goal->target_pose);

            feedback->current_pose = current_pose_;
            feedback->distance_remaining = remaining_distance;
            feedback->completion_percentage = ((total_distance - remaining_distance) / total_distance) * 100.0;
            feedback->current_speed = goal->max_speed;
            feedback->estimated_time_remaining = remaining_distance / goal->max_speed;
            feedback->status_message = "navigating...";

            goal_handle->publish_feedback(feedback);

            RCLCPP_DEBUG(
                this->get_logger(),
                "current position:(%.2f, %.2f), ramain:%.2f, finish:%.1f%%",
                current_pose_.pose.position.x,
                current_pose_.pose.position.y,
                remaining_distance,
                feedback->completion_percentage
            );

            if (remaining_distance < 0.1) {
                result->success = true;
                result->result_message = "navigation success";
                result->final_distance_to_goal = remaining_distance;
                result->total_time = this->now() - start_time;
                result->final_pose = current_pose_;

                goal_handle->succeed(result);
                RCLCPP_INFO(get_logger(), "navigation success, 误差:%.3f, total time:%.2f", remaining_distance,
                    result->total_time.sec + result->total_time.nanosec * 1e-9);

                return;
            }

            // 等待下一个控制周期
            rate.sleep();
        }
    }

    double calculate_distance(
        const geometry_msgs::msg::PoseStamped& pose1,
        const geometry_msgs::msg::PoseStamped& pose2
    ) {
        double dx = pose2.pose.position.x - pose1.pose.position.x;
        double dy = pose2.pose.position.y - pose1.pose.position.y;

        return std::sqrt(dx * dx + dy * dy);
    }

    void move_towards_goal(
        const geometry_msgs::msg::PoseStamped& target,
        double max_speed,
        double time_step
    ) {
        double dx = target.pose.position.x - current_pose_.pose.position.x;
        double dy = target.pose.position.y - current_pose_.pose.position.y;
        double distance = std::sqrt(dx * dx * dy * dy);

        if (distance > 0) {
            double move_distance = std::min(max_speed * time_step, distance);
            double ratio = move_distance / distance;

            // 更新位置
            current_pose_.pose.position.x += dx * ratio;
            current_pose_.pose.position.y += dy * ratio;
            current_pose_.header.stamp = this->now();

            // 更新朝向
            double yaw = std::atan2(dy, dx); // 计算偏航角
            tf2::Quaternion q;
            q.setRPY(0, 0, yaw); // 设置Roll-Pitch-Yaw角度
            current_pose_.pose.orientation = tf2::toMsg(q); // 转化为消息格式
        }
    }
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    auto  node = std::make_shared<NavigationActionServer>();

    rclcpp::spin(std::make_shared<NavigationActionServer>());

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

动作客户端编程方法

action_demo/src/navigation_action_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <action_interfaces/action/navigate_to_goal.hpp>
#include <geometry_msgs/msg/pose_stamped.h>
#include <chrono>

using namespace std::chrono_literals;
using NavigateToGoal = action_interfaces::action::NavigateToGoal;
using GoalHandleNavigate = rclcpp_action::ClientGoalHandle<NavigateToGoal>;

class NavigationActionClient : public rclcpp::Node {

public:
    NavigationActionClient() : Node("navigation_action_client") {
        action_client_ = rclcpp_action::create_client<NavigateToGoal>(this, "navigate_to_goal");

        // 等待动作服务可用，超时时间10s
        if  (!action_client_->wait_for_action_server(10s)) {
            RCLCPP_ERROR(get_logger(), "action serviec unline");
            return;
        }

        RCLCPP_INFO(get_logger(), "navigation client setup");

        // 开始测试导航功能
        send_navigation_goals();
    }

private:
    rclcpp_action::Client<NavigateToGoal>::SharedPtr action_client_;

    // 发送多个导航目标
    // 定义一系列导航点，按顺序发给导航服务器，m模拟机器人按预定路径移动
    void send_navigation_goals() {
        std::vector<std::pair<double, double>> waypoints = {
            {3.0, 2.0},
            {5.0, 5.0},
            {1.0, 4.0},
            {0.0, 0.0}
        };

        for (const auto& waypoint : waypoints) {
            send_goal(waypoint.first, waypoint.second, 1.5);

            std::this_thread::sleep_for(2s);
        }
    }

    void send_goal(double x, double y, double max_speed) {
        auto goal_msg = NavigateToGoal::Goal();

        goal_msg.target_pose.header.stamp = this->now(); // 时间戳设置为当前时间

        goal_msg.target_pose.header.frame_id = "map"; // 设置坐标系为地图坐标系

        goal_msg.target_pose.pose.position.x = x;
        goal_msg.target_pose.pose.position.y = y;
        goal_msg.target_pose.pose.position.z = 0.0;

        goal_msg.target_pose.pose.orientation.w = 1.0;

        goal_msg.max_speed = max_speed;
        goal_msg.use_path_planning = true;
        goal_msg.planning_algorithm = "A_star"; // A*算法

        RCLCPP_INFO(get_logger(), "send navigation target:(%.2f, %.2f), max_speed:%.2f m/s", x, y, max_speed);

        auto send_goal_options = rclcpp_action::Client<NavigateToGoal>::SendGoalOptions();

        send_goal_options.goal_response_callback = std::bind(&NavigationActionClient::goal_response_callback, this, 
            std::placeholders::_1);

        send_goal_options.feedback_callback = std::bind(&NavigationActionClient::feedback_callback, this,
            std::placeholders::_1, std::placeholders::_2);
        
        send_goal_options.result_callback = std::bind(&NavigationActionClient::result_callback, this,
            std::placeholders::_1);

        auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options);
    }

    // 当服务器响应目标请求时调用，判断目标是否接受
    void goal_response_callback(const GoalHandleNavigate::SharedPtr& goal_handle) {
        if (!goal_handle) {
            RCLCPP_ERROR(get_logger(), "target refused");
        }
        else {
            RCLCPP_INFO(get_logger(), "target accepted, begin...");
        }
    }

    void feedback_callback(
        GoalHandleNavigate::SharedPtr,
        const std::shared_ptr<const NavigateToGoal::Feedback> feedback
    ) {
        RCLCPP_INFO(get_logger(), "navigation resp: position(%.2f, %.2f), remaining:%.2f,finished:%.2f, state:%s",
            feedback->current_pose.pose.position.x,
            feedback->current_pose.pose.position.y,
            feedback->distance_remaining,
            feedback->completion_percentage,
            feedback->status_message.c_str());
    }

    void result_callback(const GoalHandleNavigate::WrappedResult& result) {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(get_logger(), "navigation success:%s, distance:%.3f, total time:%.2f",
                result.result->result_message.c_str(),
                result.result->final_distance_to_goal,
                result.result->total_time.sec + result.result->total_time.nanosec * 1e-9);
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(get_logger(), "navigation stoped:%s", result.result->result_message.c_str());
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_WARN(get_logger(), "navigation cancel:%s", result.result->result_message.c_str());
            break;
        default:
            RCLCPP_ERROR(get_logger(), "unexcept code");
            break;
        }
    }

public:
    // 取消当前目标的公共方法
    // 提供给外部调用的接口，用于取消当前正在执行的目标，这在紧急情况或用户干预时非常有用
    void cancel_current_goal() {
        RCLCPP_INFO(get_logger(), "cancel current navigation target");
        // 异步取消所有正在执行的目标
        auto cancel_future = action_client_->async_cancel_all_goals();
    }
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    auto node = std::make_shared<NavigationActionClient>();

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(action_demo)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(action_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(navigation_action_server src/navigation_action_server.cpp)
add_executable(navigation_action_client src/navigation_action_client.cpp)

ament_target_dependencies(
  navigation_action_server
  rclcpp
  rclcpp_action
  action_interfaces
  geometry_msgs
  tf2
  tf2_geometry_msgs
)

ament_target_dependencies(
  navigation_action_client
  rclcpp
  rclcpp_action
  action_interfaces
  geometry_msgs
  tf2
  tf2_geometry_msgs
)

install(
  TARGETS
  navigation_action_client
  navigation_action_server
  DESTINATION
  lib/${PROJECT_NAME}
)

ament_package()

编译和运行

cd ~/ros2_ws
colcon build --packages-select action_interfaces action_demo

source install/setup.bash

ros2 run action_demo navigation_action_server

ros2 run action_demo navigation_action_client

动作的命令行操作

动作命令行操作

# 查看可用动作
ros2 action list

# 查看动作接口定义
ros2 interface show acton_interfaces/action/NavigateToGoal

# 发送动作目标
ros2 action send_goal /navigate_to_goal action_interfaces/action/NavigateToGoal \
  '{target_pose:{header: {frame_id: "map"}, pose: {position: {x: 5.0, y: 3.0}}}, max_speed: 2.0}' \
  --feedback

# 查看动作信息
ros2 action info /navigate_to_goal

# 监听动作反馈
ros2 topic echo /navigate_to_goal/_action/feedback

# 监听动作状态
ros2 topic echo /navigate_to_goal/_action/status

# 使用rqt图形界面
rqt_action #动作监控器

动作调试和监控

# 查看动作服务端
ros2 node info /navigation_action_server

# 查看动作相关话题
ros2 topic list | grep navigate_to_goal

# 查看动作相关服务
ros2 service list | grep navigate_to_goal

# 实时监控动作执行
ros2 topic echo /navigate_to_goal/_action/feedback --once

通信接口

什么是通信接口

定义和概念

• 通信接口： 定义 ROS2 节点间数据传输格式的标准结构
• 作用： 确保不同节点间数据交换的一致性和兼容性
• 类型： 消息（.msg）、服务（.srv）、动作（.action）

ROS2 通信接口体系结构图

接口数据类型层次结构

通信接口的定义方法

创建接口功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake my_interfaces \
  --dependencies rosidl_default_generators rosidl_default_runtime

消息接口（.msg）定义

基础消息定义

msg/RobotStatus.msg：

# 机器人状态消息
# 基本数据类型
bool is_enabled
int32 battery_level
float64 speed
string current_mode

# 时间戳
builtin_interfaces/Time timestamp

# 数组类型
float64[] joint_positions
string[5] sensor_names

# 嵌套消息类型
geometry_msgs/Point position
geometry_msgs/Quaternion orientation

复杂消息定义

msg/SensorData.msg：

# 传感器数据集合
std_msgs/Header header

# 传感器信息
string sensor_id
string sensor_type
bool is_active

# 测量数据
float64 temperature
float64 humidity
float64 pressure

# 位置信息
float64[] raw_data
uint8[] status_flags

# 可选字段（使用常量）
uint8 STATUS_OK = 0
uint8 STATUS_WARNING = 1
uint8 STATUS_ERROR = 2
uint8 current_status

服务接口（.srv）定义

基础服务定义

srv/SetRobotMode.srv：

# 请求部分
string mode_name
bool enable_safety
float64 max_speed
---
# 响应部分
bool success
string message
string previous_mode
float64 actual_speed

复杂服务定义

srv/NavigationRequest.srv：

# 导航请求服务
# 请求部分
geometry_msgs/PoseStamped start_pose
geometry_msgs/PoseStamped goal_pose
string planning_algorithm
float64 max_planning_time
bool use_obstacles
string[] obstacle_types
---
# 响应部分
bool success
string result_message
nav_msgs/Path planned_path
float64 path_length
float64 estimated_time
geometry_msgs/PoseStamped[] waypoints

文件操作服务

srv/FileOperation.srv：

# 文件操作服务
# 请求服务
string operation_type # read write delete
string file_path
string file_content # 写操作
---
# 响应部分
bool success
string message
string file_content # 读操作
int64 file_size
builtin_interfaces/Time last_modified

动作接口（.action）定义

基础动作定义

action/NavigateToGoal.action

# 目标定义
geometry_msgs/PoseStamped target_pose
float64 max_speed
bool use_path_planning
---
# 结果定义
bool success
string result_message
float64 final_distance_to_goal
builtin_interfaces/Duration total_time
---
# 反馈定义
geometry_msgs/PoseStamped current_pose
float64 distance_remaining
float64 estimated_time_remaining
float64 current_speed

复杂动作定义

action/ProcessData.action

# 数据处理动作
# 目标定义
string[] input_files
string processing_type
int32 batch_size
bool save_intermediate_results
---
# 结果定义
bool success
string result_message
string[] output_files
int32 processed_items
float64 processing_time
string[] error_logs
---
# 反馈定义
int32 current_item
int32 total_items
float64 completion_percentage
string current_file
builtin_interfaces/Duration elapsed_time
string status_message

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(my_interfaces)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rosidl_default_runtime REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

rosidl_generate_interfaces(
  ${PROJECT_NAME}
  "msg/RobotStatus.msg"
  "msg/SensorData.msg"

  "srv/FileOperation.srv"
  "srv/NavigationRequest.srv"
  "srv/SetRobotMode.srv"

  "action/NavigateToGoal.action"
  "action/ProcessData.action"

  DEPENDENCIES
  std_msgs
  geometry_msgs
  sensor_msgs
  nav_msgs
  builtin_interfaces
)

ament_export_dependencies(rosidl_default_runtime)

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_interfaces</name>

  <version>0.0.0</version>

  <description>TODO: Package description</description>

  <maintainer email="lennlouis@163.com">lenn</maintainer>

  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rosidl_default_generators</depend>

  <depend>rosidl_default_runtime</depend>

  <depend>std_msgs</depend>

  <depend>geometry_msgs</depend>

  <depend>sensor_msgs</depend>

  <depend>nav_msgs</depend>

  <depend>builtin_interfaces</depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <test_depend>ament_lint_auto</test_depend>

  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>

  </export>

</package>

通信接口命令行操作

编译和验证接口

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

source install/setup.bash

ros2 interface list | grep my_interfaces

查看接口信息

# 查看所有接口
ros2 interface list
# 查看特定包的接口
ros2 interface package my_interfaces
# 查看消息接口详情
ros2 interface show my_interfaces/msg/RobotStatus
ros2 interface show my_interfaces/msg/SensorData
# 查看服务接口详情
ros2 interface show my_interfaces/srv/SetRobotMode
ros2 interface show my_interfaces/srv/NavigationRequest
# 查看动作接口详情
ros2 interface show my_interfaces/action/NavigateToGoal
ros2 interface show my_interfaces/action/ProcessData

接口类型查询

# 查看标准消息类型
ros2 interface show std_msgs/msg/String
ros2 interface show geometry_msgs/msg/Twist
ros2 interface show sensor_msgs/msg/LaserScan
# 查看标准服务类型
ros2 interface show std_srvs/srv/SetBool
ros2 interface show std_srvs/srv/Trigger
# 搜索包含特定关键字的接口
ros2 interface list | grep -i navigation
ros2 interface list | grep -i sensor

使用接口进行通信测试

source install/setup.bash

# 使用自定义消息发布话题
ros2 topic pub /robot_status my_interfaces/msg/RobotStatus \
  '{is_enabled: true, battery_level: 85, speed: 1.5, current_mode: "navigation"}'

# 调用自定义服务
ros2 service call /set_robot_mode my_interfaces/srv/SetRobotMode \
  '{mode_name: "autonomous", enable_safety: true, max_speed: 2.0}'

# 使用rqt图形界面
rqt_msg
rqt_srv

C++服务接口应用示例

创建使用自定义接口的功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake interface_demo \
  --dependencies rclcpp my_interfaces

服务端实现实例

src/robot_mode_server.cpp

// #include <rclcpp/rclcpp.hpp>
// #include <my_interfaces/srv/set_robot_mode.hpp>
// #include <memory>
// #include <string>
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <my_interfaces/srv/set_robot_mode.hpp>

class RobotModeServer : public rclcpp::Node {
public:
    RobotModeServer() : Node("robot_mode_server"), current_mode_("manual"), max_speed_(1.0){
        service_ = create_service<my_interfaces::srv::SetRobotMode>(
            "set_robot_mode",
            std::bind(&RobotModeServer::handle_set_mode, this, std::placeholders::_1, std::placeholders::_2));

        RCLCPP_INFO(get_logger(), "robot mode server start");
        RCLCPP_INFO(get_logger(), "mode: %s, max_speed: %.2f m/s", current_mode_.c_str(), max_speed_);
    }

private:
    void handle_set_mode(
        const std::shared_ptr<my_interfaces::srv::SetRobotMode::Request> request,
        std::shared_ptr<my_interfaces::srv::SetRobotMode::Response> response
    ) {
        RCLCPP_INFO(get_logger(), "get request: %s, enable_safety: %s, max_speed: %.2f",
            request->mode_name.c_str(),
            request->enable_safety ? "true" : "false",
            request->max_speed);
        
        response->previous_mode = current_mode_;

        // 验证请求参数
        if (request->mode_name.empty()) {
            response->success = false;
            response->message = "模式名称不能为空";
            response->actual_speed = max_speed_;
            return;
        }
        if (request->max_speed <= 0 || request->max_speed > 5.0) {
            response->success = false;
            response->message = "无效的最大速度，应在 0-5.0 m/s 范围内";
            response->actual_speed = max_speed_;
            return;
        }

        std::vector<std::string> supported_modes = {
            "manual", "autonomous", "navigation", "teleoperation"
        };

        if (std::find(supported_modes.begin(), supported_modes.end(), request->mode_name) == supported_modes.end()) {
            response->success = false;
            response->message = "unexcepted mode:" + request->mode_name;
            response->actual_speed = max_speed_;
            return;
        }

        current_mode_ = request->mode_name;

        if (request->enable_safety) {
            max_speed_ = std::min(request->max_speed, 2.0);
        }
        else {
            max_speed_ = request->max_speed;
        }

        response->success = true;
        response->message = "set mode success:" + current_mode_;
        response->actual_speed = max_speed_;

        RCLCPP_INFO(
            get_logger(),
            "mode changed: %s -> %s, max_speed: %.2f m/s",
            response->previous_mode.c_str(),
            current_mode_.c_str(),
            max_speed_
        );
    }
    rclcpp::Service<my_interfaces::srv::SetRobotMode>::SharedPtr service_;
    std::string current_mode_;
    double max_speed_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<RobotModeServer>());

    rclcpp::shutdown();
    return 0;
}

客户端实现实例

src/robot_mode_client.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <my_interfaces/srv/set_robot_mode.hpp>
#include <memory>
#include <chrono>

using namespace std::chrono_literals;

class RobotModeClient : public rclcpp::Node {

public:
    RobotModeClient() : Node("robot_mode_client") {
        client_ = create_client<my_interfaces::srv::SetRobotMode>("set_robot_mode");

        while(!client_->wait_for_service(1s)) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(get_logger(), "service break...");
                return;
            }

            RCLCPP_INFO(get_logger(), "wait for set_robot_mode server...");
        }

        RCLCPP_INFO(get_logger(), "robot_mode_client start");

        send_mode_settings();
    }

private:
    void send_mode_settings() {
        // std::vector<std::string> supported_modes = {
        //     "manual", "autonomous", "navigation", "teleoperation"
        // };

        set_robot_mode("autonomous", true, 1.5);
        std::this_thread::sleep_for(2s);

        set_robot_mode("manual", false, 3.0);
        std::this_thread::sleep_for(2s);

        set_robot_mode("navigation", true, 4.0);

        // 测试4: 无效模式测试
        set_robot_mode("invalid_mode", true, 2.0);
        std::this_thread::sleep_for(2s);
        // 测试5: 无效速度测试
        set_robot_mode("manual", true, 10.0);
    }

    void set_robot_mode(const std::string& mode_name, bool enable_safety, double max_speed) {
        auto request = std::make_shared<my_interfaces::srv::SetRobotMode::Request>();
        request->mode_name = mode_name;
        request->max_speed = max_speed;
        request->enable_safety = enable_safety;

        auto future = client_->async_send_request(request);

        if (rclcpp::spin_until_future_complete(get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) {
            auto response = future.get();

            if (response->success) {
                RCLCPP_INFO(get_logger(),
                    "set mode success: %s, previous: %s, speed: %.2f",
                    response->message.c_str(), response->previous_mode.c_str(), response->actual_speed);
            }
            else {
                RCLCPP_WARN(get_logger(),
                    "set mode failed: %s, speed: %.2f",
                    response->message.c_str(), response->actual_speed);
            }
        }
        else {
            RCLCPP_ERROR(get_logger(), "service failed");
        }
    }
    rclcpp::Client<my_interfaces::srv::SetRobotMode>::SharedPtr client_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    auto node = std::make_shared<RobotModeClient>();

    // rclcpp::spin(std::make_shared<RobotModeClient>());

    rclcpp::shutdown();
    return 0;
}

编译配置

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(my_interfaces)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rosidl_default_runtime REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

rosidl_generate_interfaces(
  ${PROJECT_NAME}
  "msg/RobotStatus.msg"
  "msg/SensorData.msg"

  "srv/FileOperation.srv"
  "srv/NavigationRequest.srv"
  "srv/SetRobotMode.srv"

  "action/NavigateToGoal.action"
  "action/ProcessData.action"

  DEPENDENCIES
  std_msgs
  geometry_msgs
  sensor_msgs
  nav_msgs
  builtin_interfaces
)

ament_export_dependencies(rosidl_default_runtime)

ament_package()

参数

参数是什么

定义和概念

• 参数： ROS2 中用于配置节点行为的键值对系统
• 特点： 动态可配置、类型安全、支持运行时修改
• 作用： 作为机器人系统的全局字典，存储配置信息

ROS2 参数系统概念图

参数在机器人系统中的作用

参数通信模式

参数服务架构图

参数操作时序图

参数生命周期图

参数 C++编程方法

创建参数演示功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake parameter_demo \
  --dependencies rclcpp rcl_interfaces # ROS2的核心接口包（包含参数相关接口）

基础参数使用示例

src/basic_parameter_node.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/parameter_descriptor.hpp> // 参数描述符类型
#include <rcl_interfaces/msg/parameter_event.hpp> // 参数事件类型
#include <string>
#include <memory>
#include <chrono>

using namespace std::chrono_literals;

class BasicParameterNode : public rclcpp::Node {

public:
    BasicParameterNode() : Node("basic_parameter_node") {
        // 所有要使用的参数必须先声明
        declare_basic_parameters();

        // 读取参数初始化成员变量
        get_parameters();

        setup_parameter_callback();

        timer_ = create_wall_timer(
            5s,
            std::bind(&BasicParameterNode::timer_callback, this)
        );
    }

private:

    void declare_basic_parameters() {
        // 布尔参数 - 控制是否开启调试模式
        auto bool_desc = rcl_interfaces::msg::ParameterDescriptor();
        bool_desc.description = "是否启用调试模式";
        this->declare_parameter("debug_mode", false, bool_desc);

        // 整数参数 - 设置循环频率，带有范围约束
        auto int_desc = rcl_interfaces::msg::ParameterDescriptor();
        int_desc.description = "循环频率(Hz)";
        int_desc.integer_range.resize(1);
        int_desc.integer_range[0].from_value = 1;
        int_desc.integer_range[0].to_value = 100;
        declare_parameter("loop_rate", 10, int_desc);

        // 浮点参数 - 设置最大速度，带有范围约束
        auto double_desc = rcl_interfaces::msg::ParameterDescriptor();
        double_desc.description = "最大速度(m/s)";
        double_desc.floating_point_range.resize(1);
        double_desc.floating_point_range[0].from_value = 0.0;
        double_desc.floating_point_range[0].to_value = 5.0;
        declare_parameter("max_speed", 1.0, double_desc);

        // 字符串参数 - 设置机器人名称
        auto string_desc = rcl_interfaces::msg::ParameterDescriptor();
        string_desc.description = "机器人名称";
        declare_parameter("robot_name", std::string("robot1"), string_desc);

        // 数组参数 - 设置关节位置数组（6自由度机械臂）
        auto array_desc = rcl_interfaces::msg::ParameterDescriptor();
        array_desc.description = "关节位置数组";
        std::vector<double> default_joints = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
        declare_parameter("joint_positions", default_joints, array_desc);

        // 字符串数组参数 - 设置传感器名称表
        auto string_array_desc = rcl_interfaces::msg::ParameterDescriptor();
        string_array_desc.description = "传感器名称表";
        // 默认传感器列表
        std::vector<std::string> default_sensors = {"camera", "lidar", "imu"};
        declare_parameter("sensor_names", default_sensors, string_array_desc);
    }

    void get_parameters() {
        debug_mode_ = get_parameter("debug_mode_").as_bool();
        loop_rate_ = get_parameter("loop_rate").as_int();
        max_speed_ = get_parameter("max_speed").as_double();
        robot_name_ = get_parameter("robot_name").as_string();
        joint_positions_ = get_parameter("joint_positions").as_double_array();
        sensor_names_ = get_parameter("sensor_names").as_string_array();

        display_parameters();
    }

    void setup_parameter_callback() {
        parameter_callback_handle_ = add_on_set_parameters_callback(
            std::bind(&BasicParameterNode::on_parameter_change, this, std::placeholders::_1)
        );
    }

    // 验证参数是否合法，更新成员变量，记录参数变更日志，返回操作结果
    rcl_interfaces::msg::SetParametersResult on_parameter_change(const std::vector<rclcpp::Parameter>& parameters) {
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true; // 默认设置成功

        for (auto& param : parameters) {
            if (param.get_name() == "debug_mode") {
                debug_mode_ = param.as_bool();
            }

            if (param.get_name() == "loop_rate") {
                int new_rate = param.as_int();
                if (new_rate >= 1 && new_rate <= 100) {
                    loop_rate_ = new_rate;
                }
                else {
                    result.successful = false;
                }
            }

            if (param.get_name() == "max_speed") {
                double new_speed = param.as_double();

                if (new_speed >=0.0 && new_speed <= 5.0) {
                    max_speed_ = new_speed;
                }
                else {
                    result.successful = false;
                }
            }

            if (param.get_name() == "robot_name") {
                robot_name_ = param.as_string();
            }

            if (param.get_name() == "joint_positions") {
                joint_positions_ = param.as_double_array();
            }

            if (param.get_name() == "sensor_names") {
                sensor_names_ = param.as_string_array();
            }
        }

        return result;
    }

    void timer_callback() {
        if (debug_mode_) {
            display_parameters();
        }
    }

    void display_parameters() {
        RCLCPP_INFO(get_logger(), "=====current parameters=====");
        RCLCPP_INFO(get_logger(), "debug_mode: %s", debug_mode_ ? "enable" : "disable");
        RCLCPP_INFO(get_logger(), "loop_rate: %d Hz", loop_rate_);
        RCLCPP_INFO(get_logger(), "max_speed: %.2f m/s", max_speed_);
        RCLCPP_INFO(get_logger(), "robot_name: %s", robot_name_.c_str());
        
        std::string joints_str = "joint:[";
        for (size_t i = 0; i < joint_positions_.size(); i++) {
            joints_str += std::to_string(joint_positions_[i]);
            if (i < joint_positions_.size() - 1) {
                joints_str += ",";
            }
        }
        joints_str += "]";
        RCLCPP_INFO(get_logger(), "%s", joints_str.c_str());

        std::string sensors_str = "sensor:[";
        for (size_t i = 0; i < sensor_names_.size(); i++) {
            sensors_str += sensor_names_[i];
            if (i < sensor_names_.size() - 1) {
                sensors_str += ",";
            }
        }
        sensors_str += "]";
        RCLCPP_INFO(get_logger(), "%s", sensors_str.c_str());
    }

    // ROS2 相关对象
    rclcpp::TimerBase::SharedPtr timer_;
    // 参数回调句柄
    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;

    bool debug_mode_;
    int loop_rate_;
    double max_speed_;
    std::string robot_name_;
    std::vector<double> joint_positions_;
    std::vector<std::string> sensor_names_;
};

int main(int argc, char* argv[]) {

    rclcpp::init(argc, argv);

    auto node = std::make_shared<BasicParameterNode>();

    rclcpp::spin(node);

    rclcpp::shutdown();
    
    return 0;
}

高级参数管理示例

本示例演示了ROS2中参数系统的高级使用方法，包括：

• 参数组的组织和管理
• 从YAML配置文件加载参数
• 参数验证和约束检查
• 参数事件监听和响应
• 参数的保存和导出功能
• 嵌套参数结构的处理
• 复杂参数系统的架构设计

src/advanced_parameter_node.cpp

数据分发服务DDS

DDS是什么

• DDS（Data Distribution Service）： 数据分发服务，是ROS2的底层通信中间件
• 作用：提供高性能、实时、可靠的分布式数据通信
• 标准： 基于OMG DDS规范，支持发布-订阅模式

ROS2系统架构分层图

DDS在机器人系统中的通信场景

不局限以上场景。

DDS通信模式 DDS Communication Models

点对点模式 Peer-to-Peer Model

Broker模式 Broker Model

广播模式 Broadcast Model

数据中心模式 Data-Centric Model

质量服务策略 Quality of Service-QoS

QoS策略概览图

QoS策略应用场景

命令行中配置DDS的QoS

创建QoS配置文件

config/qos_profiles.yaml

qos_profile_sensor_data:
    history: keep_last
    depth: 5
    reliability: best_effort
    durability: volatile
    deadline:
        sec: 0
    nsec: 100000000 # 100ms
  lifespan:
    sec: 1
    nsec: 0
  liveliness: automatic
  liveliness_lease_duration:
    sec: 3
    nsec: 0

qos_profile_control_commands:
  history: keep_last
  depth: 1
  reliability: reliable
  durability: volatile
  deadline:
    sec: 0
    nsec: 50000000
  lifespan:
    sec: 0
    nsec: 500000000
  liveliness: automatic
  liveliness_lease_duration:
    sec: 1
    nsec: 0
qos_profile_system_config:
  history: keep_last
  depth: 1
  reliability: reliable
  durability: transient_local
  deadline:
    sec: 2147483647
    nsec: 999999999
  lifespan:
    sec: 2147483647
    nsec: 999999999 # 无限制
  liveliness: automatic
  liveliness_lease_duration:
    sec: 10
    nsec: 0
qos_profile_diagnostics:
  history: keep_last
  depth: 50
  reliability: reliable
  durability: volatile
  deadline:
    sec: 5
    nsec: 0
  lifespan:
    sec: 30
    nsec: 0
  liveliness: manual_by_topic
  liveliness_lease_duration:
    sec: 10
    nsec: 0

命令行QoS操作

# 查看话题的QoS设置
ros2 topic info /scan --verbose
ros2 topic info /cmd_vel --verbose

# 使用特定QoS发布消息
ros2 topic pub /test_topic std_msgs/msg/String \
    '{data: "test message"}' \
    --qos-reliability reliable \
    --qos-durability transient_local \
    --qos-history keep_last \
    --qos-depth 10

# 查看QoS兼容性
ros2 topic info /scan --verbose | grep QoS

# 使用QoS配置文件启动节点
ros2 run my_package my_node --ros-args \
    --params-file config/qos_profiles.yaml

# 监控QoS匹配状态
ros2 doctor --report | grep -i qos

# 设置DDS详细日志
export RCUTILS_LOGGING_SERVERITY=DEBUG
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
export FASTRTPS_DEFAULT_PROFILES_FILE=config/dds_profiles.xml

参考资料：https://github.com/0voice