# ros2学习

**Repository Path**: linzhenghan/ros2-learning

## Basic Information

- **Project Name**: ros2学习
- **Description**: ros2 学习文档和代码记录
- **Primary Language**: C++
- **License**: Apache-2.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2026-02-08
- **Last Updated**: 2026-07-24

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# ros2 学习

本仓库按章节组织，各章代码位于对应工作空间中：

| 章节 | 工作空间路径 | 主要内容 |
|------|-------------|----------|
| 2 | `chapt2/chapt2_ws` | C++ 基础节点 |
| 3 | `chapt3/topic_ws` | 海龟话题发布/订阅 |
| 3 | `chapt3/topic_practice_ws` | 系统状态话题 + Qt 显示 |
| 4 | `chapt4/chapt4_ws` | 自定义服务 + 海龟巡逻 + 人脸检测 |
| 5 | `chapt5/chapt5_ws` | TF 坐标变换（Python / C++） |
| 6 | `chapt6/chapt6_ws` | URDF/xacro 建模 + RViz 显示 + Gazebo 仿真 |

编译前请先 `cd` 到对应工作空间目录，再执行 `colcon build`。

## 1 - ros2 安装

- 使用鱼香ros一键安装

```shell
wget http://fishros.com/install -O fishros && . fishros
```

## 2 - ros2 基础

> 本章代码位于 `chapt2/chapt2_ws`。

### 使用功能包构筑一个简单的 C++ 节点

- 使用功能包组织 C++ 节点

```shell
ros2 pkg create demo_cpp_pkg --build-type ament_cmake --license Apache-2.0
```

demo_cpp_pkg 是功能包的名称
--build-type ament_cmake 表示使用 cmake 构建
--license Apache-2.0 表示使用 Apache-2.0 许可证

- 添加 c++ 节点

```shell
cd demo_cpp_pkg/src
touch cpp_node.cpp
```

- 编写最简单的c++节点

```cpp
#include "rclcpp/rclcpp.hpp"

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("cpp_node");
  RCLCPP_INFO(node->get_logger(), "Hello ROS2!");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
```

- 编译节点
修改CMakeLists.txt

```cmake
find_package(rclcpp REQUIRED)
add_executable(cpp_node src/cpp_node.cpp)
ament_target_dependencies(cpp_node rclcpp)

install(TARGETS
  cpp_node
  DESTINATION lib/${PROJECT_NAME})
```

修改package.xml
添加依赖

```xml
<depend>rclcpp</depend>
```

- 运行节点

```shell
source install/setup.bash
ros2 run demo_cpp_pkg cpp_node
```

在 ros2 中 src 目录可以约定为工作空间，其中可以放不同的功能包，每个功能包可以有多个节点，在运行的过程中可以在工作空间同级目录下使用 colcon build 编译所有功能包。同时可以使用colcon build --packages-select demo_cpp_pkg 编译指定的功能包。
然后可以通过在 package.xml 中添加依赖来优先构筑某个功能包。

### 面向对象编程基础

在 ros2 开发中往往使用面对对象的编程方式，将节点封装为一个类，类中包含节点的属性和方法。

```cpp
#include "rclcpp/rclcpp.hpp"
#include <string>

class PersonNode : public rclcpp::Node
{
  private:
    std::string name_;
    int age_;

  public:

    PersonNode(const std::string &node_name, const std::string &name, const int &age) : Node(node_name)
    {
        this->name_ = name;
        this->age_ = age;
    }


    void eat(const std::string &food_name)
    {
        RCLCPP_INFO(this->get_logger(), "我是%s,今年%d岁,我正在吃%s", this->name_.c_str(), this->age_,
                    food_name.c_str());
    }

};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    
    auto node = std::make_shared<PersonNode>("cpp_node", "张三", 18);
    
    node->eat("鱼香肉丝");
    
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
```

在编译的参考上一章节的方法，将节点的名称改为 person_node 即可。

```shell
cd chapt2/chapt2_ws
colcon build --packages-select demo_cpp_pkg
source install/setup.bash
ros2 run demo_cpp_pkg person_node
```

## 3 - 话题-订阅和发布

> 本章代码分布在两个工作空间：`chapt3/topic_ws`（海龟话题）和 `chapt3/topic_practice_ws`（系统状态话题实践）。

ROS2 中的话题机制有四个关键点，分别是发布者、订阅者、话题名称和话题类型。

在本次案例中，利用ROS2中的海龟模拟器，先通过话题发布速度，控制海龟花园，然后通过话题订阅海龟的当前位置，根据当前位置和目标位置的差距来修正控制命令，实现对海龟位置的闭环控制。

#### 环境准备：运行海龟模拟器

```shell
ros2 run turtlesim turtlesim_node
```

#### 检查节点信息

```shell
ros2 node info /turtlesim
```

#### 观察位姿话题数据

```shell
ros2 topic echo /turtle1/pose
```

#### 查看话题详情

```shell
ros2 topic info /turtle1/cmd_vel -v
```

#### 查看消息接口定义

```shell
ros2 interface show geometry_msgs/msg/Twist
```

### 发布速度控制海龟

#### 步骤一：创建功能包

```shell
ros2 pkg create demo_cpp_topic --build-type ament_cmake --dependencies rclcpp geometry_msgs turtlesim --license Apache-2.0
```

- `demo_cpp_topic` 是功能包的名称
- `--build-type ament_cmake` 表示使用 cmake 构建系统
- `--dependencies rclcpp geometry_msgs turtlesim` 指定了功能包的依赖项：
  - `rclcpp`: ROS2的C++客户端库
  - `geometry_msgs`: 几何消息类型，用于传递位置、速度等信息
  - `turtlesim`: 海龟模拟器包
- `--license Apache-2.0` 表示使用 Apache-2.0 许可证

然后在功能包的 src 目录下创建 turtle_circle.cpp 文件，编写发布速度控制海龟花园的节点。

```cpp
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>

using namespace std::chrono_literals;

class TurtleCircle : public rclcpp::Node
{
    private:
        rclcpp::TimerBase::SharedPtr timer_;
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    public:
        explicit TurtleCircle(const std::string & node_name) : Node(node_name)
        {
            publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10);
            timer_ = this->create_wall_timer(500ms, std::bind(&TurtleCircle::timer_callback, this));
        }
    private:
        void timer_callback()
        {
            auto message = geometry_msgs::msg::Twist();
            message.linear.x = 0.5;
            message.angular.z = 0.5;
            publisher_->publish(message);
        }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtleCircle>("turtle_circle");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
```

#### 步骤二：配置 CMakeLists.txt

```cmake
cmake_minimum_required(VERSION 3.8)
project(demo_cpp_topic)

# 设置C++编译选项，开启常见警告
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(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)  # 虽然代码中没直接用，但保留你的依赖声明

# 构建可执行文件（turtle_circle节点）
add_executable(turtle_circle src/turtle_circle.cpp)
# 链接所有依赖包的头文件和库
ament_target_dependencies(turtle_circle 
  rclcpp 
  geometry_msgs 
  turtlesim  # 补充到依赖中，避免潜在的链接问题
)

# 安装可执行文件（核心修正：TARGET → TARGETS 复数形式）
install(TARGETS
  turtle_circle
  DESTINATION lib/${PROJECT_NAME}
)

# 测试相关配置（保留你的原有设置）
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # 跳过版权检查（新手可保留）
  set(ament_cmake_copyright_FOUND TRUE)
  # 跳过cpplint检查（新手可保留）
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# 声明ament包（ROS2必需）
ament_package()

```

#### 步骤三：编译节点

```shell
cd chapt3/topic_ws
colcon build --packages-select demo_cpp_topic
source install/setup.bash
```

#### 步骤四：启动海龟模拟器

```shell
ros2 run turtlesim turtlesim_node
```

#### 步骤五：运行发布节点

```shell
ros2 run demo_cpp_topic turtle_circle
```

可以看到海龟在花园中以圆形运动。

### 订阅Pose实现闭环控制

完整实现代码

```cpp
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"

class TurtleController : public rclcpp::Node
{
public:
    TurtleController() : Node("turtle_controller")
    {
        velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "/turtle1/cmd_vel", 10);
        pose_subscription_ = this->create_subscription<turtlesim::msg::Pose>(
            "/turtle1/pose", 10,
            std::bind(&TurtleController::on_pose_received_, this, std::placeholders::_1));
    }

private:
    void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose) {
    auto message = geometry_msgs::msg::Twist();
    // 1.记录当前位置
    double current_x = pose->x;
    double current_y = pose->y;
    RCLCPP_INFO(this->get_logger(), "当前位置:(x=%f,y=%f)", current_x, current_y);

    // 2.计算距离目标的距离，与当前海龟朝向的角度差
    double distance =
        std::sqrt((target_x_ - current_x) * (target_x_ - current_x) +
                  (target_y_ - current_y) * (target_y_ - current_y));
    double angle =
        std::atan2(target_y_ - current_y, target_x_ - current_x) - pose->theta;

    // 3.控制策略：距离大于0.1继续运动，角度差大于0.2则原地旋转，否则直行
    if (distance > 0.1) {
      if(fabs(angle)>0.2)
      {
        message.angular.z = fabs(angle);
      }else{
        // 通过比例控制器计算输出速度
        message.linear.x = k_ * distance;

      }
    }

    // 4.限制最大值并发布消息
    if (message.linear.x > max_speed_) {
       message.linear.x = max_speed_;
    }
    velocity_publisher_->publish(message);
  }


private:
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscription_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
    double target_x_{1.0};  // 目标位置X,设置默认值1.0
    double target_y_{1.0};  // 目标位置Y,设置默认值1.0
    double k_{1.0};         // 比例系数，控制输出=误差*比例系数
    double max_speed_{3.0}; // 最大线速度，设置默认值3.0
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtleController>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
```

将上述代码保存为 `chapt3/topic_ws/src/demo_cpp_topic/src/turtle_control.cpp`，并在 CMakeLists.txt 中添加 `turtle_control` 可执行目标（与 `turtle_circle` 并列）后编译运行：

```shell
cd chapt3/topic_ws
colcon build --packages-select demo_cpp_topic
source install/setup.bash
ros2 run demo_cpp_topic turtle_control
```

### 话题通讯实践

#### 需求说明

1、创建一个小工具监看系统的实时状态信息，包括记录信息的实现、主机名称、cpu使用率、内存使用率、内存总大小、剩余内存、网络接受数据量和网络发送数据两
2、有一个简单的界面，可以将系统信息显示出来
3、可以在其他主机上查看数据

实际就是编写一个python节点获取系统信息并通过话题发布出来，接着使用c++编写一个显示节点，订阅这个话题并进行显示

首先在 `chapt3/topic_practice_ws/src` 目录下输入下面代码创建功能包

```shell
ros2 pkg create status_interfaces --build-type ament_cmake --dependencies rosidl_default_generators builtin_interfaces --license Apache-2.0
```

上面的指令用于创建一个名为 status_interfaces 的功能包，并为其添加 builtin_interfaces 和 rosidl_default_generators 这两个依赖。builtin_interfaces 是 ROS2 中用于定义基本数据类型的包，而 rosidl_default_generators 是用于生成 ROS2 接口代码的工具包。

在 ROS2 中，话题信息定义文件需要放置到功能包的msg目录下，文件名必须以大写字母开头且只能由大写字母、小写字母、数字组成。新建 SystemStatus.msg 文件，内容如下

```shell
builtin_interfaces/Time stamp # 时间戳
string host_name # 主机名称
float32 cpu_percent # cpu使用率
float32 memory_percent # 内存使用率
float32 memory_total # 内存总大小
float32 memory_available # 剩余内存
float32 net_sent # 网络发送数据量
float32 net_rev # 网络接受数据量
```

ros2 消息接口支持的9种数据类型

```shell
bool
byte
char
float32
float64
int8
int16
int32
int64
```

定义好数据接口文件后，需要在 CMakeLists.txt 中添加如下代码

```cmake
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/SystemStatus.msg"
  DEPENDENCIES builtin_interfaces 
)
```

因为消息接口中使用了 builtin_interfaces 中的 Time 类型，所以需要在 CMakeLists.txt 中添加对 builtin_interfaces 的依赖

除了修改CMakeLists.txt 文件，还需要在 package.xml 文件中添加对 builtin_interfaces 的依赖

```xml
<member_of_group>rosidl_interface_packages</member_of_group>
```

在 package.xml 文件中添加对 member_of_group 标签来声明该功能包是一个消息接口功能包。

然后就可以编译该功能包了

```shell
cd chapt3/topic_practice_ws
colcon build --packages-select status_interfaces
source install/setup.bash
```

#### 验证接口是否生成成功

```shell
source install/setup.bash
ros2 interface show status_interfaces/msg/SystemStatus
```

接下来在src目录下，使用如下命令创建一个status_publisher功能包，并添加消息接口status_interfaces和客户端库rclpy作为依赖

```shell
ros2 pkg create status_publisher --build-type ament_python --dependencies rclpy status_interfaces --license Apache-2.0
```

在对应目录下添加 sys_status_pub.py 文件，内容如下

```python
# 导入ROS 2核心库，用于创建节点、初始化等基础操作
import rclpy
# 导入ROS 2节点基类，所有自定义节点需继承该类
from rclpy.node import Node
# 导入自定义的系统状态消息类型（需提前编译status_interfaces功能包）
from status_interfaces.msg import SystemStatus
# 导入系统监控库，用于获取CPU、内存、网络等硬件信息
import psutil
# 导入系统信息库，用于获取主机名等系统基础信息
import platform


class SystemStatusPublisher(Node):
    """
    系统状态发布节点类
    继承自ROS 2的Node基类，实现周期性发布系统状态的功能
    """

    def __init__(self):
        """
        构造函数：初始化节点、创建发布者、设置定时器
        """
        # 调用父类Node的构造函数，指定节点名称（ROS 2中节点名需唯一）
        super().__init__('system_status_publisher')
        
        # 创建发布者对象
        # 参数1：消息类型（自定义的SystemStatus）
        # 参数2：话题名称（system_status，其他节点可通过该话题订阅数据）
        # 参数3：队列大小（10，缓存最多10条未发送消息，超出则丢弃旧消息）
        self.publisher_ = self.create_publisher(SystemStatus, 'system_status', 10)
        
        # 创建定时器，实现周期性执行回调函数
        # 参数1：定时周期（1.0秒，即每秒执行1次）
        # 参数2：定时器触发时执行的回调函数（publish_status）
        self.timer = self.create_timer(1.0, self.publish_status)
        
        # 打印初始化日志，提示节点已启动
        self.get_logger().info('System status publisher node has been initialized.')

    def publish_status(self):
        """
        定时器回调函数：采集系统状态并发布消息
        每1秒被定时器触发一次，完成数据采集、消息封装、话题发布
        """
        # 采集系统状态数据
        # 获取CPU整体使用率（百分比，如25.5表示25.5%）
        cpu_percent = psutil.cpu_percent()
        # 获取虚拟内存信息（包含总内存、可用内存、使用率等）
        memory_info = psutil.virtual_memory()
        # 获取网络IO统计信息（包含累计发送/接收字节数等）
        net_io_counters = psutil.net_io_counters()

        # 创建自定义消息对象，用于封装要发布的系统状态数据
        msg = SystemStatus()
        
        # 填充消息字段
        # 设置消息时间戳（ROS 2标准时间格式）
        msg.stamp = self.get_clock().now().to_msg()
        # 设置主机名（如ubuntu-pc）
        msg.host_name = platform.node()
        # 设置CPU使用率
        msg.cpu_percent = cpu_percent
        # 设置内存使用率（百分比）
        msg.memory_percent = memory_info.percent
        # 设置总内存（字节转换为MB，方便阅读）
        msg.memory_total = memory_info.total / 1024 / 1024
        # 设置可用内存（字节转换为MB）
        msg.memory_available = memory_info.available / 1024 / 1024
        # 设置累计发送网络数据量（字节转换为MB）
        msg.net_sent = net_io_counters.bytes_sent / 1024 / 1024
        # 设置累计接收网络数据量（字节转换为MB）
        msg.net_recv = net_io_counters.bytes_recv / 1024 / 1024

        # 发布消息到system_status话题
        self.publisher_.publish(msg)
        
        # 打印INFO级日志，提示消息已发布（调试用）
        self.get_logger().info(f'Published system status for {msg.host_name} | CPU: {cpu_percent}% | Memory: {memory_info.percent}%')


def main(args=None):
    """
    主函数：ROS 2节点入口
    """
    # 初始化ROS 2 Python客户端库
    rclpy.init(args=args)
    
    # 创建系统状态发布节点实例
    node = SystemStatusPublisher()

    try:
        # 进入ROS 2事件循环（自旋），保持节点运行，处理定时器/回调等事件
        rclpy.spin(node)
    finally:
        # 无论是否发生异常，最终销毁节点，释放资源
        node.destroy_node()
        # 关闭ROS 2 Python客户端库，清理环境
        rclpy.shutdown()


# 脚本直接运行时执行主函数（导入为模块时不执行）
if __name__ == '__main__':
    main()
```

在代码中，首先导入rclpy库的核心类Node，接着从status_interfaces.msg模块导入自定义消息类SystemStatus，同时导入psutil与platform系统交互库。其中，psutil用于采集系统底层维度数据，涵盖 CPU 利用率、内存占用率及网络流量等核心指标；platform模块则负责识别与获取当前主机的节点名称，为系统身份标识提供支撑。
随后，定义SysStatusPub类并使其继承自Node类，以此封装 ROS 2 节点的业务逻辑。在类的初始化函数__init__中，完成两个核心组件的创建：一是实例化发布者status_publisher_，指定消息类型为SystemStatus并配置通信主题；二是初始化定时器timer，设置定时周期为 1 秒，绑定回调函数timer_callback，实现系统状态数据的周期性发布。
在timer_callback回调方法中，先通过get_clock().now()获取 ROS 2 节点时钟的当前时间，调用to_msg()方法将时间戳转换为builtin_interfaces.msg.Time类型，赋值给消息体的stamp字段，完成消息时间戳的标定。接着，借助psutil采集实时系统数据 —— 通过cpu_percent()获取 CPU 整体占用率，利用virtual_memory()解析内存的总容量、已用容量及使用率，结合网络接口信息统计网络收发字节数；再通过platform.node()获取主机名称，填充消息对应的设备标识字段。
考虑到系统默认数据单位为字节（B），对内存、网络流量等数据执行连续两次除以 1024 的换算，将单位转换为 MB，提升数据可读性。之后，将采集并处理后的时间戳、主机名、CPU / 内存 / 网络数据等信息，依次赋值给SystemStatus消息对象的各个字段。完成消息封装后，先将数据转换为字符串格式打印输出，便于实时日志查看，最后通过status_publisher_发布者的publish()方法，将系统状态消息发送至 ROS 2 通信网络。
main函数中包含节点初始化、自旋执行与资源回收等基础流程，为保证代码简洁性，此处不展开详细说明。后续需在setup.py配置文件中，将sys_status_pub节点注册到项目构建列表中，完成节点的编译与运行配置。编译阶段可执行代码清单 3-33 中的编译命令，完成代码的构建打包；运行阶段则通过对应的启动命令，即可启动该系统状态发布节点，实现持续的系统数据上报。

然后再setup.py文件中添加如下代码

```python
entry_points={
    'console_scripts': [
        'sys_status_pub = status_publisher.sys_status_pub:main',
    ],
},
```

#### 编译发布节点

```shell
cd chapt3/topic_practice_ws
colcon build --packages-select status_publisher
source install/setup.bash
```

#### 运行发布节点

```shell
source install/setup.bash
ros2 run status_publisher sys_status_pub
```

#### 在另一个终端查看话题数据

```shell
source install/setup.bash
ros2 topic echo /system_status
```

#### 在功能包中使用 Qt 进行界面显示

#### 步骤一：创建 Qt 功能包

```shell
ros2 pkg create status_display --build-type ament_cmake --dependencies rclpy status_interfaces --license Apache-2.0
```

然后在src文件夹下新建hello_qt.cpp文件，代码如下

```cpp
#include <QApplication>
#include <QLabel>
#include <QString>

int main(int argc, char* argv[]) {
    QApplication app(argc, argv);
    QLabel* label = new QLabel();
    QString message = QString::fromStdString("Hello Qt!");
    label->setText(message);
    label->show();
    app.exec();
    return 0;
}
```

然后在CMakeLists.txt文件中添加如下代码

```cmake
find_package(Qt5 REQUIRED Widgets)

add_executable(hello_qt src/hello_qt.cpp)
target_link_libraries(hello_qt Qt5::Widgets)

install(TARGETS
  hello_qt
  DESTINATION lib/${PROJECT_NAME})
```

#### 步骤二：编译 Qt 示例

```shell
cd chapt3/topic_practice_ws
# 注意：如果没有提前编译自定义消息包，需要先编译一次
colcon build --packages-select status_interfaces
# 然后再编译 status_display 包
colcon build --packages-select status_display
source install/setup.bash
```

#### 步骤三：运行 hello_qt 示例

```shell
source install/setup.bash
ros2 run status_display hello_qt
```

#### 步骤四：订阅话题并更新 Qt 界面

具体代码如下

```cpp
#include <QApplication>
#include <QLabel>
#include <QString>
#include "rclcpp/rclcpp.hpp"
#include "status_interfaces/msg/system_status.hpp"
using SystemStatus = status_interfaces::msg::SystemStatus;
class SysStatusDisplay : public rclcpp::Node {
public:
    SysStatusDisplay() : Node("sys_status_display") {
        subscription_ = this->create_subscription<SystemStatus>(
            "system_status", 10, [&](const SystemStatus::SharedPtr msg) -> void {
                label_->setText(get_qstr_from_msg(msg));
            });
        // 创建一个空的 SystemStatus 对象，转化成 QString 进行显示
        label_ = new QLabel(get_qstr_from_msg(std::make_shared<SystemStatus>()));
        label_->show();
    }

    QString get_qstr_from_msg(const SystemStatus::SharedPtr msg) {
        std::stringstream show_str;
        show_str
            << "========== 系统状态可视化显示工具 =========="
            << "\n 数 据 时 间:\t" << msg->stamp.sec << "\ts\n"
            << " 用 户 名:\t" << msg->host_name << "\t\n"
            << "CPU使用率:\t" << msg->cpu_percent << "\t%\n"
            << " 内存使用率:\t" << msg->memory_percent << "\t%\n"
            << " 内存大小:\t" << msg->memory_total << "\tMb\n"
            << " 剩余有效内存:\t" << msg->memory_available << "\tMB\n"
            << " 网络发送量:\t" << msg->net_sent << "\tMB\n"
            << " 网络接收量:\t" << msg->net_recv << "\tMB\n"
            << "=====================================";

        return QString::fromStdString(show_str.str());
    }

private:
    rclcpp::Subscription<SystemStatus>::SharedPtr subscription_;
    QLabel* label_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    QApplication app(argc, argv);
    auto node = std::make_shared<SysStatusDisplay>();
    std::thread spin_thread([&]() -> void { rclcpp::spin(node); });
    spin_thread.detach();
    app.exec();
    rclcpp::shutdown();
    return 0;
}

```

这段代码实现了一个 ROS2 + Qt 的系统状态可视化小工具。它定义了 `SysStatusDisplay` 节点，订阅 `system_status` 话题（消息类型 `status_interfaces::msg::SystemStatus`），每次收到消息就把 CPU、内存、网络等指标格式化为字符串，并实时更新到 `QLabel` 上显示。程序启动时先初始化 ROS2 和 Qt，然后创建节点与窗口；通过单独线程执行 `rclcpp::spin(node)` 保持 ROS2 回调运行，主线程执行 `app.exec()` 维持 Qt 事件循环。整体设计的关键点是：**用线程把 ROS2 通信循环和 Qt GUI 循环并行起来**，从而实现"后台订阅数据 + 前台实时刷新界面"的效果。

#### 步骤五：编译并联合运行

```shell
cd chapt3/topic_practice_ws
colcon build --packages-select status_interfaces status_publisher status_display
source install/setup.bash
```

终端 1 — 发布系统状态：

```shell
ros2 run status_publisher sys_status_pub
```

终端 2 — Qt 界面显示：

```shell
ros2 run status_display sys_status_display
```

两个节点同时运行后，Qt 窗口会实时显示系统状态，并随话题数据更新而刷新。

## 4 - 服务和参数

> 本章代码位于 `chapt4/chapt4_ws`。前半部分通过 turtlesim 了解服务和参数的基础概念；后半部分（4.1 起）为自定义服务实践。

服务是基于请求和响应的双向通信机制，而参数主要用于管理节点的设置，在ROS2中参数通讯主要是基于服务通信进行实现的。

### 通过海龟模拟器了解服务

#### 步骤一：启动海龟模拟器

```shell
ros2 run turtlesim turtlesim_node
```

#### 步骤二：查看可用服务

```shell
ros2 service list -t
```

#### 服务列表输出示例

```shell
/clear [std_srvs/srv/Empty]
/kill [turtlesim/srv/Kill]
/reset [std_srvs/srv/Empty]
/spawn [turtlesim/srv/Spawn]
/turtle1/set_pen [turtlesim/srv/SetPen]
/turtle1/teleport_absolute [turtlesim/srv/TeleportAbsolute]
/turtle1/teleport_relative [turtlesim/srv/TeleportRelative]
/turtle2/set_pen [turtlesim/srv/SetPen]
/turtle2/teleport_absolute [turtlesim/srv/TeleportAbsolute]
/turtle2/teleport_relative [turtlesim/srv/TeleportRelative]
/turtlesim/describe_parameters [rcl_interfaces/srv/DescribeParameters]
/turtlesim/get_parameter_types [rcl_interfaces/srv/GetParameterTypes]
/turtlesim/get_parameters [rcl_interfaces/srv/GetParameters]
/turtlesim/list_parameters [rcl_interfaces/srv/ListParameters]
/turtlesim/set_parameters [rcl_interfaces/srv/SetParameters]
/turtlesim/set_parameters_atomically [rcl_interfaces/srv/SetParametersAtomically]
```

#### 步骤三：查看 Spawn 服务接口

```shell
ros2 interface show turtlesim/srv/Spawn 
```

#### Spawn 接口定义示例

```shell
float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name
```

`---` 隔开的上半部分为请求接口的定义，下半部分为响应接口的定义。`/spawn` 服务用于在海龟模拟器中生成新的海龟，请求接口包含海龟的初始位置 (x, y, theta) 和可选的名称。响应接口返回新海龟的名称。

#### 步骤四：调用 Spawn 服务

```shell
ros2 service call /spawn turtlesim/srv/Spawn "{x: 1,y: 1}"
```

得到如下结果

```shell
requester: making request: turtlesim.srv.Spawn_Request(x=1.0, y=1.0, theta=0.0, name='')

response:
turtlesim.srv.Spawn_Response(name='turtle2')
```

#### 步骤五：使用 rqt 图形化调用服务

```shell
rqt
```

### 基于服务的参数通讯

在ROS2中，参数被视为节点的设置，而参数通信机制是基于服务通信实现的。

运行海龟模拟器后，输入如下命令查看可用参数服务

```shell
ros2 service list -t | grep parameter

/turtlesim/describe_parameters [rcl_interfaces/srv/DescribeParameters]
/turtlesim/get_parameter_types [rcl_interfaces/srv/GetParameterTypes]
/turtlesim/get_parameters [rcl_interfaces/srv/GetParameters]
/turtlesim/list_parameters [rcl_interfaces/srv/ListParameters]
/turtlesim/set_parameters [rcl_interfaces/srv/SetParameters]
/turtlesim/set_parameters_atomically [rcl_interfaces/srv/SetParametersAtomically]
```

可以使用如下命令查看参数信息

```shell
ros2 param list

lin@lin-Lenovo-Legion-R7000-2020:~/ros2-learning$ ros2 param list
/turtlesim:
  background_b
  background_g
  background_r
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  use_sim_time
```

可以通过如下命令读取某个参数的具体描述

```shell
ros2 param describe /turtlesim background_r
```

输出示例：

```shell
Parameter name: background_r
  Type: integer
  Description: Red channel of the background color
  Constraints:
    Min value: 0
    Max value: 255
    Step: 1
```

获取节点的参数值

```shell
ros2 param get /turtlesim background_r
```

修改参数的值

```shell
ros2 param set /turtlesim background_r 255
```

### 4.1 工作空间与功能包概览

第 4 章代码位于 `chapt4/chapt4_ws`，包含三个功能包：

| 功能包 | 类型 | 说明 |
|--------|------|------|
| `chapt4_interfaces` | 接口包 | 定义 `Patrol`、`FaceDetector` 两个 `.srv` 服务 |
| `demo_cpp_service` | C++ 包 | 海龟巡逻：服务端 `turtle_control` + 客户端 `patrol_client` |
| `demo_python_service` | Python 包 | 人脸检测：服务端 `face_detect_node` + 客户端 `face_detect_client_node` |

整体通信关系：

```
patrol_client  ──Patrol 服务──▶  turtle_control  ──Twist──▶  turtlesim
                                      ▲                          │
                                      └──────── Pose ────────────┘

face_detect_client  ──FaceDetector 服务──▶  face_detect_node
```

> **与第 3 章的区别**：第 3 章 `chapt3/topic_ws` 中的 `turtle_control` 仅通过话题实现闭环控制，目标点固定为 `(1.0, 1.0)`；第 4 章 `turtle_control` 在此基础上增加了 `Patrol` 服务，允许客户端动态设置巡逻目标点。

### 4.2 自定义服务接口：`chapt4_interfaces`

#### 逻辑分析

ROS2 服务采用**请求-响应**模式：客户端发送 Request，服务端处理后返回 Response，两者用 `---` 分隔。

本包定义两个服务：

1. **Patrol**：客户端传入目标坐标 `(target_x, target_y)`，服务端校验后返回是否接受（`SUCCESS` / `FALL`）
2. **FaceDetector**：客户端发送图像，服务端检测人脸后返回数量、耗时及各人脸框坐标

#### 语法分析

**Patrol.srv**（`chapt4/chapt4_ws/src/chapt4_interfaces/srv/Patrol.srv`）

```shell
float32 target_x # 目标 x 值
float32 target_y # 目标 y 值
---
int8 SUCCESS = 1
int8 FALL = 0
int8 result
```

- `---` 上方为 Request，下方为 Response
- `int8 SUCCESS = 1` 是**常量定义**，编译后可通过 `Patrol::Response::SUCCESS` 访问
- `FALL` 表示目标点校验失败（代码中拼写为 FALL，语义为 FAIL）

**FaceDetector.srv**（`chapt4/chapt4_ws/src/chapt4_interfaces/srv/FaceDetector.srv`）

```shell
sensor_msgs/Image image # 原始图像
---
int16 number            # 人脸数
float32 use_time        # 识别耗时（秒）
int32[] top
int32[] right
int32[] bottom
int32[] left
```

- Request 使用 `sensor_msgs/Image`，需在 CMakeLists.txt 中声明 `sensor_msgs` 依赖
- Response 用四个 `int32[]` 数组存储每张人脸的 `(top, right, bottom, left)`，同一索引对应同一张脸

**CMakeLists.txt 关键配置：**

```cmake
find_package(rosidl_default_generators REQUIRED)
find_package(sensor_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/FaceDetector.srv"
  "srv/Patrol.srv"
  DEPENDENCIES sensor_msgs
)
```

**package.xml** 需声明接口包身份：

```xml
<member_of_group>rosidl_interface_packages</member_of_group>
```

#### 编译与验证

```shell
cd chapt4/chapt4_ws
colcon build --packages-select chapt4_interfaces
source install/setup.bash

ros2 interface show chapt4_interfaces/srv/Patrol
ros2 interface show chapt4_interfaces/srv/FaceDetector
```

### 4.3 C++ 海龟巡逻服务：`demo_cpp_service`

#### 逻辑分析

在 chapt3 话题闭环控制的基础上，增加 **Patrol 服务**，实现"客户端设目标 → 服务端驱动海龟移动"。

**turtle_control（服务端）流程：**

1. 订阅 `/turtle1/pose` 获取海龟当前位置
2. 注册 `patrol` 服务，接收客户端传入的 `(target_x, target_y)`
3. 校验坐标是否在 `(0, 12)` 范围内：
   - 合法 → 更新 `target_x_` / `target_y_`，返回 `SUCCESS`
   - 非法 → 保持原目标不变，返回 `FALL`
4. 在 pose 回调中执行比例控制：
   - 距离 > 0.1 时继续运动
   - 角度差 `|angle| > 0.2` 时原地旋转（`angular.z = |angle|`）
   - 否则直行（`linear.x = k_ × distance`，上限 `max_speed_`）

**patrol_client（客户端）流程：**

1. 每 10 秒触发一次定时器
2. 等待 `patrol` 服务上线
3. 随机生成 `(0~14, 0~14)` 的目标坐标（部分超出有效范围，用于测试 `FALL`）
4. 通过 `async_send_request` 异步发送请求，在回调中打印 SUCCESS / FALL

#### 语法分析

**服务创建（lambda 回调）：**

```cpp
using Patrol = chapt4_interfaces::srv::Patrol;

patrol_server_ = this->create_service<Patrol>(
    "patrol",
    [this](
        const std::shared_ptr<Patrol::Request> request,
        std::shared_ptr<Patrol::Response> response) -> void {
        if ((0 < request->target_x && request->target_x < 12.0f) &&
            (0 < request->target_y && request->target_y < 12.0f)) {
            target_x_ = request->target_x;
            target_y_ = request->target_y;
            response->result = Patrol::Response::SUCCESS;
        } else {
            response->result = Patrol::Response::FALL;
        }
    });
```

- `create_service<Patrol>("patrol", callback)`：注册名为 `patrol` 的服务（完整服务名 `/patrol`）
- lambda 捕获 `[this]`，可在回调中访问成员变量 `target_x_`、`target_y_`

**异步客户端：**

```cpp
patrol_client_ = this->create_client<Patrol>("patrol");

patrol_client_->async_send_request(
    request,
    [&](rclcpp::Client<Patrol>::SharedFuture result_future) -> void {
        auto response = result_future.get();
        if (response->result == Patrol::Response::SUCCESS) {
            RCLCPP_INFO(this->get_logger(), "目标点处理成功 ");
        } else if (response->result == Patrol::Response::FALL) {
            RCLCPP_INFO(this->get_logger(), "目标点处理失败 ");
        }
    });
```

- `create_client<Patrol>("patrol")`：创建服务客户端
- `wait_for_service()`：阻塞等待服务端上线
- `async_send_request` + `SharedFuture`：异步调用，不阻塞 spin 线程

#### 编译与运行

**编译：**

```shell
cd chapt4/chapt4_ws
colcon build --packages-select chapt4_interfaces demo_cpp_service
source install/setup.bash
```

**终端 1 — 海龟模拟器：**

```shell
ros2 run turtlesim turtlesim_node
```

**终端 2 — 巡逻服务端：**

```shell
source chapt4/chapt4_ws/install/setup.bash
ros2 run demo_cpp_service turtle_control
```

**终端 3 — 巡逻客户端（或命令行测试）：**

```shell
# 方式 A：自动随机巡逻（每 10 秒发一次请求）
ros2 run demo_cpp_service patrol_client

# 方式 B：手动调用服务
ros2 service call /patrol chapt4_interfaces/srv/Patrol "{target_x: 5.0, target_y: 5.0}"
ros2 service call /patrol chapt4_interfaces/srv/Patrol "{target_x: 15.0, target_y: 5.0}"  # 超出范围，返回 FALL
```

**调试命令：**

```shell
ros2 service list -t | grep patrol
ros2 service type /patrol
ros2 node info /turtle_controller
ros2 node info /patrol_client
```

### 4.4 Python 人脸检测服务：`demo_python_service`

#### 逻辑分析

用 Python 实现图像人脸检测服务，演示 **sensor_msgs/Image 服务传输 + OpenCV/face_recognition 处理**。

| 节点 | 入口命令 | 作用 |
|------|----------|------|
| `learn_face_detect` | 独立脚本 | 学习用，不依赖 ROS，直接 OpenCV 显示检测结果 |
| `face_detect_node` | 服务端 | 提供 `/face_detect` 服务 |
| `face_detect_client_node` | 客户端 | 发送图片、接收结果、画框显示 |

**face_detect_node（服务端）流程：**

1. 注册 `/face_detect` 服务
2. 收到请求后解析图像：
   - `request.image.data` 非空 → 通过 CvBridge 转为 OpenCV 图像
   - 为空 → 读取包内默认图片 `resource/default.jpg`
3. 调用 `face_recognition.face_locations()` 检测（HOG 模型，CPU 运行）
4. 填充响应字段：`number`（人脸数）、`use_time`（耗时）、`top/right/bottom/left`（坐标数组）
5. 返回 response

**face_detect_client_node（客户端）流程：**

1. 读取默认图片 `default.jpg`
2. 等待 `/face_detect` 服务上线
3. 通过 `call_async` + `spin_until_future_complete` 同步等待响应
4. 根据返回坐标在图像上画矩形框，`cv2.imshow` 显示结果

#### 语法分析

**服务注册（Python）：**

```python
self.service = self.create_service(
    FaceDetector, '/face_detect', self.detect_face_callback)

def detect_face_callback(self, request, response):
    if request.image.data:
        cv_image = self.bridge.imgmsg_to_cv2(request.image)
    else:
        cv_image = cv2.imread(self.default_image_path)
    face_locations = face_recognition.face_locations(
        cv_image, number_of_times_to_upsample=self.upsample_times, model=self.model)
    response.number = len(face_locations)
    response.use_time = end_time - start_time
    for top, right, bottom, left in face_locations:
        response.top.append(top)
        # ...
    return response
```

- Python 服务回调签名：`(request, response) -> response`，直接返回 response 对象
- 与 C++ 不同，无需 `shared_ptr` 和 lambda

**CvBridge 图像转换：**

```python
# ROS Image → OpenCV
cv_image = self.bridge.imgmsg_to_cv2(request.image)

# OpenCV → ROS Image
request.image = self.bridge.cv2_to_imgmsg(self.image)
```

**setup.py 资源安装：**

```python
data_files=[
    ('share/' + package_name + '/resource', ['resource/default.jpg']),
],
entry_points={
    'console_scripts': [
        'learn_face_detect = demo_python_service.learn_face_detect:main',
        'face_detect_node = demo_python_service.face_detect_node:main',
        'face_detect_client_node = demo_python_service.face_detect_client_node:main',
    ],
},
```

`default.jpg` 安装到 `install/.../share/demo_python_service/resource/`，运行时通过 `get_package_share_directory('demo_python_service')` 定位。

#### 依赖安装

`package.xml` 未声明以下运行时依赖，需手动安装：

```shell
# ROS2 包
sudo apt install ros-${ROS_DISTRO}-cv-bridge ros-${ROS_DISTRO}-sensor-msgs

# Python 库（face_recognition 依赖 dlib，编译较慢）
pip install face_recognition opencv-python
```

#### 编译与运行

**编译：**

```shell
cd chapt4/chapt4_ws
colcon build --packages-select chapt4_interfaces demo_python_service
source install/setup.bash
```

**终端 1 — 学习脚本（可选，不依赖 ROS 服务）：**

```shell
ros2 run demo_python_service learn_face_detect
```

**终端 2 — 人脸检测服务端：**

```shell
ros2 run demo_python_service face_detect_node
```

**终端 3 — 人脸检测客户端：**

```shell
ros2 run demo_python_service face_detect_client_node
```

**命令行测试（空 image 使用默认图）：**

```shell
ros2 service call /face_detect chapt4_interfaces/srv/FaceDetector \
  "{image: {header: {frame_id: ''}, height: 0, width: 0, encoding: '', is_bigendian: 0, step: 0, data: []}}"
```

### 4.5 测试

#### chapt4 工作空间

**C++ 包（ament lint）：**

```shell
cd chapt4/chapt4_ws
colcon test --packages-select demo_cpp_service chapt4_interfaces
colcon test-result --verbose
```

**Python 包（flake8 / pep257 / copyright）：**

```shell
colcon test --packages-select demo_python_service
colcon test-result --verbose
```

`demo_python_service/test/` 目录包含：

- `test_flake8.py`：PEP8 代码风格检查
- `test_pep257.py`：docstring 规范检查
- `test_copyright.py`：版权声明检查

**一键编译并测试全部包：**

```shell
cd chapt4/chapt4_ws
colcon build --packages-select chapt4_interfaces demo_cpp_service demo_python_service
colcon test
colcon test-result --verbose
```

#### 其他章节工作空间

```shell
# chapt3 话题实践
cd chapt3/topic_practice_ws
colcon test --packages-select status_publisher
colcon test-result --verbose

# chapt3 海龟话题
cd chapt3/topic_ws
colcon test --packages-select demo_cpp_topic
colcon test-result --verbose
```

#### 集成测试（手动验证）

| 场景 | 步骤 | 预期结果 |
|------|------|----------|
| Patrol SUCCESS | turtlesim + turtle_control + `ros2 service call ... {5.0, 5.0}` | 海龟移向 (5, 5) |
| Patrol FALL | 调用 `{15.0, 5.0}` | 日志显示"目标点处理失败" |
| 随机巡逻 | turtlesim + turtle_control + patrol_client | 每 10 秒随机目标，部分失败 |
| 人脸检测 | face_detect_node + face_detect_client_node | 弹出窗口，蓝框标注人脸 |

### 4.6 ROS2 参数机制

ROS2 中每个节点在 `declare_parameter` 之后，会自动暴露 `{节点名}/set_parameters` 等服务，参数通信本质上是基于服务实现的。

#### 参数相关命令

```shell
ros2 param list
ros2 param get /turtle_controller k
ros2 param set /turtle_controller max_speed 3.0
```

#### Python 节点（face_detect_node）

**启动时声明并读取：**

```python
from rcl_interfaces.msg import SetParametersResult

self.declare_parameter('face_locations_upsample_times', 1)
self.declare_parameter('face_locations_model', "hog")
self.model = self.get_parameter("face_locations_model").value
self.upsample_times = self.get_parameter("face_locations_upsample_times").value
```

- `declare_parameter`：注册参数并设置默认值
- `get_parameter(...).value`：读取当前值（定义在 `rclpy.node.Node` 中）
- `get_parameter` 不是用户自定义函数，而是继承自 ROS2 的 `Node` 基类

**运行中动态更新（参数回调）：**

```python
self.add_on_set_parameters_callback(self.parameter_callback)

def parameter_callback(self, parameters):
    for parameter in parameters:
        self.get_logger().info(f'参数 {parameter.name} 设置为: {parameter.value}')
        if parameter.name == 'face_locations_upsample_times':
            self.upsample_times = parameter.value
        elif parameter.name == 'face_locations_model':
            self.model = parameter.value
    return SetParametersResult(successful=True)
```

| 机制 | 时机 | 作用 |
|------|------|------|
| `declare_parameter` + `get_parameter` | 节点启动时 | 读取初始配置 |
| `add_on_set_parameters_callback` | 运行中 `ros2 param set` 时 | 动态更新成员变量 |

#### C++ 节点（turtle_control）

**启动时声明并读取：**

```cpp
this->declare_parameter("k", 1.0);
this->declare_parameter("max_speed", 1.0);
this->get_parameter("k", k_);
this->get_parameter("max_speed", max_speed_);
```

- `declare_parameter` / `get_parameter` 定义在 `rclcpp::Node` 中
- C++ 的 `get_parameter("k", k_)` 直接把值写入变量，无需 `.value`

**运行中动态更新（参数回调）：**

```cpp
parameters_callback_handle_ = this->add_on_set_parameters_callback(
    [&](const std::vector<rclcpp::Parameter> &params) -> rcl_interfaces::msg::SetParametersResult {
        for (auto param : params) {
            RCLCPP_INFO(this->get_logger(), "更新参数 %s 值为: %f",
                        param.get_name().c_str(), param.as_double());
            if (param.get_name() == "k") {
                k_ = param.as_double();
            } else if (param.get_name() == "max_speed") {
                max_speed_ = param.as_double();
            }
        }
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        return result;
    });

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;
```

> **注意**：必须保存 `parameters_callback_handle_`，否则回调可能被注销。

**参数在控制逻辑中的使用：**

```cpp
message.linear.x = k_ * distance;          // 比例控制
if (message.linear.x > max_speed_) {       // 速度限幅
    message.linear.x = max_speed_;
}
```

#### 客户端远程修改服务端参数（patrol_client）

通过 ROS2 标准服务 `/turtle_controller/set_parameters` 修改对方节点参数：

```cpp
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"

using SetP = rcl_interfaces::srv::SetParameters;

std::shared_ptr<SetP::Response> call_set_parameters(rcl_interfaces::msg::Parameter &parameter)
{
    auto param_client = this->create_client<SetP>("/turtle_controller/set_parameters");
    while (!param_client->wait_for_service(std::chrono::seconds(1))) {
        if (!rclcpp::ok()) return nullptr;
    }
    auto request = std::make_shared<SetP::Request>();
    request->parameters.push_back(parameter);

    auto future = param_client->async_send_request(request);
    rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
    return future.get();
}

void update_server_param_k(double k)
{
    rcl_interfaces::msg::Parameter param;
    param.name = "k";
    param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
    param.value.double_value = k;
    call_set_parameters(param);
}
```

| 通信方式 | 服务名 | 作用 |
|----------|--------|------|
| 自定义 Patrol 服务 | `/patrol` | 修改巡逻目标点 |
| 标准 SetParameters 服务 | `/turtle_controller/set_parameters` | 修改 k、max_speed 等参数 |

> `update_server_param_k` 是**普通成员函数**，不是回调；需在 `timer_callback` 等地方**手动调用**才会执行。

#### 参数修改方式对比

| 方式 | 示例 | 生效时机 |
|------|------|----------|
| 硬编码 | `double k_{1.0}` | 编译时固定 |
| 启动传参 | `ros2 run ... --ros-args -p k:=2.0` | 节点启动时 |
| Launch 传参 | `launch_max_speed:=2.0` | 节点启动时 |
| 命令行改参 | `ros2 param set /turtle_controller k 2.0` | 运行中（需参数回调） |
| 客户端远程改参 | `call_set_parameters` | 运行中（通过服务） |

### 4.7 Launch 文件：一键启动多节点

Launch 文件用**一条命令**替代多个终端分别 `ros2 run`。

#### 基本结构

```python
import launch
import launch_ros

def generate_launch_description():
    action_node_turtle_control = launch_ros.actions.Node(
        package='demo_cpp_service',
        executable='turtle_control',
        output='screen')

    action_node_patrol_client = launch_ros.actions.Node(
        package='demo_cpp_service',
        executable='patrol_client',
        output='log')

    action_node_turtlesim_node = launch_ros.actions.Node(
        package='turtlesim',
        executable='turtlesim_node',
        output='both')

    return launch.LaunchDescription([
        action_node_turtle_control,
        action_node_patrol_client,
        action_node_turtlesim_node,
    ])
```

#### 执行流程

```
ros2 launch demo_cpp_service patrol.launch.py
        ↓
调用 generate_launch_description()
        ↓
得到 LaunchDescription（包含 3 个 Node 动作）
        ↓
Launch 系统并行启动各节点进程
        ↓
等价于同时执行三条 ros2 run 命令
```

| 语法 | 含义 |
|------|------|
| `generate_launch_description()` | Launch 固定入口函数，系统自动调用 |
| `launch_ros.actions.Node(...)` | 描述要启动的一个节点 |
| `package` + `executable` | 对应 `ros2 run package executable` |
| `LaunchDescription([...])` | 启动动作清单 |
| `output='screen'/'log'/'both'` | 日志输出到终端 / 文件 / 两者 |

#### 目录与安装

```
demo_cpp_service/
├── launch/
│   └── patrol.launch.py
└── CMakeLists.txt   # install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
```

运行：

```shell
colcon build --packages-select demo_cpp_service
source install/setup.bash
ros2 launch demo_cpp_service patrol.launch.py
```

### 4.8 Launch 参数传递

Launch 参数（对外）与节点 ROS2 参数（对内）是两层不同的名字：

```
launch_max_speed  →  Launch 参数名（命令行使用）
max_speed         →  节点 ROS2 参数名（C++ declare_parameter 使用）
```

#### 完整示例

```python
action_declare_arg_max_speed = launch.actions.DeclareLaunchArgument(
    'launch_max_speed', default_value='2.0')

action_node_turtle_control = launch_ros.actions.Node(
    package='demo_cpp_service',
    executable='turtle_control',
    output='screen',
    parameters=[{
        'max_speed': launch.substitutions.LaunchConfiguration(
            'launch_max_speed', default='2.0')
    }])

return launch.LaunchDescription([
    action_declare_arg_max_speed,
    action_node_turtle_control,
    action_node_patrol_client,
    action_node_turtlesim_node,
])
```

#### 语法说明

| 代码 | 作用 |
|------|------|
| `DeclareLaunchArgument('launch_max_speed', default_value='2.0')` | 向 Launch 系统注册参数，允许命令行覆盖 |
| `LaunchConfiguration('launch_max_speed')` | 启动时读取 Launch 参数的实际值 |
| `parameters=[{'max_speed': ...}]` | 将值传递给节点的 ROS2 参数 `max_speed` |
| `action_declare_arg_max_speed` | 仅是 Python 变量名，可任意命名；与命令行无关 |

> **易混淆点**：命令行使用的是 `'launch_max_speed'`（DeclareLaunchArgument 中的字符串），不是 Python 变量名 `action_declare_arg_max_speed`。变量可以省略，直接内联写在 `LaunchDescription([...])` 中，但 `DeclareLaunchArgument` 动作本身不能省略。

#### 参数传递链路

```
ros2 launch ... launch_max_speed:=3.5
        ↓
DeclareLaunchArgument 注册 launch_max_speed
        ↓
LaunchConfiguration 读取值 3.5
        ↓
Node(parameters=[{'max_speed': 3.5}])
        ↓
turtle_control: declare_parameter + get_parameter → max_speed_ = 3.5
```

#### 使用方式

```shell
# 使用默认值 2.0
ros2 launch demo_cpp_service patrol.launch.py

# 命令行覆盖
ros2 launch demo_cpp_service patrol.launch.py launch_max_speed:=3.5

# 等价于手动启动时：
ros2 run demo_cpp_service turtle_control --ros-args -p max_speed:=3.5
```

## 5 - ROS2常用工具

坐标变换示例，下面这条命令会发布一个静态 TF 变换：从 base_link 到 base_laser，平移为 (0.1, 0.0, 0.2) 米，旋转为 (0, 0, 0) 弧度。

```shell
ros2 run tf2_ros static_transform_publisher --x 0.1 --y 0.0 --z 0.2 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id base_laser
```

```shell
lin@lin-Lenovo-Legion-R7000-2020:~$ ros2 run tf2_ros static_transform_publisher --x 0.1 --y 0.0 --z 0.2 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id base_laser
[INFO] [1783427916.698352876] [static_transform_publisher_uPMM8F1YFxLJFYVd]: Spinning until stopped - publishing transform
translation: ('0.100000', '0.000000', '0.200000')
rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
from 'base_link' to 'base_laser'
^C[INFO] [1783427920.839920143] [rclcpp]: signal_handler(SIGINT/SIGTERM)

```

教程中的命令无法正常安装替换为如下命令
```shell
sudo apt install mrpt-apps -y
```

## 6 - 建模与仿真（创建自己的机器人）

> 本章代码位于 `chapt6/chapt6_ws`，功能包为 `fishbot_description`。  
> 目标：用 **URDF / xacro** 描述机器人，用 **RViz** 查看模型，用 **Gazebo** 做物理仿真与差速控制。

### 6.0 本章目录结构

```
chapt6/chapt6_ws/src/fishbot_description/
├── CMakeLists.txt / package.xml
├── launch/
│   ├── display_robot.launch.py   # RViz 显示模型
│   └── gazebo_sim.launch.py      # Gazebo 仿真
├── rviz/
│   └── display_robot.rviz
├── world/
│   └── custom_room.world         # Gazebo 房间世界
└── urdf/
    ├── first_robot.urdf          # 入门纯 URDF
    ├── first_robot.urdf.xacro    # 入门 xacro（宏）
    └── fishbot/                  # 模块化 fishbot
        ├── fishbot.urdf.xacro    # 总装入口
        ├── base.urdf.xacro
        ├── imu.urdf.xacro
        ├── camera.urdf.xacro
        ├── laser.urdf.xacro
        ├── wheel.urdf.xacro
        ├── caster.urdf.xacro
        ├── common_inertia.xacro
        └── plugins/
            └── gazebo_control_plugin.xacro
```

### 6.1 创建功能包

```shell
cd ~/ros2-learning/chapt6/chapt6_ws/src
ros2 pkg create fishbot_description --build-type ament_cmake --license Apache-2.0
```

在 `CMakeLists.txt` 中安装资源目录（否则 `get_package_share_directory` 找不到文件）：

```cmake
install(DIRECTORY launch urdf rviz world
  DESTINATION share/${PROJECT_NAME}
)
```

`package.xml` 中常见运行依赖：

```xml
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
```

相关 apt 安装：

```shell
sudo apt install ros-$ROS_DISTRO-robot-state-publisher
sudo apt install ros-$ROS_DISTRO-joint-state-publisher
sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs
sudo apt install ros-$ROS_DISTRO-xacro
```

### 6.2 URDF 基础语法

URDF（Unified Robot Description Format）是 **XML**，用来描述机器人结构。标签名由规范规定，不能随意改名；你能自定义的是属性值（如 `name="base_link"`）和模型怎么搭。

#### XML / 标签结构（可类比 C 的 `{ }`）

```xml
<link name="base_link">   <!-- 类似进入一块作用域 -->
  ...
</link>                   <!-- 类似 }，且必须写清结束的是哪个标签 -->
```

| 写法 | 含义 |
|------|------|
| `<?xml version="1.0"?>` | XML 声明 |
| `<!-- 注释 -->` | 注释（**不能嵌套**；注释里不要再写 `-->`，否则会提前结束注释导致解析失败） |
| `<标签 属性="值">...</标签>` | 成对标签 |
| `<标签 ... />` | 自闭合标签 |

#### 核心三层

```
<robot>          根元素，一份文件只有一个
  ├── <link>     刚体部件（机体、传感器、轮子…）
  └── <joint>    连接两个 link 的关节
```

#### link 内常见子标签

| 标签 | 作用 |
|------|------|
| `<visual>` | **外观**（RViz 显示用） |
| `<collision>` | **碰撞体**（物理引擎接触用，可简化形状） |
| `<inertial>` | **惯量**（质量 + 惯性张量，仿真动力学用） |
| `<origin xyz rpy>` | 位姿：平移（米）+ 欧拉角（弧度，roll/pitch/yaw） |
| `<geometry>` | 形状容器，内放一种：`box` / `cylinder` / `sphere` / `mesh` |
| `<material>` / `<color rgba>` | 颜色；`rgba` 四个数 0~1，最后一位是透明度 |

#### joint 常见属性

| 内容 | 说明 |
|------|------|
| `type="fixed"` | 固定，相对不可动 |
| `type="continuous"` | 可连续旋转（驱动轮） |
| `type="revolute"` | 有角度限位的旋转 |
| `type="prismatic"` | 滑动 |
| `<parent>` / `<child>` | 父、子 link |
| `<origin xyz>` | 子相对父的安装位姿 |
| `<axis xyz>` | 旋转/滑动轴方向 |

#### 入门示例 `first_robot.urdf` 结构

```
map/机体概念：
  base_link（白色圆柱）
      │ imu_joint (fixed, z=+0.03)
      ↓
  imu_link（黑色小方块）
```

校验 URDF：

```shell
cd ~/ros2-learning/chapt6/chapt6_ws/src
check_urdf fishbot_description/urdf/first_robot.urdf
urdf_to_graphviz fishbot_description/urdf/first_robot.urdf first_robot
```

### 6.3 xacro：宏与参数化

xacro 是带宏的 URDF。写的是模板，运行前用 `xacro` 展开成标准 URDF。

#### 必备声明

```xml
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
```

`xmlns:xacro=...` 声明后才能写 `xacro:macro`、`xacro:include` 等。

#### 宏 ≈ C 的函数

```xml
<!-- 定义 -->
<xacro:macro name="base_xacro" params="length radius">
  <cylinder length="${length}" radius="${radius}" />
</xacro:macro>

<!-- 调用 -->
<xacro:base_xacro length="0.12" radius="0.1" />
```

| xacro | 可类比 |
|-------|--------|
| `xacro:macro` | 函数定义 |
| `params="a b"` | 形参 |
| `${a}` | 使用形参 / 字符串拼接 |
| `<xacro:宏名 ... />` | 函数调用 |

查看展开结果：

```shell
xacro first_robot.urdf.xacro
# 或
xacro $(ros2 pkg prefix fishbot_description)/share/fishbot_description/urdf/fishbot/fishbot.urdf.xacro
```

### 6.4 模块化搭建 fishbot

思路：**每个部件一个 xacro 文件（只定义宏）→ 总装文件 include 并调用**。

#### 6.4.1 机体 `base.urdf.xacro`

- 宏：`base_xacro`，参数 `length`（圆柱高）、`radius`（半径）
- 虚拟部件 `base_footprint`：贴地参考坐标系（无外观）
- `base_joint`：`base_footprint → base_link`，Z 偏移约为  
  `${length/2.0+0.032-0.001}`（半高 + 轮半径，让轮子接近着地）
- 含 `visual` / `collision` / `cylinder_inertia`

#### 6.4.2 传感器

| 文件 | 宏 | 主要参数 | 外观 |
|------|-----|----------|------|
| `imu.urdf.xacro` | `imu_xacro` | `xyz` 安装位置 | 2cm 黑立方体 |
| `camera.urdf.xacro` | `camera_xacro` | `xyz` | `0.02×0.10×0.02` 绿长方体 |
| `laser.urdf.xacro` | `laser_xacro` | `xyz` | 支撑杆 + 雷达圆柱（两级关节） |

雷达 TF 链：

```
base_link → laser_cylinder_link → laser_link
```

#### 6.4.3 执行器（轮子）

| 文件 | 宏 | 关节类型 | 说明 |
|------|-----|----------|------|
| `wheel.urdf.xacro` | `wheel_xacro` | `continuous` | `wheel_name`+`xyz`；圆柱 `rpy≈π/2` 侧立；绕 Y 轴转 |
| `caster.urdf.xacro` | `caster_xacro` | `fixed` | 球体万向轮，`caster_name`+`xyz` |

#### 6.4.4 惯量库 `common_inertia.xacro`

| 宏 | 参数 | 用途 |
|----|------|------|
| `box_inertia` | `m w h d` | 长方体 |
| `cylinder_inertia` | `m r h` | 圆柱 |
| `sphere_inertia` | `m r` | 球体 |

在 `base.urdf.xacro` 中 `include` 一次即可（总装先 include base），其它部件直接调用宏，避免重复定义。

#### 6.4.5 总装 `fishbot.urdf.xacro`

两步：

1. **`xacro:include`**：引入宏定义（还不生成部件）  
2. **`xacro:xxx_xacro ...`**：传入参数，真正展开

当前调用示例：

```xml
<xacro:base_xacro length="0.12" radius="0.1" />
<xacro:imu_xacro xyz="0 0 0.02" />
<xacro:laser_xacro xyz="0 0 0.10" />
<xacro:camera_xacro xyz="0.10 0 0.075" />
<xacro:wheel_xacro wheel_name="left" xyz="0 0.10 -0.06" />
<xacro:wheel_xacro wheel_name="right" xyz="0 -0.10 -0.06" />
<xacro:caster_xacro caster_name="front" xyz="0.08 0.0 -0.076" />
<xacro:caster_xacro caster_name="back" xyz="-0.08 0.0 -0.076" />
<xacro:gazebo_control_plugin />
```

关节依赖（父子关系写在各模块内部）：

```
base_footprint
  └── base_link
        ├── imu_link
        ├── camera_link
        ├── laser_cylinder_link → laser_link
        ├── left_wheel_link / right_wheel_link   (continuous)
        └── front_caster_link / back_caster_link (fixed)
```

> **注意**：传感器/轮子宏里写死了 `parent link="base_link"`，因此总装里必须**先调用** `base_xacro`。

### 6.5 RViz 显示（display_robot.launch.py）

#### Launch 关键逻辑

1. `get_package_share_directory('fishbot_description')` 找安装路径  
2. `DeclareLaunchArgument('model')` 允许命令行换模型  
3. `Command(['xacro ', LaunchConfiguration('model')])` 展开 xacro → `robot_description`  
4. 启动 `robot_state_publisher`、`joint_state_publisher`、`rviz2`

> 早期用 `cat` 只能读纯 URDF；改成 `xacro` 才能展开宏。

#### 运行

```shell
cd ~/ros2-learning/chapt6/chapt6_ws
colcon build --packages-select fishbot_description
source install/setup.bash
ros2 launch fishbot_description display_robot.launch.py
```

RViz 设置：

- **Fixed Frame**：`base_link` 或 `base_footprint`  
- **Add → RobotModel**（Description Topic 填 `/robot_description`）  
- 可选 **Add → TF**  
- 相机距离不要太远（模型约 0.2 m，距离 10 m 时几乎看不见）

#### joint_state_publisher 注意

`joint_state_publisher` 也需要传入 `robot_description`，否则会一直 Waiting。驱动轮是 `continuous` 关节，需要关节状态才能正确发布轮子 TF。

### 6.6 Gazebo 仿真扩展

#### 6.6.1 准备模型库与依赖

```shell
git clone https://gitee.com/ohhuo/gazebo_models.git ~/.gazebo/models
rm -rf ~/.gazebo/models/.git   # 否则 Gazebo 会把 .git 当成模型目录报错
sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs
```

#### 6.6.2 `<gazebo>` 标签（仅仿真生效）

| 位置 | 作用 |
|------|------|
| `laser.urdf.xacro` | `Gazebo/Black` 改雷达颜色 |
| `wheel.urdf.xacro` | `mu1/mu2=20` 高摩擦抓地；`kp`/`kd` 接触刚度阻尼 |
| `caster.urdf.xacro` | `mu1/mu2=0` 可滑动 |

#### 6.6.3 差速驱动插件 `gazebo_control_plugin.xacro`

- 插件：`libgazebo_ros_diff_drive.so`  
- 订阅 `/cmd_vel`，发布 `/odom` 与 `odom → base_footprint` TF  
- 关节：`left_wheel_joint` / `right_wheel_joint`  
- 几何：`wheel_separation=0.2`，`wheel_diameter=0.064`  
- 另含 `libgazebo_ros_joint_state_publisher.so`，把左右轮状态发到 `/joint_states`，供 `robot_state_publisher` 计算轮子 TF  

总装中：

```xml
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_control_plugin.xacro" />
<xacro:gazebo_control_plugin />
```

#### 6.6.4 仿真 Launch `gazebo_sim.launch.py`

流程：

1. xacro 展开 → `robot_description`  
2. `robot_state_publisher`（建议 `use_sim_time:=True`）  
3. `IncludeLaunchDescription` 启动 `gazebo_ros/gazebo.launch.py`，加载 `custom_room.world`  
4. `spawn_entity.py` 从 `/robot_description` 生成实体 `fishbot`

```shell
cd ~/ros2-learning/chapt6/chapt6_ws
colcon build --packages-select fishbot_description
source install/setup.bash
ros2 launch fishbot_description gazebo_sim.launch.py
```

键盘遥控（另开终端）：

```shell
source install/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```

### 6.7 仿真时如何配合 RViz（重要）

**不要**同时再开 `display_robot.launch.py`。

| Launch | 用途 |
|--------|------|
| `display_robot.launch.py` | 只看模型（墙钟时间，自带 RSP + JSP + RViz） |
| `gazebo_sim.launch.py` | 仿真（sim time，Gazebo 发 odom/关节） |

两边一起开会有 **两套 robot_state_publisher**、时间不同步，TF/模型容易异常。

仿真时单独开 RViz：

```shell
ros2 run rviz2 rviz2 --ros-args -p use_sim_time:=true
```

设置：

- Fixed Frame：`odom`  
- RobotModel → Description Topic：`/robot_description`（若为空需手动填写）  
- 建议 Add → TF；Views 的 Target Frame 可设为 `base_footprint` 让相机跟着车  

#### 常见现象与原因

| 现象 | 常见原因 |
|------|----------|
| 能看见 TF，看不见模型 | Description Topic 未设为 `/robot_description`；或相机没对准车 |
| 轮子“钻地底”/轮子 link 报 No transform | continuous 关节缺 `/joint_states`，轮子 TF 缺失 |
| RobotModel Ok 但视野空 | 车掉出地面/飞走，相机仍盯着原点；Target Frame 改为 `base_footprint` |
| `Missing model.config for .../.git` | 未删除 `~/.gazebo/models/.git` |
| URDF 解析失败 `Could not find robot` | XML 注释嵌套或注释内含 `-->` 弄坏了结构 |

### 6.8 一键命令小结

```shell
# 编译
cd ~/ros2-learning/chapt6/chapt6_ws
colcon build --packages-select fishbot_description
source install/setup.bash

# 仅 RViz 看模型
ros2 launch fishbot_description display_robot.launch.py

# Gazebo 仿真
killall gzserver gzclient 2>/dev/null
ros2 launch fishbot_description gazebo_sim.launch.py

# 仿真旁路 RViz
ros2 run rviz2 rviz2 --ros-args -p use_sim_time:=true

# 遥控
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```