系列文章目录

---

前言

由于现有的ROS2与计算机视觉（特别是机器人视觉）教程较少，因此根据以往所学与积累的经验，对ROS2与机器人视觉相关理论与代码进行分析说明。

本文简要介绍了机器人视觉。首先介绍 ROS2 中图像发布者和订阅者的基本概念，然后应用一些基本命令介绍数字图像处理理论；最后，介绍一些 RGBD 和点云的概念和应用。

---

一、

1.1 先决条件

安装软件以及安装方法

You should have ROS2 installed.
You should have OpenCV for ROS2 installed.

1.1.1 安装

0. 安装要求

安装 git：

sudo apt-get install git

1. 安装 ROS2：

ROS2 Humble for Ubuntu 22.04：
按照这里的说明进行安装：

Installation — ROS 2 Documentation: Humble documentation

2. cv_bridge

如果已经安装了 ROS Humble，可以使用以下命令重新安装 cv_bridge：

sudo apt-get install --reinstall ros-humble-cv-bridge

1.2 获取机器人视觉库简介：

1.2.1 克隆该资源库

首先，创建一个工作区：

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

1.2.2 下载附加数据

本项目需要额外的数据文件，可在此处获取。

将这些文件夹下载到 ~/ros2_ws/src/ros2/data/rosbags/ 文件夹中。

1.2.3 编译代码

cd ~/ros2_ws
colcon build

故障排除

如果 cv_bridge 出现错误，可能需要从源代码编译。

安装依赖项

sudo apt-get update
sudo apt-get install -y python3-opencv libopencv-dev ros-humble-cv-bridge-msgs

设置工作空间

mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/ros-perception/vision_opencv.git
cd vision_opencv && git checkout humble

构建和源代码

cd ~/ros2_ws
colcon build --packages-select cv_bridge
source ~/ros2_ws/install/setup.bash

(可选）添加到 .bashrc：

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc

1.3 测试代码

在终端运行以下命令

source ~/ros2_ws/install/setup.bash
ros2 run images my_publisher ~/ros2_ws/src/ros2/data/images/baboon.png

在第二个终端中运行这些命令：

source ~/ros2_ws/install/setup.bash
ros2 run images my_subscriber

二、ROS2 中的图像发布者和订阅者

介绍如何使用 ROS2 和 OpenCV 发布和订阅图像。

2.0 CMakeList.txt 和 package.xml

在 ROS2 中构建和安装软件包时，我们需要向编译器提供一些名称和库的信息。将介绍如何使用 CMakeLists.txt 和 package.xml 文件在 ROS2 中配置和构建软件包。

2.0.1 package.xml

package.xml 文件包含软件包元数据和依赖声明。

关键部分：

• <name>： 定义软件包名称（robovision_images）。
• <version>：指定软件包版本。
• <维护者>： 提供维护者和联系电子邮件。
• <license>（许可证）： 许可证的占位符。
• <buildtool_depend>: 用于构建软件包的工具，如 ament_cmake 和 ament_cmake_python。
• <depend>： 运行时依赖项，如 rclcpp、cv_bridge、sensor_msgs 和 std_msgs。
• <test_depend>： 用于测试的依赖项，如 ament_lint_auto。

2.0.2 CMakeLists.txt

CMakeLists.txt 文件定义了软件包的构建方式。

主要组件：

• 指定 CMake 最低版本和编译器选项。
• 定位依赖项（rclcpp、cv_bridge、OpenCV 等）。
• 声明 OpenCV 和 cv_bridge 的包含目录。
• 为 my_publisher 和 my_camera_publisher 等 C++ 节点添加可执行文件，并链接它们的依赖关系。
• 将 Python 脚本和 C++ 可执行文件安装到相应目录。
• 使用 ament_package()将软件包与 ROS2 集成。

2.0.3 示例：robovision_images

2.0.3.1 package.xml

我们声明项目的名称

<name>robovision_images</name>

我们在项目中同时使用 C++ 和 Python 代码，因此需要对其进行声明：

<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>

<depend>rclcpp</depend>
<depend>rclpy</depend>

此外，我们还需要添加我们将在项目中使用的所有基于 ros 的依赖项

<depend>cv_bridge</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>

2.0.3.2 CMakeList.txt

首先，我们声明项目的名称

project(robovision_images)

同样，我们需要声明同时使用 C++ 和 Python 代码

find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)

以及我们使用的依赖关系

find_package(OpenCV REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)

要构建一个项目，我们需要指定要编译哪些文件，以及如何将它们引用到我们的软件包中。对于 C++ 文件，我们可以像下面的示例那样声明它们

add_executable(my_publisher src/my_publisher.cpp)
ament_target_dependencies(my_publisher 
    rclcpp 
    cv_bridge 
    sensor_msgs 
    OpenCV
)

而对于 Python，我们使用

ament_python_install_package(${PROJECT_NAME})

最后，我们需要将它们安装到我们的安装文件夹中，以便日后获取它们的源代码

install(TARGETS
  my_publisher
  DESTINATION lib/${PROJECT_NAME}
)
install(PROGRAMS
  scripts/my_subscriber.py
  DESTINATION lib/${PROJECT_NAME}
)

本文将简要介绍如何使用这些文件配置您的项目。请在每一节中查看这些文件！

2.1. ROS 发布者和订阅者

我们已经了解这些主题。如果没有，可以从这里开始学习 C++：

Writing a simple publisher and subscriber (C++) — ROS 2 Documentation: Humble documentation

和这里的 Python： 

Writing a simple publisher and subscriber (Python) — ROS 2 Documentation: Humble documentation

ROS2 是为面向对象编程（OOP）而设计的，因此我们将提供基于这种模式（即基于类的模板）的解决方案。

C++ 的基本结构如下：

#include "rclcpp/rclcpp.hpp"

class MyCustomNode : public rclcpp::Node // CHANGE CLASS NAME
{
public:
    MyCustomNode() : Node("node_name") // CHANGE CLASS AND NODE NAME
    {
    }

private:
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<MyCustomNode>(); // CHANGE CLASS NAME
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

 而 Python 的基本结构是

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node


class MyCustomNode(Node): # CHANGE CLASS NAME
    def __init__(self):
        super().__init__("node_name") # CHANGE NODE NAME


def main(args=None):
    rclpy.init(args=args)
    node = MyCustomNode() # CHANGE CLASS NAME
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == "__main__":
    main()

2.1.1 静态图像发布器

请看 my_publisher.cpp 文件。该文件介绍了在 ROS2 中构建发布器的基本结构。

2.1.1.1 创建发布者

主部分保持不变，我们用它来调用我们的类；不过，我们对它进行了修改，以获取一些参数。首先，我们需要在 ImagePublisherNode 类中创建一个节点对象，并为其命名：

ImagePublisherNode(const std::string & image_path) : Node("image_publisher")

然后，我们创建一个 ROS2 发布者，并给它起一个唯一的名字，将其显示为 ROS2 主题，这样其他人就可以访问它而不会混淆。结构很简单：create_publishermsg::Type(“topic/name”, 10)。在本例中，类型是 sensor_msgs::msg::Image，输出主题的名称是 camera/image，如下所示：

image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/image", 10);

现在，让我们来收集一些数据。我们将使用 OpenCV 打开一幅图像，然后将其发布到我们的主题中。首先，我们读取图像：

cv::Mat cv_image = cv::imread(image_path, cv::IMREAD_COLOR);
if (cv_image.empty()) {
    RCLCPP_ERROR(this->get_logger(), "Failed to load image from path: %s", image_path.c_str());
    return;
}

然后我们将其转换为 ROS 消息，即可以通过 ROS 框架发送的数据类型：

image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg();

现在我们可以发布这条信息了。

我们可以只发布一次（image_publisher_->publish(*image_msg_)），但这意味着信息只能在一小段时间内使用；如果你不在那一刻访问它，你将无法再使用它。所以，让我们一直发布吧！我们首先要确定帧频，即每秒帧数（fps）。一般来说，我们认为实时帧率在 30 帧/秒左右。我们使用 while 循环以 30 Hz（即 30 帧/秒）的速度发布图像。在 ROS2 中，我们需要创建一个迭代器，一旦节点开始在主要部分中旋转，它就会以给定的频率进行迭代：

image_timer_ = this->create_wall_timer(std::chrono::milliseconds(30),
				       std::bind(&ImagePublisherNode::publish_image, this));

该定时器将调用一个函数（ImagePublisherNode::publish_image），我们可以在该函数中发布图片：

void publish_image()
{
    image_publisher_->publish(*image_msg_);
}

最后，请注意我们需要定义我们的变量；特别是在 C++ 中，我们的 ROS2 变量都是共享指针，因此我们需要将它们定义为共享指针：

sensor_msgs::msg::Image::SharedPtr image_msg_;

rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
rclcpp::TimerBase::SharedPtr image_timer_;

就这样，我们有了第一个 ROS2 发布器。

2.1.1.2 测试代码

在终端运行以下命令

ros2 topic list

你应该看到类似的内容：

/parameter_events
/rosout

现在，在同一终端运行以下命令：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png

然后，在新终端中再次运行此命令：

ros2 topic list

还记得 image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(“camera/image”, 10); 这行吗？现在，我们可以看到我们的主题 /camera/image！此外，我们还可以获得它的详细信息。如果我们输入命令

ros2 topic info /camera/image

我们可以看到

Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 0

最后，输入：

ros2 node list

和

ros2 node info /image_publisher

我们看到

image_publisher
  Publishers:
    /camera/image: sensor_msgs/msg/Image

因此，我们可以看到我们的主题是 sensor_msgs/msg/Image 数据，而发布者对应于我们在上面几行定义节点类时给它起的名字 ImagePublisherNode(const std::string & image_path) ： Node(“image_publisher”). 但是，我们还没有订阅者。让我们在第 2.1.3 节中解决这个问题。

2.1.1.3 作业 1.1

在你的代码中添加一个新的发布器，发布原始图片一半的缩放版本。
为了让你了解 ROS2 是如何工作的，这次我们会帮助你完成这个任务，但你需要自己完成它。

还记得我们的 ROS2 发布者吗？我们需要为每个主题创建一个发布器，因此，让我们添加一个新的发布器（别忘了为每个主题创建一个不同且唯一的名称，在本例中，我们将其命名为 camera/scaled_image）：

scaled_image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/scaled_image", 10);

不要忘记定义：

rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr scaled_image_publisher_;

我们在互联网上搜索了一下，发现要在 OpenCV 中缩放图像，可以使用以下命令：

cv::Mat scaled_image;
cv::resize(cv_image, scaled_image, cv::Size(), 0.5, 0.5, CV_INTER_AREA);

现在，我们需要创建一条新的图像信息：

scaled_image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", scaled_image).toImageMsg();

并发布我们缩放的图像：

scaled_image_publisher_->publish(*scaled_image_msg_);

您的代码应该是这样的

#include "rclcpp/rclcpp.hpp"

#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.hpp>
#include <opencv2/opencv.hpp>

class ImagePublisherNode : public rclcpp::Node
{
public:
    ImagePublisherNode(const std::string & image_path) : Node("image_publisher")
    {
        image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/image", 10);
        scaled_image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/scaled_image", 10);

        // Load the image from file
        cv::Mat cv_image = cv::imread(image_path, cv::IMREAD_COLOR);
        if (cv_image.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to load image from path: %s", image_path.c_str());
            return;
        }

        //Resize your image
        cv::Mat scaled_image;
        cv::resize(cv_image, scaled_image, cv::Size(), 0.5, 0.5, CV_INTER_AREA);

        //Convert the output data into a ROS message format
        image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg();
        scaled_image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", scaled_image).toImageMsg();

        image_timer_ = this->create_wall_timer(std::chrono::milliseconds(30),
                                               std::bind(&ImagePublisherNode::publish_image, this));
        
        RCLCPP_INFO(this->get_logger(), "Starting image_publisher application in cpp...");
    }

private:
    
    sensor_msgs::msg::Image::SharedPtr image_msg_, scaled_image_msg_;

    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr scaled_image_publisher_;
    rclcpp::TimerBase::SharedPtr image_timer_;

    void publish_image()
    {
        image_publisher_->publish(*image_msg_);
        scaled_image_publisher_->publish(*scaled_image_msg_);
    }
};

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

    if (argc != 2) {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Usage: my_publisher <image_path>");
        rclcpp::shutdown();
        return 1;
    }

    std::string image_path = argv[1];

    auto node = std::make_shared<ImagePublisherNode>(image_path);
    rclcpp::spin(node);

    rclcpp::shutdown();
    return 0;
}

现在我们来测试一下！

首先，我们需要在一个新的终端运行中编译我们的代码：

cd ~/robovision_ros2_ws
colcon build

现在，运行以下命令：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png

最后，在新终端中运行此命令：

ros2 topic list

和

ros2 node info /image_publisher

你能看到什么？请解释一下。

2.1.2 相机发布程序

请查看 my_camera_publisher.cpp 文件。该文件介绍了在 ROS2 中打开相机和创建发布器的基本结构。你能注意到静态图像发布器和摄像机发布器之间的异同吗？

2.1.2.1 创建发布器

首先，我们像之前一样启动节点：

image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/image", 10);

请注意，静态图像和相机发布者使用相同的主题名，因此一次只能使用其中一个。

让我们打开摄像机！每台连接到电脑的摄像头都有一个 ID 号，从 0 开始。 如果您有一台笔记本电脑，上面还连接了一个 USB 摄像头，那么笔记本电脑摄像头的 ID 号为 0，USC 摄像头的 ID 号为 1，以此类推。无论如何，只要至少有一个摄像头连接到电脑，就会有一个设备的 ID 为 0，因此默认值为 0。我们可以使用硬编码的 ID 值，也可以在运行节点时将其作为动态参数接收；我们更倾向于使用后一种情况，因此我们就这样做吧。

现在，请记住我们是如何在主函数中输入图像路径的：

if (argc != 2) {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Usage: my_publisher <image_path>");
    rclcpp::shutdown();
    return 1;
}

std::string image_path = argv[1];

不过，ROS 有一个输入动态参数的内置选项，从现在起我们将使用它--这在创建启动文件时会很有用。为此，我们在类定义中为 camera_index 变量声明以下内容

// Declare and get the camera index parameter (default is 0)
this->declare_parameter<int>("camera_index", 0);
int camera_index = this->get_parameter("camera_index").as_int();

然后，以下几行应能打开一个 ID 为 camera_index 的摄像机：

// Open the camera
capture_.open(camera_index);

// Check if the camera opened successfully
if (!capture_.isOpened())
{
    RCLCPP_ERROR(this->get_logger(), "Failed to open camera");
    throw std::runtime_error("Camera not available");
}

现在，我们应该注意到，与静态图像不同，视频序列是随时间变化的，因此我们应该以所需的帧速率（取决于设备规格）更新帧。因此，我们需要在定时器回调函数（capture_ >> cv_image;）中获取新帧并发布它。一般来说，相机在调用后需要一段时间才能启动，因此我们需要等待，直到我们的程序开始接收视频流（我们使用 !capture_.isOpened() 来做到这一点）。然后，我们读取和发布摄像机图像的代码是

void publish_image()
{
    cv::Mat cv_image;
    capture_ >> cv_image; // Capture an image frame

    if (cv_image.empty())
    {
        RCLCPP_WARN(this->get_logger(), "Empty frame captured");
        retu0rn;
    }

    //Convert the output data into a ROS message format
    sensor_msgs::msg::Image::SharedPtr image_msg_;
    image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg();
    image_publisher_->publish(*image_msg_);
}

2.1.2.2 测试代码

首先，我们需要在新的终端运行中编译我们的代码：

cd ~/robovision_ros2_ws
colcon build

现在，运行下一条命令：

ros2 topic list

你应该看到这样的内容：

/parameter_events
/rosout

现在，在同一终端运行以下命令：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_images my_camera_publisher

然后，再次在新终端中运行此命令：

ros2 topic list

你能解释输出结果吗？如何从主题中获取额外信息？

如果要测试多台摄像机，可以使用 --ros-args 和参数声明 -p param_name:=param_value；在这种情况下，我们用摄像机索引设置 camera_index 变量（例如，我们将其声明为 0）：

ros2 run robovision_images my_camera_publisher --ros-args -p camera_index:=0

2.1.2.3 作业 1.2

在代码中添加一个新的发布器，发布原始视频帧一半的缩放版本。
提示 您应在定时器函数 publish_image() 之外启动新的发布器，但要在其中处理缩放图像。

2.1.3 图像订阅器

现在我们有了一个在 ROS 中发布的图像流，让我们对它们做些什么吧。请查看 my_subscriber.cpp 和 my_subscriber.py 文件。这里最重要的部分是回调函数。请再次使用之前提供的链接复习有关 ROS 发布者和订阅者的概念。

2.1.3.1 创建订阅者

C++

与 ROS 中的任何节点一样，我们需要启动它并给它起一个唯一的名字：

ImageSubscriberNode() : Node("image_subscriber")

现在，我们需要再次创建一个 ROS 订阅器。其基本结构是：create_subscription<msg::Type>（“topic/name”, queue_size, callback_function）；msg::Type 与我们要获取的主题类型相同。此外，我们还要告诉 ROS，每次有新图像到达时，将调用哪个函数。在我们的例子中，相机/图像主题的类型是 sensor_msgs::msg::Image，而我们调用的是 callback_image，我们用 std::bind 声明它（不用担心，每次订阅主题时都使用这个结构即可！）。然后我们就有了

image_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
    "camera/image", 10, std::bind(&ImageSubscriberNode::callback_image, this, std::placeholders::_1));

这很容易......但现在我们需要创建回调函数。基本结构包括函数名称和作为参数的变量，变量的数据类型将被输入。在本例中，我们的 msg 变量类型为 sensor_msgs::msg::Image::SharedPtr：

void callback_image(const sensor_msgs::msg::Image::SharedPtr msg)

在这个函数中，我们可以处理我们的数据。不过，我们更倾向于使用我们的用户回调函数来更新我们的变量，并在自定义函数（例如我们的定时器函数）中进行处理；这是因为，在 compĺex 程序中，我们可能需要同时处理来自多个主题的信息。

因此，首先让我们关注一下我们的回调函数。首先，我们需要将 msg 变量中的 sensor_msgs::msg::Image::SharedPtr 数据转换为 cv::Mat：

void callback_image(const sensor_msgs::msg::Image::SharedPtr msg)
{
    image_ = cv_bridge::toCvCopy(msg, "bgr8")->image;
    is_image_ = true;
}

然后，我们可以在我们的定时器函数 image_processing() 中使用它；在这种情况下，我们只会用 OpenCV 显示它（不要忘记使用 cv::waitKey(1); 函数来告诉 OpenCV 停止并显示图像）：
 

void image_processing()
{
    if (is_image_){
        cv::imshow("view", image_);
        cv::waitKey(1);
    }
}

Python

同样，在 Python 中，我们首先启动节点并为其命名：

super().__init__("image_subscriber")

然后，我们必须创建一个订阅器，告诉 ROS 当有新消息时我们将使用哪个函数，如下所示：

self.subscriber_ = self.create_subscription(
    Image, "camera/image", self.callback_camera_image, 10)

与 C++ 示例一样，我们可以在回调函数中处理新数据。但是，我们将使用回调函数更新我们的变量，然后在自定义函数（例如我们的定时器函数）中处理它们。因此，在 Python 中，我们只用信息的名称声明回调函数，但数据类型作为隐式值：

def callback_camera_image(self, msg)

然后，我们将 ROS 信息转换为 OpenCV 数组：

#Transform the new message to an OpenCV matrix
bridge_rgb=CvBridge()
self.img = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy()

我们让 Python 知道，我们收到了第一条信息：

self.is_img = True

最后，我们使用基本结构 create_timer(time_in_seconds,callback_function)来声明定时器：

self.processing_timer_ = self.create_timer(0.030, self.image_processing) #update image each 30 miliseconds

然后，我们可以在回调函数中处理数据；在我们的示例中，我们将显示图像：

def image_processing(self):
    if self.is_img:
        #Show your images
        if(self.display):
            cv2.imshow("view", self.img)
            cv2.waitKey(1)

正如您所看到的，我们的面向对象编程 (OOP) 类在 C++ 和 Python 中是相似的；不过，为了性能，我们更喜欢 C++，但如果不需要高性能，我们也会使用 Python 来简化。

2.1.3.2 测试代码

在终端运行以下命令编译代码：

cd ~/robovision_ros2_ws
colcon build

然后，运行下一条命令：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png

现在，在另一个终端运行以下命令：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_images my_subscriber

发生了什么？

在新终端中运行此命令：

ros2 node list

ros2 node info /image_subscriber

你能明白发生了什么吗？

/image_subscriber
  Subscribers:
    /camera/image: sensor_msgs/msg/Image

通过 ros2 节点信息，我们知道图像主题 /camera/image 正在被名为 /image_subscriber 的节点访问。请记住，我们将图像发布器命名为 create_publisher<sensor_msgs::msg::Image>(“camera/image”, 10) 并使用 create_subscription<sensor_msgs::msg::Image>(“camera/image”, 10, std::bind(&ImageSubscriberNode::callback_image, this, std::placeholders::_1) 声明订阅了 /camera/image 主题。）

2.1.3.3 作业 1.3

创建第二个订阅者，订阅 /camera/scaled_image 主题中的第二个主题（缩放输入图像）并显示它。
提示 在 C++ 中，您需要创建第二个订阅者，并为所选主题创建相应的回调：

scaled_image_subscriber_ = create_subscription<sensor_msgs::msg::Image>(
            "camera/scaled_image", 10, std::bind(&ImageSubscriberNode::callback_scaled_image, this, std::placeholders::_1));

并声明新的回调函数 callback_scaled_image：

void callback_scaled_image(const sensor_msgs::msg::Image::SharedPtr msg)

同样，在 Python 中，您需要订阅新主题：

self.scaled_image_subscriber_ = self.create_subscription(
    Image, "camera/scaled_image", self.callback_scaled_image, 10)

并定义新的回调函数 callback_scaled_image：

def callback_scaled_image(self, msg)

三、使用 OpenCV 和 ROS 进行 RGB 图像处理

目标是介绍如何使用 OpenCV 和 ROS 进行 RGB 图像处理。

3.1. 了解图像

在开始处理图像之前，我们应该先了解什么是图像以及如何访问图像数据。在这项工作中，我们将图像理解为一个二维数组或矩阵，数组中的每个元素（也称为像素）都有一个颜色值。我们对数组中的每个元素使用三个颜色通道： 红色、绿色和蓝色。如下图所示，该图像矩阵的原点位于左上角，列值从左到右正向增加，而行值从上到下正向增加：

每个颜色通道都有一个介于 0 和 255 之间的整数值。例如，RGB = [255, 0, 0] 表示红色，因为红色 = 255 是最大值，而绿色 = 0 和蓝色 = 0 表示没有这些颜色。同样，RGB = [0, 255, 0] 表示绿色，RGB = [0, 0, 255] 表示蓝色。将这三个不同强度的通道组合起来，我们就能感知到真正的颜色（例如，如果将红色和绿色的不同值组合起来，就会得到一系列黄色的色调，如 RGB = [128, 128, 0]）。

3.2. 基本操作

在前面的教程中，我们学习了如何用 C++ 订阅 ROS 图像主题：

image_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
    "camera/image", 10, std::bind(&ImageProcessingNode::callback_image, this, std::placeholders::_1));

并将 ROS 图像信息转换为 OpenCV 矩阵数组：

image_ = cv_bridge::toCvCopy(msg, "bgr8")->image;

在 Python 中，创建图像订阅：

self.subscriber_ = self.create_subscription(
    Image, "camera/image", self.callback_camera_image, 10)

并将其转换为矩阵数组：

bridge_rgb=CvBridge()
self.img = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy()

3.2.1 图像尺寸

C++

请看 my_processing.cpp 文件。让我们检查一下图像。首先，我们来看看图像的尺寸（我们添加了一个计数器索引，这样我们只在第一次迭代时打印这些值）：

if (counter_ == 0)
    std::cout << "size: rows: " << image_.rows << 
                 ", cols: " << image_.cols << 
                 ", depth: " << image_.channels() << std::endl;

让我们测试一下我们的代码。首先，我们需要编译代码：

cd ~/robovision_ros2_ws
colcon build

现在，和前面的例子一样，运行：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png

最后，在新终端中运行此命令：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_processing my_processing

在这里我们可以看到有关图像大小的信息。

Python

请看 my_processing.py 文件。同样，我们首先要确定图像的尺寸。在 Python 中，我们在矩阵中使用 shape 运算符，它对彩色图像返回三个值（行数、列数和通道数），对灰度图像返回两个值（行数和列数）。因此，您可以使用该向量的长度来确定图像是多通道还是单通道数组。

if (self.counter == 0):
    (rows,cols,channels) = self.img.shape #Check the size of your image
    print ('size: rows: {}, cols: {}, channels: {}'.format(rows, cols, channels))

现在，让我们试试我们的代码。在纯 Python 项目中，我们可以使用 --symlink-install 标志一次编译代码，然后测试对文件的修改，而无需再次编译；但在本项目中，我们混合了 C++ 和 Python 代码，因此每次修改都需要编译。因此，运行以下命令

cd ~/robovision_ros2_ws
colcon build

然后，我们运行：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png

最后，在一个新的终端里：

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_processing my_processing.py

在这里，我们可以看到有关图像的信息。

作业 2.1

图像的大小（列、行和通道）是多少？

3.2.2 图像处理

为了了解如何操作图像，让我们先做一些简单的编辑。在本例中，我们将在图像中添加一个递增的 ID 号。请记住，左上角是图像的原点（0,0），列数从左到右递增，行数从上到下递增。

C++

要在图像 img 中添加文字，我们使用 OpenCV 函数 cv::putText。每次接收和处理新图像时，我们都会增加一个整数计数变量，然后使用 std::to_string(counter) 函数将其转换为文本序列。

要了解 OpenCV 如何对图像进行编码，请使用 cv::Point 和 CV_RGB 值。cv::Point(col_id, row_id) 标记了文本的原点（即文本框的左下角）；使用不同的 col_id 和 row_id 值，看看会发生什么。

另一方面，CV_RGB（红、绿、蓝）参数决定了文本的颜色。请记住，单个颜色通道的范围是 0 到 255，因此请尝试不同的红、绿、蓝组合，看看你能创造出什么颜色！

cv::putText(image_,
	std::to_string(counter_),
	cv::Point(25, 25), //change these values cv::Point(col_id, row_id)
	cv::FONT_HERSHEY_DUPLEX,
	1.0,
	CV_RGB(255, 0, 0), //change these values CV_RGB(red, green, blue)
	2);

编译并测试您的代码，如上一节所述。

Python

同样，我们使用 cv2.putText 函数将文本字符串添加到 img 数组中。二维向量 (col_id, row_id) 标出了文本框的原点（左下角）。三维向量表示文本的颜色。然而，在 Python 中，颜色通道的顺序是（蓝、绿、红）。当您同时使用 C++ 和 Python 时，请注意这种差异。现在，给颜色矢量赋予不同的值，看看它的表现如何。

cv2.putText(self.img, 
	str(self.counter),
	(25,25), 
	cv2.FONT_HERSHEY_SIMPLEX, 
	1, 
	(255, 0, 0), 
	2, 
	cv2.LINE_AA)

再次尝试上一节中的代码。

你应该看到类似于

作业 2.2

提供五种不同文字位置和颜色组合的输出图像。

3.2.3 图像转换

3.2.3.1 将 BGR 转换为灰度图像

既然我们已经了解了图像的构成并理解了我们的代码，那么让我们添加一些图像变换。我们将向您展示如何应用 OpenCV 的一个内置函数，因此您可以根据手头的任务探索所有可用的函数。

让我们将图像从彩色变为灰度。我们可以将灰度图像理解为彩色图像，其中的灰度值是不同颜色通道的组合，如下所示：

且其三个通道的值相同，即 RGB = [灰度、灰度、灰度]。因此，为了减少内存使用量，许多系统只使用单通道矩阵，每个像素只有一个灰度值： RGB = [Gray]。

C++

请根据以下说明完成提供的代码。我们首先创建一个新矩阵 image_gray，然后将 OpenCV cvtColor 函数应用于原始图像：

cv::Mat image_gray;
cv::cvtColor(image_, image_gray, CV_BGR2GRAY);

让我们检查一下我们的图像：

if (counter_ == 0)
    std::cout << "image_gray size: rows: " << image_gray.rows << 
                 ", cols: " << image_gray.cols << 
                 ", depth: " << image_gray.channels() << std::endl;

并在新窗口中显示：

cv::imshow("gray", image_gray);

编译并测试您的代码，如前几节所述。

Python

同样，我们将 cvtColor 函数应用于原始图像，并将其存储到一个新的 img_gray 数组中。

img_gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY)

然后，我们检查图像。请记住，我们的形状运算符会为彩色图像返回三个值，为灰度图像返回两个值，您可以使用该向量的长度来确定图像是多通道还是单通道数组。

if (self.counter == 0):
    (rows,cols) = img_gray.shape
    print ('img_gray size: rows: {}, cols: {}'.format(rows, cols))
    print ('length (img_gray): {}'.format(len(img_gray.shape)))

并显示出来：

cv2.imshow("gray", img_gray)

现在，保存文件并像以前一样进行测试。你能看到什么？你是否注意到灰度图像只用一个通道来表示所有信息，而不是三个具有相同值的通道？

请再次按照上一节的方法尝试您的代码。

预期输出为

作业 2.3.1

灰度图像的大小（列、行和通道）是多少？显示生成的图像。

灰度图像变量的范围是什么？是否需要将它们设为全局变量？为什么？

2.3.2 彩色阈值处理

现在，我们将创建一个非常基本的颜色分割器。为简单起见，我们将使用 BGR 色彩空间，但使用 HSV 色彩空间更为常见；我们会让感兴趣的读者自行查找使用 HSV 图像进行色彩分割的额外步骤。

创建基于颜色的分割器的简单方法是找到目标颜色周围的所有像素。例如，如果我们要分割红色通道值在 my_red=[225] 左右的高强度红色，我们需要找到范围为 (my_red-delta , my_red+delta) 的所有像素，其中 delta 是我们定义的值；需要注意的是，在本例中，其他通道的范围为 0 至 255。

因此，我们有两个步骤来创建分割图像。首先，我们要找到所需的范围内的像素，并将其标记为 “ONE”，范围外的所有像素标记为 “0”，我们将这个包含 “0 ”和 “ONE ”的数组称为掩码。然后，我们创建一个输出图像，其中 MASK 中的所有元素都具有输入图像中的颜色值，所有其他像素都为零（或黑色）。

C++

首先，我们创建遮罩图像（别忘了声明它！），并根据像素是在颜色范围内还是范围外（记住数组中的顺序：[蓝、绿、红]），用 0 和 1 填充它，然后将输出图像中的所有元素置为 0（或黑色）。

cv::Mat mask;
cv::inRange(image_, cv::Scalar(0,0,220), cv::Scalar(240,255,240), mask);

在示例中，我们的目标颜色是红色=255，delta=20，蓝色和绿色通道的范围是 0 到 255。然后，我们只复制符合范围条件的像素。我们使用 image_.copyTo(output,mask)，只将掩码中带有 ONE 的像素复制到输出图像中：

cv::Mat image_filtered;
image_.copyTo(image_filtered, mask);

最后，别忘了展示您的图片

cv::imshow("mask", mask);
cv::imshow("image_filtered", image_filtered);

按照前面的章节编译和测试代码。

红色通道的分割图像应如下所示

Python

在这里，我们定义了要使用的有效颜色范围（记住数组中的顺序：[蓝、绿、红]）。现在，我们将选择一种蓝色，因此蓝色=220，delta=10，红色和绿色通道从 0 到 255，如下所示：

lower_val = np.array([200,0,0])
upper_val = np.array([240,255,255])

然后，我们使用 cv2.inRange 函数创建蒙版，每当一个像素位于该颜色范围内时，蒙版就会显示一个值：

mask = cv2.inRange(self.img, lower_val, upper_val)

最后，我们将这些像素从原始图像复制到分割后的图像，如下所示：

image_filtered = cv2.bitwise_and(self.img,self.img, mask= mask)

现在，我们展示我们的成果：

cv2.imshow("mask", mask)
cv2.imshow("image_filtered", image_filtered)

再次尝试上一节中的代码。

蓝色通道的分割图像应如下所示

作业 2.3.2

使用不同的颜色范围创建五张分割图像。在上一单元中，你还记得如何使用相机启动图像发布器吗？试着使用它，将不同颜色的物体放在相机前，看看会发生什么！

3.3. 每元素操作

尽管 OpenCV 提供了多种函数，但出于多种原因（从简单的检查到实现自己的算法），我们总是需要访问矩阵中的元素。在此，我们将提供一种简单的方法来访问数组中的所有元素。

3.3.1 单个元素访问

首先，让我们检查数组中给定（row_id，col_id）位置上的一个像素值。请记住，我们的数组从 (0,0) 开始，因此图像 image_ 中的最后一个元素（位于右下角）是 (image_.rows - 1, image_.cols - 1)。

C++

我们在 image_ 矩阵中使用 image_.at<cv::Vec3b>(row_id,col_id)属性来获取[蓝、绿、红]顺序的颜色元素，并使用 image_.at<uchar>(row_id,col_id)来获取灰度值--如果您不知道什么是 uchar 数据类型，请复习一下这个概念；简而言之，它是一个从 0 到 255 的整数。请注意我们函数中的 id 输入顺序，第一个索引总是对应行，第二个索引对应列；使用 OpenCV 函数时要注意元素的顺序，例如，请记住在我们的 cv::putText 函数中，cv::Point 元素的顺序是 (col_id, row_id)，而图像的 at<> 属性的顺序是 (row_id,col_id) 。

在这种情况下，我们将按如下方式检查数组中的中间点：

if (counter == 0)
{
	int row_id, col_id;
	row_id = image_.rows/2;
	col_id = image_.cols/2;

        std::cout << "pixel value in img at row=" << row_id <<
                     ", col=" << col_id <<
                     " is: " << image_.at<cv::Vec3b>(row_id,col_id) << std::endl;
        std::cout << "pixel value in img_gray at row=" << row_id <<
                     ", col=" << col_id <<
                     " is: " << (int)image_gray.at<uchar>(row_id,col_id) << std::endl;
}

按照前面的章节编译和测试代码。

重要：您应该注意到，对于彩色图像，img.at<cv::Vec3b>(row_id,col_id) 运算符的输出是一个 3D 向量，依次包含 [蓝、绿、红] 信息。

Python

在这里，我们像访问其他 Python 数组一样访问 img 数组中的元素：img[row_id,col_id] 用于获取按 [蓝、绿、红] 顺序排列的颜色像素或灰度值标量。我们总是先表示行，然后再表示列；同样，当您在 Python 中使用不同的 OpenCV 函数时，请注意输入顺序。然后，我们会得到中间像素元素的值，如下所示：

if (counter == 0):
    (rows,cols,channels) = self.img.shape
    row_id = int(rows/2)
    col_id = int(cols/2)

    print ('pixel value in img at row: {} and col: {} is {}'.format(row_id, col_id, self.img[row_id,col_id]))
    print ('pixel value in img_gray at row: {} and col: {} is {}'.format(row_id, col_id, img_gray[row_id,col_id]))

请再次尝试上一节中的代码。

重要：您应该注意到，对于彩色图像，img[row_id,col_id] 运算符的输出是一个三维矢量，依次包含[蓝、绿、红]信息。

作业 3.1

提供图像中 10 个不同像素的值。
你还记得从不同颜色通道的组合中获取灰度值的公式吗？它适用于您的图像吗？

3.3.2 多元素访问

既然我们已经知道如何访问图像中的元素以及其中的信息类型（颜色或灰度），那么我们就来连贯地访问所有像素。请记住，灰度值是图像中三个不同颜色通道的组合。近似灰度值的常用函数如下：

因此，让我们将该等式应用于图像中的所有像素。为此，我们需要运行一个嵌套 for 循环，其中一个索引从第一列到最后一列，另一个索引从第一行到最后一行。请记住，一般来说，数组索引从 0 开始，因此，它们应该以 (width-1) 和 (height-1) 结尾。

C++

首先，我们需要创建一个单通道矩阵来存储新图像，因此它的尺寸应与输入相同，每个值都应为 uchar：

cv::Mat image_gray_2 = cv::Mat::zeros(image_.rows,image_.cols, CV_8UC1);

然后，我们创建索引来访问图像中的所有元素。请记住，哪个索引对应行，哪个索引对应列！在我们的例子中，i 变量对应行，j 索引对应列：

for(int i=0; i<image_.rows; i++)
	for(int j=0; j<image_.cols; j++)

现在，我们访问所有 (i,j) 像素的像素值，并通过颜色通道的组合创建一个灰色值。我们使用 image_.at<cv::Vec3b>(row_id,col_id)运算符；请记住，该运算符的输出是一个 [Blue,Green,Red] 向量，因此我们可以得到

int gray_val = 0.11*image_.at<cv::Vec3b>(i,j)[0] + 0.59*image_.at<cv::Vec3b>(i,j)[1] + 0.3*image_.at<cv::Vec3b>(i,j)[2];

我们将 int 灰度值存储在 uchar new uchar 矩阵的 (i,j) 位置：

image_gray_2.at<uchar>(i,j) = (unsigned char)gray_val;

您的代码应该是这样的

cv::Mat image_gray_2 = cv::Mat::zeros(image_.rows,image_.cols, CV_8UC1);
for(int i=0; i<image_.rows; i++)
    for(int j=0; j<image_.cols; j++)
    {
        int gray_val = 0.11*image_.at<cv::Vec3b>(i,j)[0] + 0.59*image_.at<cv::Vec3b>(i,j)[1] + 0.3*image_.at<cv::Vec3b>(i,j)[2];
        image_gray_2.at<uchar>(i,j) = (unsigned char)gray_val;
    }

最后，别忘了显示您的图片（每个新窗口都应有唯一的名称）：

cv::imshow("gray_2", image_gray_2);

编译并测试您的代码，如前几节所述。

Python

同样，我们先定义新的 uint8 矩阵（Python 中的 uint8 数据类型类似于 C++ 中的 uchar 数据类型）：

(rows,cols,channels) = img.shape
img_gray_2 = np.zeros( (rows,cols,1), np.uint8 )

然后，我们创建索引（请参阅手册了解 range(int) 函数的工作原理；基本上，它会创建一个从 0 到 int-1 的数字序列），记住哪个索引对应行和列：

for i in range(rows):
	for j in range(cols):

然后，获取像素 (i,j) 的信息，将其组合成灰度值，并将其存储在新的灰度图像中：

for i in range(rows):
	for j in range(cols):

您的代码应该是这样的

(rows,cols,channels) = self.img.shape
img_gray_2 = np.zeros( (rows,cols,1), np.uint8 )

for i in range(rows):
    for j in range(cols):
        p = self.img[i,j]
        img_gray_2[i,j] = int( int(0.11*p[0]) + 0.59*int(p[1]) + int(0.3*p[2]) )

最后，别忘了展示您的新图片：

cv2.imshow("gray_2", img_gray_2)

请再次按照上一节的方法尝试您的代码。

重要： 你能注意到速度上的差异吗？虽然 C++ 看起来比 Python 更麻烦，但执行速度却快得多。通常的做法是在应用程序中使用 C++ 进行核心操作，而使用 Python 进行高级处理。

作业 3.2

创建一个循环，访问灰度图像中的所有元素并将其旋转 180 度。结果应如下图所示。

提示 在开始循环之前，您应该创建一个新矩阵。此外，请观察原始图像和新图像中的指数。它们之间的关系是什么？

四、使用 ROS 处理点云

我们将介绍 ROS 中的点云数据，并提出一个简单的任务，跟踪移动机器人上安装的 RGBD 摄像机前移动的人。

4.0. 获取附加数据

4.0.1 Rosbag 数据

请确保您已经下载了可在此处（谷歌硬盘）获取的附加数据。

这些文件夹应位于 ~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/ 文件夹中。

为了确保万无一失，让我们来测试一下。在一个终端中运行

ros2 bag play ~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/person_static --loop

在第二个终端运行此命令：

ros2 topic list

您应该可以看到几个主题！

4.1. 了解您的 RGBD 图像

在这一课中，我们将应用过去两个单元中学到的所有知识。首先，让我们检查 rgbd_reader.py 文件中的代码。

在主函数中，我们可以看到，与之前所做的一样，我们首先初始化了节点

super().__init__("point_cloud_centroid")

然后，我们订阅要使用的主题：

self.rgb_subscriber_ = self.create_subscription(
    Image, "/camera/rgb/image_rect_color", self.callback_rgb_rect, 10)
self.depth_subscriber_ = self.create_subscription(
    Image, "/camera/depth_registered/image", self.callback_depth_rect, 10)
self.point_cloud_subscriber_ = self.create_subscription(
    PointCloud2, "/camera/depth_registered/points", self.callback_point_cloud, 10)

在这里，您可以注意到让我们的订阅者的回调函数以不同的速率更新我们的变量以及使用不同的定时器函数来处理这些信息的相关性。在本例中，我们订阅了三个主题：RGB 和深度图像以及 3D 点的点云，它们都来自同一个 RGBD 传感器

RGB 图像是具有三个通道（红、绿、蓝）的彩色图像，而深度图像则对应于图像中物体的每个像素到通过摄像机中心的正交平面的度量距离；您可以将其想象为从上方看到的给定点到摄像机的水平距离，与高度无关，如图所示

我们曾经使用过 RGB 图像的图像主题。深度图像是一个浮点矩阵，与以毫米为单位的度量距离相对应。因此，在回调函数 callback_depth_rect 中，我们将其读取为

self.depth_=bridge_depth.imgmsg_to_cv2(msg,"32FC1").copy()
self.depth_mat_ = np.array(self.depth_, dtype=np.float32)

由于数值范围在 400（40 厘米）到 10000（10 米）之间，我们将其归一化为有效的图像值，并将其保存在图像数组中以便显示

v2.normalize(self.depth_mat_, self.depth_, 0, 1, cv2.NORM_MINMAX)

重要提示：深度信息来自结构化红外光传感器，因此反光或透明表面往往会扭曲深度信息；这些点在深度图像中显示为黑色（零值）像素。我们的 RGBD 传感器能够读取的最小距离是 40 厘米，任何接近这个距离的点也将是零值。

此外，从 RGB 和深度图像中，我们可以获得图像中每个像素在空间中的 XYZ 公制位置--我们将不再继续讨论这个问题，因为幸运的是，我们看到 ROS 已经计算出了它，PointCloud2 类型的/camera/depth_registered/points 提供了这些信息。如果输入

ros2 interface show sensor_msgs/msg/PointCloud2

在终端中，您可以看到这类报文的组成。

点云报文是一个以毫米为单位的元组（x、y、z......）列表，其类型为 PointCloud2。因此，在回调函数 callback_point_cloud 中，我们将其读作

self.point_cloud_ = np.array(list(pc2.read_points(msg, field_names=None, skip_nans=False)))

这会以列表形式读取点云，因此我们要将其重塑为与 RGB 图像对齐的矩阵形式

if msg.height > 1:
    self.point_cloud_ = self.point_cloud_.reshape((msg.height, msg.width, -1))
    
    rows, cols, _ = self.point_cloud_.shape
    print ('new message has arrived; point cloud size: rows: {}, cols: {}'.format(rows, cols))

回到我们的代码，我们可以看到有一个 ROS 发布器，我们想在这里发布摄像机前物体的 3D 位置；请记住，主题应该有一个唯一的名称--在本例中，我们将其命名为 /object_centroid，并且其类型为 Pose：

self.centroid_publisher_ = self.create_publisher(
    Pose, "/object_centroid", 10)

姿势信息属于 geometry_msgs 类型，包括空间中每个点相对于摄像机中心的三维位置（以米为单位）和四元数形式的四维方位：

• object_centroid.position.x
• object_centroid.position.y
• object_centroid.position.z
• object_centroid.orientation.x = quaternion[0]
• object_centroid.orientation.y = quaternion[1]
• object_centroid.orientation.z = quaternion[2]
• object_centroid.orientation.w = quaternion[3]

在 PointCloud2 信息中，轴线如下：

• x: positive from the center of the camera to the right
• y: positive from the center of the camera to the bottom
• z: positive from the center of the camera to the front

以摄像机中心为原点，平面上点 p 的 XY 轴（正视图）和 ROLL 角分别为

平面上点 p 的 YZ 轴（侧视图）和 PITCH 角分别为

和 XZ 轴（俯视图）以及平面上点 p 的 YAW 角分别为

作业 1.1

请查看 rgbd_reader.cpp 实现。C++ 和 Python 的结构非常相似，因此您可以根据自己的需要使用其中任何一种。

4.2. 点云操作

4.2.1 单元素访问

现在，我们将在 Python 的定时器函数 point_cloud_processing 中处理这些信息

self.processing_timer_ = self.create_timer(0.030, self.point_cloud_processing)

而在 C++ 中

processing_timer_ = this->create_wall_timer(
    std::chrono::milliseconds(30),
    std::bind(&PointCloudCentroidNode::point_cloud_processing, this));

我们访问数组中的单个元素就像访问 Python 中的数组一样 self.point_cloud_[row_id,col_id,0]。要访问单一维度 X、Y 或 Z，我们可以直接输入 self.point_cloud_[row_id,col_id,0][XYZ]，其中 XYZ=0 表示维度 X，XYZ=1 表示维度 Y，XYZ=2 表示维度 Z。

rows, cols, _= self.point_cloud_.shape
row_id = int(rows/2)
col_id = int(cols/2)

p = [float(self.point_cloud_[row_id, col_id, 0][0]), 
     float(self.point_cloud_[row_id, col_id, 0][1]), 
     float(self.point_cloud_[row_id, col_id, 0][2])]

在 C++ 中，我们以 point_cloud_.at<cv::Vec4f>(row_id, col_id) 的形式访问矩阵中的元素，以 point_cloud_.at<cv::Vec4f>(row_id, col_id)[XYZ] 的形式访问每个分量，其中 XYZ=0 表示维数 X，XYZ=1 表示维数 Y，XYZ=2 表示维数 Z。

int rows = point_cloud_.rows;
int cols = point_cloud_.cols;
int row_id = rows / 2;
int col_id = cols / 2;

cv::Vec4f point = point_cloud_.at<cv::Vec4f>(row_id, col_id);

最后，我们将其存储在全局变量中，以便 ROS 发布者稍后在程序主循环中发布。在 Python 中

self.centroid_=Pose()

self.centroid_.position.x = p[0]
self.centroid_.position.y = p[1]
self.centroid_.position.z = p[2]

self.centroid_.orientation.x=0.0
self.centroid_.orientation.y=0.0
self.centroid_.orientation.z=0.0
self.centroid_.orientation.w=1.0

#Publish centroid pose
self.centroid_publisher_.publish(self.centroid_)

而在 C++ 中

geometry_msgs::msg::Pose centroid;

centroid.position.x = static_cast<float>(point[0]); // x
centroid.position.y = static_cast<float>(point[1]); // y
centroid.position.z = static_cast<float>(point[2]); // z

centroid.orientation.x = 0.0;
centroid.orientation.y = 0.0;
centroid.orientation.z = 0.0;
centroid.orientation.w = 1.0;

// Publish the centroid pose
centroid_publisher_->publish(centroid);

4.2.2 运行代码

现在，让我们试试我们的代码。运行以下命令

cd ~/robovision_ros2_ws
colcon build

如果您没有 RGBD 摄像机，也不用担心，我们会为您提供一个 ROS 包，其中包含使用安装在 Turtlebot 2 顶部、距离地面 1.0 米高处的 XTion Pro 收集到的一些数据。在其他终端运行

ros2 bag play ~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/person_static --loop

然后，在另一个终端中输入

ros2 topic list

你能看到所有不同的主题吗？

现在输入 Python 实现

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_rgbd rgbd_reader.py

或者，对于 C++ 执行

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_rgbd rgbd_reader

您能在图片中看到我们中间点的三维信息吗？

最后，在一个新的终端：

ros2 topic info /object_centroid

然后

ros2 topic echo /object_centroid

您应该可以在我们的 ROS 主题中看到发布的相同信息！

作业 2.1

提供图像中五个不同点的 3D 位置（输入不同的 row_id 和 col_id）。您能看出相对于中心点的 X、Y 和 Z 值是如何变化的吗？中心点右侧的 X 值为正，中心点下方的 Y 值为正。

点（row_id=0，col_id=0）的 3D 信息是什么？请注意，由于结构光的反射特性，当某一点的信息不可用时，系统会返回 “nan ”值。在 Python 中，您可以使用数学库中的 math.isnan() 函数来检查变量是否为 nan -- 该函数返回 True 或 False 值。例如，您可以使用 if ( not math.isnan(( self.point_cloud_[row_id, col_id, 0][0] ): 结构验证您的数据。类似地，在 C++ 中我们有 std::isan()。

4.3. 最终项目

现在你已经掌握了所有工具，可以编写一个漂亮的机器人视觉项目。

问题是这样的： 我们有一个机器人，它正对着一个移动的人，我们希望找到这个人与机器人中心的相对位置（这里表示为摄像头位置）。

换句话说，我们要找到 person_1 的 3D 位置

如何实现？

如果没有 RGBD 摄像机，可以使用 ~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/ 文件夹中的 person_static 和 person_dynamic ROS 包。

提示 请记住，“点云 ”会返回摄像机可见的所有点的所有 3D 信息，因此为什么不对其进行限制呢？您可以创建一个有效区域，在此区域内您可以更好地跟踪您的人，而无需任何额外信息

您可以使用一种结构，比较每个图像 3D 点是否都在该方框内（首先检查您的点是否不在方框内）。

不要忘记使用适当的符号，尤其是在 Y 轴上，如果摄像机距离地面 1.0 米，那么与地面不同的任何一点应该满足什么条件？同样，要特别注意 Y 轴方向。

提示 记住 ROLL、PITCH 和 YAW 的定义

您可以使用 Python 函数将这些值转换为四元数形式

from tf_transformations import quaternion_from_euler
quaternion = quaternion_from_euler(roll, pitch, yaw)

然后将其存储到我们的变量

object_centroid.orientation.x = quaternion[0]
object_centroid.orientation.y = quaternion[1]
object_centroid.orientation.z = quaternion[2]
object_centroid.orientation.w = quaternion[3]

同样，在 C++ 中

tf2::Quaternion quaternion;
quaternion.setRPY(roll, pitch, yaw);

和

centroid.orientation.x = quaternion.x();
centroid.orientation.y = quaternion.y();
centroid.orientation.z = quaternion.z();
centroid.orientation.w = quaternion.w();

现在，尝试用 Python 和 C++ 实现它。祝你好运

挑战 3.1

如何提高性能？重要： 请记住，每个二维图像点在点云中都有其对应的三维点，你可以利用这些信息！也许您可以尝试先检测人脸，然后取该区域内 3D 点的平均值。OpenCV 提供了一系列函数，或许能帮到你，例如

OpenCV: Cascade Classifier

那么，检测整个人的身体并获取人的边界框内三维点的平均值呢？在上面的示例中，您可以添加一个额外的分类器：

bodydetection = cv2.CascadeClassifier('cascades/haarcascade_fullbody.xml')

你愿意接受挑战吗？

五、ROS 中的服务和客户端

在 ROS2 中，客户端和服务支持节点之间的同步通信。客户端和服务与发布者和订阅者不同，发布者和订阅者可促进连续数据流，而客户端和服务则专为请求-响应交互而设计。客户端节点向服务发送请求，服务节点处理请求并发回响应。这非常适合需要特定操作或即时反馈的任务，例如控制机械臂或查询传感器状态。

您可以查看 C++ 的一些基本概念：

Writing a simple service and client (C++) — ROS 2 Documentation: Foxy documentation

和 Python：

Writing a simple service and client (C++) — ROS 2 Documentation: Foxy documentation

5.1. ROS2 接口

在 ROS2 中，接口允许节点使用预定义的数据结构进行通信。接口有两种形式：

• 消息（msg）： 定义主题的数据结构。
• 服务（srv）： 为服务定义请求-响应交互。

本教程使用 robovision_interfaces 软件包中的示例来演示如何创建和使用 ROS2 接口。

5.1.1 设置软件包

为自定义接口组织文件夹结构如下：

robovision_interfaces/
├── CMakeLists.txt
├── package.xml
├── msg/
│   └── ObjectCentroid.msg
└── srv/
    └── GetPointCenter.srv

该文件夹与项目中的任何新 ROS2 软件包处于同一级别。

5.1.2 定义自定义消息： ObjectCentroid.msg

自定义信息描述了主题的数据结构。ObjectCentroid.msg 定义了中心点坐标和一个数组：

float64   x
float64   y
float64   z
float64[] centroid

其中

• float64 x、y、z： 表示中心点的三维坐标。
• float64[] centroid： 用于存储附加数据点的动态数组。

5.1.3 定义自定义服务： GetPointCenter.srv

自定义服务定义了请求和响应的结构。GetPointCenter.srv 文件如下所示：

int64          x
int64          y
---
ObjectCentroid point

其中

• 请求（int64 x、y）： 接受两个整数输入（如像素坐标）。
• 响应（ObjectCentroid 点）： 以 ObjectCentroid 消息的形式返回计算出的中心点。

--- 分隔了服务定义的请求和响应部分。请注意，如果消息是在同一个软件包中定义的，则软件包名称不会出现在服务或消息定义中。如果消息是在其他地方定义的，我们就必须注明，例如 robovision_interfaces/msg/ObjectCentroid point。

5.1.4 将接口集成到构建系统中

更新 CMakeLists.txt 以包含消息和服务定义：

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/ObjectCentroid.msg"
  "srv/GetPointCenter.srv"
)

ament_export_dependencies(rosidl_default_runtime)

并更新 package.xml 以声明依赖关系：

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>

5.2. ROS 服务

主题和服务的主要区别在于，当主题在工作时，服务是在请求下工作的（这可能会节省资源）。

让我们比较一下我们的 “rgbd_reader ”和 “robovision_service ”文件（均为 C++ 和 Python 语言）。它们非常相似！我们有两个主要变化。首先，我们没有发布者，因为它会根据请求发送响应。其次，我们没有计时器，因为它不是无限期工作的。取而代之的是，我们创建了一个 ROS2 服务，当我们调用该服务时，它会进入一个回调函数。在 Python 中是

self.get_point_center_service_ = self.create_service(
    GetPointCenter, "get_point_center", self.point_cloud_processing)

而在 C++ 中

get_point_center_service_ = this->create_service<robovision_interfaces::srv::GetPointCenter>(
    "get_point_center", 
    std::bind(&PointCloudCentroidNode::point_cloud_processing, this, 
	      std::placeholders::_1, std::placeholders::_2));

请注意，我们需要声明服务类型，在本例中，我们使用的是自定义接口 robovision_interfaces/srv/GetPointCenter。此外，我们还更改了回调函数的定义，以纳入请求和响应。最后，还需要注意的是，我们需要填充响应并将其返回（而不是将结果发布到自定义主题中）。

5.2.1 测试代码

首先，让我们编译它

cd ~/robovision_ros2_ws
colcon build

并启动它（别忘了启动你的挎包！）。

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_services robovision_service

在另一个终端，输入

ros2 service list

您看到我们刚刚宣布的服务了吗？现在输入

ros2 service type /get_point_center

它使用的是什么接口？您可以检查它

ros2 interfaces show robovision_interfaces/srv/GetPointCenter

最后，让我们调用我们的服务

ros2 service call /get_point_center robovision_interfaces/srv/GetPointCenter "{x: 320, y: 240}"

结果如何？

5.3. ROS 客户端

在 ROS2 中，客户端用于以同步请求-响应的方式与服务交互。本教程将演示如何实现和使用 ROS2 客户端。示例中的 PointCloudCentroidNode 会调用名为 get_point_center 的服务来计算二维点的中心点。

5.3.1 Python 中的客户端

robovision_client.py 文件演示了与 GetPointCenter 服务交互的 ROS2 客户端实现。下面，我们将分解其关键组件。

5.3.1.1 节点初始化

PointCloudCentroidNode 类初始化了一个名为 point_cloud_client 的 ROS2 节点。

self.declare_parameter("x", 320)
self.declare_parameter("y", 240)

self.x_ = self.get_parameter("x").value
self.y_ = self.get_parameter("y").value

参数 x 和 y 定义了要从服务中查询的目标坐标。

5.3.1.2 服务调用设置

为了证明我们可以随时调用服务，我们创建了一个 ROS2 定时器，每 2.5 秒定期调用一次服务：

self.client_call_timer_ = self.create_timer(2.5, self.client_caller)

client_caller() 函数通过 call_get_point_center_server 准备并发送服务请求：

def client_caller(self):
    self.call_get_point_center_server(self.x_, self.y_)

每次我们要调用服务时，都会用到这个函数，所以，你可以把它当作一个模板。接下来我们将对此进行说明。

创建客户端并发出请求
使用指定的服务类型（GetPointCenter）和名称（get_point_center）创建 ROS2 客户端：

client = self.create_client(GetPointCenter, "get_point_center")
while not client.wait_for_service(1.0):
    self.get_logger().warn("Waiting for get_point_center service...")

填入请求信息，并进行异步调用：

request = GetPointCenter.Request()
request.x = _x
request.y = _y

future = client.call_async(request)
future.add_done_callback(
    partial(self.callback_call_point_cloud, _x=request.x, _y=request.y))

处理响应
回调 callback_call_point_cloud() 会处理服务响应（我们会将响应存储在全局变量中，以备将来使用）：

def callback_call_point_cloud(self, future, _x, _y):
    try:
        response = future.result()
        self.get_logger().info(
            "(" + str(_x) + ", " + str(_y) + ") position is: " + str(response.point))

        self.point_ = response.point
    except Exception as e:
        self.get_logger().error("Service call failed %r" % (e,))

5.3.1.3 测试代码

首先，让我们编译它

cd ~/robovision_ros2_ws
colcon build

并开始我们的服务（别忘了启动您的rosbag！）。

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_services robovision_service.py

在另一个终端，输入

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_services robovision_client.py

结果如何？

5.3.2 C++ 中的客户端

robovision_client.cpp 文件提供了 ROS2 客户端在 C++ 中的等效实现。以下是与 Python 示例的重点和主要区别。

在 Python 实现中，我们创建了一个计时器，以显示我们可以在任何时刻调用我们的服务：

client_call_timer_ = this->create_wall_timer(
    std::chrono::milliseconds(2500),
    std::bind(&PointCloudCentroidNode::client_caller, this));

client_caller 函数调用一个 get_point_center() 函数；我们将此结构用作模板。与 Python 的一个主要区别是，我们需要在一个线程中调用我们的服务，这样程序流程才能继续：

threads_.push_back(std::thread(std::bind(&PointCloudCentroidNode::call_get_point_center_server, this, x, y)));

我们线程中的 call_get_point_center_server 与 Python 实现一样。我们创建了一个 C++ 客户端，服务类型为 robovision_interfaces::srv::GetPointCenter，名称为 get_point_center：

auto client = this->create_client<robovision_interfaces::srv::GetPointCenter>("get_point_center");
while (!client->wait_for_service(std::chrono::seconds(1))) {
    RCLCPP_WARN(this->get_logger(), "Waiting for get_point_center service...");
}

请求以异步方式构建和发送：

auto request = std::make_shared<robovision_interfaces::srv::GetPointCenter::Request>();
request->x = x;
request->y = y;

auto future = client->async_send_request(request);

通过等待未来对象，同步处理来自服务的响应：

try {
    auto response = future.get();
    RCLCPP_INFO(this->get_logger(),
                "(%d, %d) position is: [%f, %f, %f]",
                x, y, response->point.centroid[0],
                response->point.centroid[1],
                response->point.centroid[2]);

    point_ = response->point.centroid;
} catch (const std::exception &e) {
    RCLCPP_ERROR(this->get_logger(), "Service call failed.");
}

5.3.2.1 测试代码

首先，让我们编译它

cd ~/robovision_ros2_ws
colcon build

并开始我们的服务（别忘了启动您的rosbag！）。

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_services robovision_service

在另一个终端，输入

source ~/robovision_ros2_ws/install/setup.bash
ros2 run robovision_services robovision_client

结果如何？