前言：苯人已经在vscode中配置好WSL开发环境，后续程序都均在基于 WSL 的 Ubuntu 子系统中运行

ROS简介

ROS包含大量搭建机器人所需要的软件和工具，是目前应用最为广泛的机器人操作系统

ROS的核心是通信，有四大核心通信机制：

话题Topic：基于发布-订阅模式，运行节点之间单向、异步交换数据，发布者只发不接收，订阅者只收不回应，适合高频、实时、无反馈要求的数据流传输

服务Service：双向同步通信方式，客户端发送请求，服务端处理并返回响应。适合低频、有明确结果要求的一次性操作

参数Parameter：用于机器人参数的设置、读取与动态修改，是 ROS 中统一的配置管理机制

动作Action：带反馈、可中断、异步的复杂逻辑控制，可根据实时反馈做过程处理，最终返回结果。专门解决 Service 无法处理的长时间复杂任务

启动海龟模拟器

相当于学习编程语言时编写“Hello world”

打开终端，输入如下代码，输入部分首字母后可按Tab键自动补全

ros2 run turtlesim turtlesim_node
       // 功能包名字  可执行文件的名字

下图的蓝色窗口就是小海龟模拟器了

使用键盘控制小海龟

需要在键盘控制窗口输入指令，由键盘窗口向程序发送数据，实现对海龟模拟器窗口的控制

打开一个新的终端，输入如下命令，打开按键控制

ros2 run turtlesim turtle_teleop_key

回车就可以进行控制了，注意，光标要放在该终端处，不是点海龟窗口。可能会遮挡什么的，自己调整合适窗口就可以了

rqt

rqt是ROS官方推出的可视化图形化工具框架（名字源于 “ROS Qt”，基于Qt框架开发），可以将ROS开发/运行中的各类调试、监控、配置操作从命令行迁移到可视化界面，是 ROS 开发中高频必备的调试工具

再打开一个终端，输入如下命令

rqt

点击插件“Plugins”，选择“Introspection”->"Node Graph"

界面如下，可使用鼠标滚轮放大

也可点击工具栏的“Fit”，调整视图

可以看得“teleop_turtle”（海龟键盘控制节点）和“turtlesim_node”（小海龟模拟器节点）两个节点

teleop_turtle发布了“/turtle1/cmd_vel”话题，并通过该话题向turtlesim_node节点发送速度控制数据（cmd_vel）

/turtle1/cmd_vel是ROS2 turtlesim的默认速度控制话题，turtle1是话题的命名空间 （用于区分多只海龟，生成新海龟后会有/turtle2/cmd_vel），cmd_vel是话题的核心标识，代表 “速度指令”，是ROS 中机器人速度控制的通用话题名。

整个流程就是：teleop_turtle通过键盘获取你的操作，转换成线速度/角速度数据，通过/turtle1/cmd_vel话题发布，turtlesim_node持续订阅该话题，获取数据后控制小海龟移动

节点

节点是ROS 2的最小运行单元，由C++/Python等语言编写（后面的代码只展示Python），是ROS 2系统的基本组成单元和核心执行载体

职责单一：一个节点只做一件核心事

松耦合通信：节点间不直接交互，通过话题、服务、动作交换数据/调用功能

可独立启停/替换：单个节点的启动、关闭、修改，不会影响其他正常运行的节点

创建节点

rclpy是ROS 2官方提供的Python 语言核心客户端库，也是Python开发ROS2程序的唯一基础入口

注意，不能直接点击“运行”，要在终端指示明确“python3 文件夹名/Node.py”

# 导入核心依赖库
import rclpy
from rclpy.node import Node

def main():
  rclpy.init()       # 初始化rclpy库
  node=Node("node")  # 创建ROS2节点，指定唯一节点名
  rclpy.spin(node)   # 启动节点的运行循环（程序会停在这一行，直到节点被手动终止），同时持续监听ROS2的各类事件
  rclpy.shutdown()   # 关闭rclpy，释放ROS2相关资源

if __name__=="__main__":
  main()

运行后是没有输出的，会卡在.spin()，ctrl+C手动终止

日志打印

ROS2节点的标准化日志输出方式

先通过节点对象获取ROS2的日志器（logger），专门用于节点的日志输出：node.get_logger()

然后指定日志级别为INFO（普通信息级别），还有warn(警告)、error(错误)等：.info()

括号里面填写日志的具体内容

import rclpy
from rclpy.node import Node

def main():
  rclpy.init()
  node=Node("python_node")
  node.get_logger().info('你好，Python节点')
  rclpy.spin(node)
  rclpy.shutdown()

if __name__=="__main__":
  main()

可以新建一个终端，通过“ros2 node list”查看当前运行节点

使用功能包组织节点

功能包是ROS2官方定义的标准化项目管理容器，专门用来组织、存放ROS2开发的所有相关文件。简单说，功能包是ROS2的项目文件夹。

节点的运行、编译依赖功能包的配置文件（声明依赖库、编译规则、节点入口），脱离功能包的节点代码，ROS2无法识别、无法通过colcon build编译、也无法用ros2 run启动

ROS2的核心启动命令ros2 run 功能包名 可执行文件，本质就是从指定功能包中找到并运行对应的节点可执行文件

ROS2项目文件结构

~/ros2/                # 你的ROS2工作空间（根目录，可自定义命名）
└── src/               # 工作空间核心目录，所有功能包必须放在src下
    └── my_py_pkg/     # 功能包（自定义命名）
        ├── my_py_pkg/ # 节点源码目录（与功能包同名）
        │   └── node.py# 你的Python节点代码
        ├── package.xml# 功能包核心配置文件，声明包名、版本、依赖、作者等
        └── setup.py   # Python功能包编译配置文，声明节点可执行名、入口、安装路径等

功能包创建步骤

cd ~/ros2  # 进入工作空间
mkdir -p src  # 创建src目录，-p表示若已存在则不报错
cd src  # 必须在src下创建功能包
ros2 pkg create --build-type ament_python --license Apache-2.0 my_py_pkg  # 创建Python功能包，指定包名、语言、许可证

''' 也可直接指定依赖，但我想后面在文件中编辑
ros2 pkg create --build-type ament_python --dependencies rclpy my_py_pkg  
'''

执行完上述命令后，my_py_pkg功能包目录的结构如下图。my_py_pkg子目录是Python 节点源码的专属存放目录（和功能包同名）；resource子目录是ROS2功能包的元数据目录，存放包的标识信息，无需手动修改，由系统自动维护；test子目录是测试代码存放目录，用于编写单元测试或集成测试，新手阶段可以暂时忽略；LICENSE文件是功能包的开源许可证文件；package.xml文件是功能包的核心元信息与依赖声明文件，包含包名、版本、作者、依赖库等，ROS2系统通过它识别功能包的基本信息和依赖关系；setup.cfg文件是辅助配置文件，主要用于配置Python包的安装路径和脚本入口，一般无需手动修改；setup.py文件是Python功能包的编译与安装配置文件，核心作用是声明节点的可执行名、入口函数、安装路径等，是让你的 Python 节点能被ros2 run启动的关键配置文件。

我们在my_py_pkg子目录中存放我们前面写的节点代码，新建文件

去掉if main的部分，执行colcon build编译时，ROS2会自动帮我们生成可执行文件调用这个代码

但需要去setup.py文件声明，找到entry_points里的console_scripts。第一个'python_node'是可执行文件名字，可自定义，后面是文件地址+要调用的函数名

我们的节点代码里的rclpy库还没添加，去package.xml文件中添加依赖声明

最后就可以该项目的构建功能包了，因为我的src文件夹下只有一个功能包，所以可以直接“colcon build”编译所有功能包（基础编译，修改代码后需重新编译），若有多个则“colcon build --packages-select 包名”编译指定功能包。“colcon build --symlink-install”命令更推荐，能创建源码软链接，修改Python代码无需重新编译（只适用于Python功能包构建）

但是软链接本质是指向源码的快捷方式，如果你后续移动了功能包的源码路径，或者在其他机器上复用编译后的包，软链接会失效，导致节点无法运行

规避方法：开发完成、准备发布 / 部署时，用普通的colcon build重新编译一次，生成独立的可执行文件，而非软链接

回车后的结果如下（警告是我开始的package.xml里用了过时的依赖标签，上面的代码均正确，已修改为新版的<depend>）

colcon build会在当前目录下生成build（编译构建目录，编译过程的临时工作目录）、install（安装部署目录，存储最终的编译产物）、log（日志记录目录）三个核心目录

install目录下的lib文件夹是ROS2功能包可执行文件、运行依赖库、脚本入口的核心存储目录，是存放功能包运行资源的核心子目录

python_node是生成的可执行文件，文件名就是我们前面在setup.py文件中定义的可执行文件名称

右键my_py_pkg文件夹，“在集成终端打开”

输入ls，列出当前目录下的文件和子目录。python_node是绿色的，表示它是可执行文件

因为我们前面是用“colcon build --symlink-install”构建的，我们修改src/里的.py文件后，无需重新编译，直接重启节点（中断正在运行的程序，重新运行）就能加载最新代码（若是使用colcon build构建的，则还需重新执行colcon build）

平时测试也可以不用功能包，直接写python测试（像开始创建节点那样）。

那为什么还要这么复杂？因为install文件是可以直接复制粘贴发送给别人用的（C++不能直接把源码发给别人吧，会泄露；python还好，它是解释型的语言）

运行可执行文件之前，还要使用source命令加载install/setup.bash脚本，配置环境变量，让功能包能被当前系统识别

然后就可以运行节点了（第一条命令是我查看一下功能包是否存在）

话题

先启动小海龟模拟器

ros2 run turtlesim turtlesim_node

再打开一个终端，使用如下命令查看节点信息

ros2 node list    # 查看当前运行的所有ROS2节点
ros2 node info /turtlesim     # 查看/turtlesim节点的完整详细信息

Subscribers是一些订阅者，前者为话题的名字（如/turtle1/cmd_vel），后者为话题对应的消息类型/消息接口（如geometry_msgs/msg/Twist）（可以通过/turtle1/cmd_vel话题控制小海龟的移动）

Publishers是发布的话题，发布了/turtle1/pose 话题，类型为 turtlesim/msg/Pose，用来向外广播海龟的实时位置和姿态……

获取指定话题的消息数据

ros2 topic echo是话题消息的 “实时监听器”，执行后终端会实时监听并打印指定话题的消息数据

获取海龟的位置信息，话题指令前缀ros2 topic + 动作echo + 目标话题名称/turtle1/pose

ros2 topic echo /turtle1/pose

会一直接收数据，可ctrl+C打断。x，y是x轴、y轴的坐标，theta表示乌龟脑袋的朝向，linear_velocity是线速度，angular_velocity是角速度

向指定话题发布数据

要先确认它的消息接口

我们之前使用键盘控制小海龟，其实也是发布/turtle1/cmd_vel话题进行控制的

ros2 topic pub是ROS2操作话题的发布指令，核心指令ros2 topic pub + 目标话题名/turtle1/cmd_vel + 消息类型geometry_msgs/msg/Twist + 消息数据

ros2 topic pub /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0} , angular: {z: -1.0}}"

又有线速度又有角速度，显然这会画一个圆

可以通过ros2 interface show命令，查看查看消息 / 服务 / 动作接口定义的标准指令

话题通信实践

任务：1.制作一个工具，通过这个工具可以看到系统的实时状态信息，包括记录信息的时间、主机名称、CPU使用率、内存使用率、内存总大小、剩余内存、网络接收数据量和网络发送数据量。2.要有一个简单的界面，可以将系统信息显示出来。3.局域网内的其它主机上也能查看数据。

（不好意思，苯人想要全用Python编程的美好幻想破灭了，还是要使用C++了，但思想应该都是一样的）

1.Python的psutils库可以获取系统状态信息。2.Qt可做界面展示。3.ROS2话题进行共享数据

所以我们用python编写系统状态信息的发布节点，C++编写订阅节点并显示，通过话题通信。每个话题都有对应的接口类型，ROS2已有的接口没有满足我们需求的，所以我们要先自定义通信接口。

自定义通信接口

接口也是在功能包里定义的（名称大家自定义，我是将之前工作空间里的内容清理了）

mkdir ros2   #创建工作空间
cd ros2      #进入工作空间
mkdir -p src #创建src目录
cd src       #进入src

创建一个纯接口功能包，不包含任何节点代码，专门用于项目中所有节点（Python/C++均可）共享数据规范，这是ROS2模块化开发的标准做法（未手动指定--build-type ament_python，会默认生成CMake 类型功能包。ROS2 的纯接口功能包，只能是 CMake 类型，因为接口生成工具rosidl_default_generators仅支持CMake构建系统）

ros2 pkg create  status_interfaces  --dependencies  builtin_interfaces  rosidl_default_generators --license Apache-2.0
#  核心指令       自定义包名          依赖参数           内置接口包          接口生成器包               证书

在status_interfaces功能包下新建一个msg文件夹

在msg文件夹下创建消息接口文件SystemStatus.msg

可以通过以下代码查找与时间相关的接口

ros2 interface list | grep Time

如果你的消息接口文件（.msg文件）依赖其它功能包下的消息接口，中间的“/msg”要去掉

定义消息格式文件

.msg文件在CMakeLists.txt文件中配置一下就能转换为代码

添加代码如下

# cmake函数，来自依赖rosidl_default_generators，用于将msg等消息接口定义文件转换成库或者头文件类
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/SystemStatus.msg"
  DEPENDENCIES builtin_interfaces
)

还要在package.xml中添加如下命令，标明这是一个包含消息接口的功能包

<member_of_group>rosidl_interface_packages</member_of_group>

回到工作空间，进行项目构建

按我图示路径可以发现SystemStatus_类，它的成员就是我们前面定义的消息接口信息，这就是自动构建的C++版本的消息接口

Python版本的在local/lib目录下（前面的是include目录）

这样我们就可以在代码中使用这个消息接口了

先更改一下环境变量

source install/setup.bash

再查看一下消息接口数据类型

ros2 interface show status_interfaces/msg/SystemStatus

系统信息获取与发布

使用Python导入消息接口，只需要从功能包下面的msg模块导入消息接口就可以了

创建一个状态发布者的功能包（在src文件夹下）

ros2 pkg create status_publisher --build-type ament_python --dependencies rclpy status_interfaces --license Apache-2.0
# 功能包创建命令     功能包名       构建类型，Python语言实现     依赖声明  RP客户端库  自定义接口包         许可证指定

新建一个代码文件

创建系统状态发布节点类

import rclpy       # 导入ros2的Python客户端库
from status_interfaces.msg import SystemStatus    # 从消息接口包中导入SystemStatus消息类型
from rclpy.node import Node    # 导入Node类以创建ROS2节点
import psutil      # 导入psutil库以获取系统状态信息
import platform    # 导入platform库以获取系统平台信息

class SysStatusPub(Node):
    def __init__(self, node_name):
        super().__init__(node_name)           # 父类初始化
        self.status_publisher_ = self.create_publisher(SystemStatus, 'system_status', 10)    # 创建话题发布者(消息类型, 话题名, 队列大小)
        self.timer=self.create_timer(1.0, self.timer_callback)    # 创建定时器(定时周期, 回调函数)

    def timer_callback(self):
        # 采集系统硬件状态
        cpu_percent = psutil.cpu_percent()    
        memory_info = psutil.virtual_memory()   
        net_io_counters = psutil.net_io_counters()    

        # 创建SystemStatus消息实例并填充数据
        msg = SystemStatus()    
        msg.stamp = self.get_clock().now().to_msg()   
        msg.host_name = platform.node()    
        msg.cpu_percent = cpu_percent   
        msg.memory_percent = memory_info.percent   
        msg.memory_total = memory_info.total   
        msg.memory_available = memory_info.available    
        msg.net_sent = net_io_counters.bytes_sent/1024/1024    # 获取发送数据总量（MB）
        msg.net_recv = net_io_counters.bytes_recv/1024/1024    # 获取接收数据总量（MB）

        self.get_logger().info(f'发布：{str(msg)}')    # 日志输出
        self.status_publisher_.publish(msg)    # 发布消息

def main():        
    rclpy.init()        # 初始化ROS2客户端库
    node = SysStatusPub('sys_status_pub')    # 创建SysStatusPub节点实例
    rclpy.spin(node)    # 进入节点的事件循环
    rclpy.shutdown()    # 关闭ROS2客户端库

在setup.py中把它配置成可执行文件

现在可以回到状态空间目录（一定要记得啊，我开始就错误地在src下构建了）进行colcon build构建了

然后再使用source命令配置一下环境变量

然后就可以运行了（下面就是发布的信息日志，红色是我打断了）

我们也可以再开一个终端，通过ros2 topic echo实时订阅并打印该话题发布的消息内容

但注意，新的终端也要通过source命令配置环境变量

话题名在SysStatusPub节点代码里，创建发布者里面定义的，可通过topic list查看

订阅数据并用Qt显示

完成界面显示

还是要在src目录下新建一个功能包（Cmake类型）

ros2 pkg create status_display --dependencies rclcpp status_interfaces --license Apache-2.0

在功能包的src目录下新建一个系统状态显示的代码文件

// Qt图形界面核心库
#include <QApplication>
#include <QLabel>
#include <QString>
// ROS2C++核心库
#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") 
    {
        label_ = new QLabel();
        // 创建ROS2订阅器（订阅话题名，队列大小，回调函数）
        subscriber_ = this->create_subscription<SystemStatus>("system_status", 10, [&]
        (const SystemStatus::SharedPtr msg)->void{
            label_->setText(get_qstr_from_msg(msg));
        });
        label_->setText(get_qstr_from_msg(std::make_shared<SystemStatus>()));
        label_->show();
    }

    QString get_qstr_from_msg(const SystemStatus::SharedPtr msg)
    {
        std::stringstream ss;
        ss<<
        "=========系统状态可视化显示工具=========\n"<<
        "数据时间：\t"<<msg->stamp.sec<<"\ts\n"<<
        "主机名字：\t"<<msg->host_name<<"\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(ss.str());
    }

private:
    rclcpp::Subscription<SystemStatus>::SharedPtr subscriber_;
    QLabel* label_;
};
int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    QApplication app(argc, argv);
    auto node = std::make_shared<SysStatusDisplay>();
    // ROS2的spin（事件循环）和Qt的exec（应用循环）均为阻塞式运行，使用多线程
    std::thread spin_thread([&](){
        rclcpp::spin(node);
    });
    spin_thread.detach();  // 子线程分离
    app.exec();
    rclcpp::shutdown();
    return 0;
}

在CMakeLists.txt中添加编译配置，target_link_libraries()是CMake库链接的核心命令，ament_target_dependencies是ROS2库依赖的命令

find_package(Qt5 REQUIRED COMPONENTS Widgets)  #定位Qt5的开发库

add_executable(sys_status_display src/sys_status_display.cpp)  #添加可执行文件（生成的可执行文件名称，源文件路径）
target_link_libraries(sys_status_display Qt5::Widgets)         #添加Widgets依赖库
ament_target_dependencies(sys_status_display rclcpp status_interfaces)    #添加ROS2相关依赖

install(TARGETS sys_status_display DESTINATION lib/${PROJECT_NAME})   #安装可执行文件

然后就可以回到工作空间使用colcon build构建了

配置环境变量后就可以run了

噢字体不能显示，需要安装Ubuntu 官方中文字体包

sudo apt update && sudo apt install fonts-wqy-microhei -y  #安装中文包
sudo update-locale LANG=en_US.UTF-8 LANGUAGE=en_US:en && source /etc/default/locale  #配置系统默认UTF-8编码

然后就能正常显示了，没有数据是因为我们没有发布

现在开启两个终端（记得环境配置），一个发布，一个接收展示

完成

服务与参数

它们都是话题的变种

服务

我们先打开一个小海龟模拟器，再打开一个终端通过ros2 service list -t命令查看一下当前系统中所有可用服务及对应的服务类型（接口类型）

使用ros2 interface show命令看一下服务的接口

“---”上面部分是请求部分，下面是返回部分。

• 客户端调用服务时，必须按上半部分请求结构填充数据并发送；
• 服务端处理请求后，会按下半部分响应结构封装结果并返回。

我们可以使用ros2 service call命令是在终端调用该服务（完整格式 ros2 service call 服务名称 服务接口类型 请求数据）

ros2 service call /spawn turtlesim/srv/Spawn "{x: 1, y: 1}"    #theta没写默认为0

可以看到，在（1，1）位置又多了一只小海龟

也可以使用可视化工具rqt进行请求

选择/spawn服务

Request是请求部分，可以填写数据

填写完后点击右上角的“Call”

可以看到在（3，1）位置多了一只小海龟，并且有响应值返回（它的名字是turtle3）

基于服务的参数通信

参数是节点的“配置项”，节点对参数的所有操作，底层都是通过服务通信来完成的

还是打开一个小海龟模拟器，然后查看一下参数相关的服务列表

直接使用服务的方式调用这些接口去获取参数、设置参数比较麻烦，要写一大堆代码，所以ROS2中有一套参数相关的工具和库供我们使用

通过ros2 param list命令可以查看当前系统所有可用参数列表，然后可以使用“ros2 param describe+节点+参数”命令查看参数的描述

通过“ros2 param get+节点+参数”命令就可用获得当前参数的值

通过“ros2 param set+节点名+参数名”就可以修改参数的值

如果有多个参数需要配置，可使用文件来配置参数

首先需要将某个节点的配置文件导出为一个yaml格式的文件

ros2 param dump /turtlesim > turtlesim_param.yaml

然后可以直接打开，也可以使用cat命令查看文件

将参数配置保存后，下次运行节点时可以直接调用这个参数应用到节点上

将之前的小海龟模拟器关闭，使用如下命令调用参数并启动一个海龟模拟器

ros2 run turtlesim turtlesim_node --ros-args --params-file turtlesim_param.yaml

可以看到，我们启动的海归模拟器就是粉色背景的

rqt里面也有和参数相关的一些工具

选中turtlesim

就可以进行动态参数配置

服务通信实践

需求：创建一个人脸检测服务，提供图像，返回人脸数量、位置信息

1.python中的yolo、face_rcoginition库都可以用来人脸识别。2.一方提供图像，一方返回相应，那就是服务通信。3.没有合适消息接口，自定义消息接口

自定义服务接口

服务接口分为请求部分和相应部分，中间用“---”分隔

还是先创建一个工作空间，创建src文件夹，在src文件夹中创建一个消息接口功能包（CMake类型）

ros2 pkg create interfaces --dependencies sensor_msgs rosidl_default_generators --license Apache-2.0

在功能包下新建一个srv文件夹（ROS2用固定的文件夹名称区分不同类型的通信数据接口，msg文件夹专门存放话题使用的消息接口文件，srv文件夹专门存放服务使用的消息接口文件）

在srv文件夹中新建一个FaceDetector.srv文件

定义消息接口

sensor_msgs/Image image      # 人脸图像
---
int16 number
float32 use_time
int32[] top
int32[] right
int32[] bottom
int32[] left

然后再配置一下CMakeLists.txt，添加消息接口

还要在package.xml中声明该消息接口功能包（第11行）

然后回到工作空间colcon build构建

使用Python进行人脸检测

我们使用python的face_recoginition库进行人脸检测，先安装一下该库

pip install face_recognition -i https://pypi.tuna.tsinghua.edu.cn/simple --trusted-host pypi.tuna.tsinghua.edu.cn

然后在src文件夹下创建一个人脸检测的功能包

ros2 pkg create python_service --build-type ament_python --dependencies pclpy interfaces --license Apache-2.0

然后准备一张人脸图片（不能是.webp格式），放到功能包的resource目录下

colcon build构建时仅会自动处理源码和setup.py中data_files字段声明的文件/目录，所以我们要在setup.py中配置该照片资源（把resource目录下的faces.webp文件拷贝到install目录下的share下的package_name下的resource目录中）

然后现在来编写人脸识别的代码，在功能包的python_service目录下创建learn_face_detect.py文件

import face_recognition
import cv2
from ament_index_python.packages import get_package_share_directory   #获取功能包share目录绝对路径

def main():
    # 获取图片的真实路径
    default_image_path = get_package_share_directory('python_service') + '/resource/faces.jpg'
    # 使用cv2加载图片
    image = cv2.imread(default_image_path)
    # 使用face_recognition检测人脸位置
    face_loctions = face_recognition.face_locations(image, number_of_times_to_upsample=1, model="hog")
    # 在图片上绘制人脸位置矩形框
    for top, right, bottom, left in face_loctions:
        cv2.rectangle(image, (left, top), (right, bottom), (255, 0, 0), 4)
    # 显示图片
    cv2.imshow("Face Detection", image)
    cv2.waitKey(0)

然后在setup.py里声明可执行文件

在工作空间使用colcon build构建

然后配置一下环境变量，运行可执行文件

能正常显示图片，但是没检测出来（这是模型的问题了，苯人现在只注重项目流程，就不重新找图片什么的了）

人脸检测服务实现

ROS2里的图片格式(sensor_msgs里的Image格式)和OpenCV里的图片格式不一样，我们需要使用CV BRIGE库进行转换。

编写服务端节点，接收请求Request，调用face_recoginition识别，处理识别结果合成Response返回

在python_service功能包的python_service目录下新建face_detect_node.py

import rclpy
from rclpy.node import Node
from interfaces.srv import FaceDetector
import face_recognition
import cv2
from ament_index_python.packages import get_package_share_directory
from cv_bridge import CvBridge  
import time

class FaceDetectNode(Node):
    def __init__(self):
        super().__init__('face_detect_node')     # 初始化父类
        self.services_ = self.create_service(FaceDetector, 'face_detect', self.detect_face_callback)   # 创建ROS2服务端（服务接口类型，服务名，回调函数）
        self.bridge = CvBridge()
        self.number_of_times_to_upsample = 1
        self.model = "hog"
        self.default_image_path = get_package_share_directory('python_service') + '/resource/faces.jpg'

    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)
            self.get_logger().info('No image data received, using default image.')

        start_time = time.time()
        self.get_logger().info('Starting face detection...')
        face_locations = face_recognition.face_locations(cv_image, number_of_times_to_upsample=self.number_of_times_to_upsample, model=self.model)
        # 构造响应数据，返回给客户端
        response.use_time = time.time() - start_time
        response.number = len(face_locations)
        for top, right, bottom, left in face_locations:
            response.top.append(top)
            response.right.append(right)
            response.bottom.append(bottom)
            response.left.append(left)

        return response    # 必须返回response

def main():
    rclpy.init()
    node = FaceDetectNode()
    rclpy.spin(node)
    rclpy.shutdown()

在setup.py中配置可执行文件

然后在工作空间colcon build构建，配置环境，运行服务

打开一个新的终端，配置环境，使用“ros2 service call 服务名 接口类型 请求数据”命令调用该服务

ros2 service call /face_detect interfaces/srv/FaceDetector "{}"  #请求数据为空加载默认图片

因为那张图片检测不出来，所以都为0

人脸检测客户端实现

我换了一张默认人脸图片，不然还是太不爽了（这个可以检测到）。然后还需要准备一张图片，作为客户端发送的请求图片

                       

文件结构如下图

然后在setup.py中配置这张图片，直接在拷贝列表里添加一下就行了

在python_service功能包的python_service代码文件夹下新建一个python_detect_client_node.py

import rclpy
from rclpy.node import Node
from interfaces.srv import FaceDetector
import cv2
from ament_index_python.packages import get_package_share_directory
from cv_bridge import CvBridge   # ROS2和OpenCV的图片格式转换工具
import time

class FaceDetectClientNode(Node):
    def __init__(self):
        super().__init__('face_detect_client_node')
        self.bridge = CvBridge()
        self.default_image_path = get_package_share_directory('python_service') + '/resource/new.png'
        self.get_logger().info('人脸检测客户端已启动！')
        self.client = self.create_client(FaceDetector,'face_detect')   #创建客户端，绑定名为face_detect的服务
        self.image=cv2.imread(self.default_image_path)

    def send_request(self):
        # 判断服务端是否在线
        while self.client.wait_for_service(timeout_sec=1.0) is False:
            self.get_logger().info('等待服务端上线...')
        # 构造Request
        request = FaceDetector.Request()
        request.image = self.bridge.cv2_to_imgmsg(self.image)
        #发送请求并等待处理完成
        future = self.client.call_async(request)  
        rclpy.spin_until_future_complete(self,future)  # 在后台边检查是否有返回边接收返回
        # 接收响应
        response = future.result()
        self.get_logger().info(f'接收到响应，共检测到{response.number}张人脸，耗时{response.use_time}s')
        self.show_reponse(response)

    def show_reponse(self,response):
        for i in range(response.number):
            top = response.top[i]
            right = response.right[i]
            bottom = response.bottom[i]
            left = response.left[i]
            cv2.rectangle(self.image,(left,top),(right,bottom),(255,0,0),4)
        cv2.imshow("人脸检测结果", self.image)
        cv2.waitKey(0)
        
def main():
    rclpy.init()
    node = FaceDetectClientNode()
    node.send_request()   # 调用发送请求函数
    rclpy.spin(node)
    rclpy.shutdown()

在setup.py中配置可执行文件

然后构建一下，source一下，先启动一下服务端

ros2 run python_service face_detect_node

再打开一个终端，运行客户端，真的成功检测出来了！

参数实现

参数的声明与获取

# 参数的设置
self.declare_parameter('number_of_times_to_upsample',1)  # 名字，值
self.declare_parameter('model','hog')
# 参数的获取
self.number_of_times_to_upsampleself.get_parameter('number_of_times_to_upsample').value
self.model = self.get_parameter('mddel').value

订阅参数更新事件

参数更新是运行了setParameter服务，所以可以在服务通信的回调函数中设置参数回调

人脸检测服务改进代码如下

import rclpy
from rclpy.node import Node
from interfaces.srv import FaceDetector
import face_recognition
import cv2
from ament_index_python.packages import get_package_share_directory
from cv_bridge import CvBridge  
import time
from rcl_interfaces.msg import SetParametersResult

class FaceDetectNode(Node):
    def __init__(self):
        super().__init__('face_detect_node')     # 初始化父类
        self.services_ = self.create_service(FaceDetector, 'face_detect', self.detect_face_callback)   # 创建ROS2服务端（服务接口类型，服务名，回调函数）
        self.bridge = CvBridge()
        # 参数的设置
        self.declare_parameter('number_of_times_to_upsample',1)  # 名字，值
        self.declare_parameter('model','hog')
        # 参数的获取
        self.number_of_times_to_upsample = self.get_parameter('number_of_times_to_upsample').value
        self.model = self.get_parameter('model').value
        self.default_image_path = get_package_share_directory('python_service') + '/resource/faces.jpg'
        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 == 'number_of_times_to_upsample':
                self.number_of_times_to_upsample = parameter.value
            if parameter.name == 'model':
                self.model = parameter.value
        return SetParametersResult(successful = True)
    
    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)
            self.get_logger().info('No image data received, using default image.')

        start_time = time.time()
        self.get_logger().info('Starting face detection...')
        face_locations = face_recognition.face_locations(cv_image, number_of_times_to_upsample=self.number_of_times_to_upsample, model=self.model)
        # 构造响应数据，返回给客户端
        response.use_time = time.time() - start_time
        response.number = len(face_locations)
        for top, right, bottom, left in face_locations:
            response.top.append(top)
            response.right.append(right)
            response.bottom.append(bottom)
            response.left.append(left)

        return response    # 必须返回response

def main():
    rclpy.init()
    node = FaceDetectNode()
    rclpy.spin(node)
    rclpy.shutdown()

构建+配置，运行该服务

ros2 run python_service face_detect_node

再打开另一个终端，设置一下参数

ros2 param set /face_detect_node model cnn

可以看到左侧底部的消息提醒

使用launch启动脚本

使用lauch启动多节点（之前需要打开多个终端）

在功能包下新建一个launch文件夹，并在里面创建一个.launch.py文件

            

import launch
import launch_ros

# 产生launch描述（函数名固定）
def generate_launch_description():
    action_node_face_detect_node = launch_ros.actions.Node(
        package='python_service',
        executable='face_detect_node',
        output='log'         # 输出日志
    )
    action_node_face_detect_client_node = launch_ros.actions.Node(
        package='python_service',
        executable='face_detect_client_node',
        output='both'         # 两者都输出
    )
    return launch.LaunchDescription([
        action_node_face_detect_node,
        action_node_face_detect_client_node,
    ])

在setup.py中配置一下，先导入glob库，添加路径（第15行）

在构建，配置环境，再launch启动

ros2 launch python_service demo.launch.py

使用launch传递参数

import launch
import launch_ros

# 产生launch描述（函数名固定）
def generate_launch_description():
    # 声明一个launch参数
    action_declare_arg_background_g = launch.actions.DeclareLaunchArgument('launch_arg_bg',default_value='150')
    # 把launch参数手动传递给某个节点
    action_node_face_detect_node = launch_ros.actions.Node(
        package='python_service',
        executable='face_detect_node',
        parameters=[{'background_g':launch.substitutions.LaunchConfiguration('launch_arg_bg',default='150')}],
        output='log'         
    )
    action_node_face_detect_client_node = launch_ros.actions.Node(
        package='python_service',
        executable='face_detect_client_node',
        output='both'         
    )
    return launch.LaunchDescription([
        action_declare_arg_background_g,
        action_node_face_detect_node,
        action_node_face_detect_client_node,
    ])

构建，配置，然后传参运行

 ros2 launch python_service demo.launch.py launch_arg_bg:=0
                                               # 参数名:=值

launch

Launch的三大组件是动作、条件和替换

动作：可以是节点、打印、终端指令，甚至是另一个launch文件

替换：使用launch的参数替换节点的参数值

条件：利用条件可以决定哪些动作启动