机器人与机器视觉：在ROS中使用OpenCV
ROS 教程：如何在计算机视觉的机器人拾取和放置任务中使用 OpenCV计算机视觉是机器人技术的重要组成部分。它帮助机器人从相机数据中提取信息以了解其环境。应用范围从提取物体及其位置到检查制造零件是否存在生产错误，再到检测自动驾驶应用中的行人。
在本文中，我将展示如何在机器人中使用计算机视觉来使机器人手臂执行有点智能的拾取和放置任务。为此，我们将使用流行的开源库OpenCV。我们将学习如何安装 OpenCV，将其连接到 ROS 和 Kinect 深度摄像头，应用计算机视觉算法提取必要的信息并将其发送给我们的机器人。这将是一篇更长的文章，我将在其中详细解释所有内容，以描述它从头到尾是如何工作的。对于那些只对特定主题感兴趣的人，在我们深入研究之前，我整理了一份内容列表。
在本教程中，我们将讨论：
- 什么是OpenCV以及如何安装它？
- 如何设置拾取和放置任务并运行应用程序
- 演练 OpenCV 节点实现
- 使用 cv_bridge 包在 ROS 和 OpenCV 之间转换相机图像
- 将计算机视觉算法应用于图像
- 从图像中提取边界及其中心
- 提取对象位置
- 将 2D 图像像素转换为世界帧中的 3D 位置
- 将结果作为服务调用发送
- 如何从拾取和放置节点调用 OpenCV 节点服务
让我们开始吧！
什么是OpenCV以及如何安装它？
OpenCV最初是在英特尔开始的，后来也由Willow Garage开发（这就是ROS发明的地方）。它提供了一组优化的计算机视觉算法，这些算法可移植且易于使用。它是开源的，自2012年以来，非营利组织 OpenCV.org 接管了支持。
如果您的机器上正确安装了 ROS，则 OpenCV 也应该已经安装。您可以通过运行以下命令来检查： $ pkg-config --modversion opencv 如果这没有产生任何结果，您可以尝试： $ dpkg -l | grep libopencv
如果您发现 OpenCV 尚未安装，请按照以下链接中的说明进行操作。
我使用ROS Melodic，默认情况下使用OpenCV 3，所以这是本教程中使用的版本。也可以使用带有Melodic的较新的OpenCV 4，但需要更多的努力才能使其运行。其中一个附加步骤涉及我们用于转换图像cv_bridge。要使用OpenCV 4，您需要从源代码克隆和编译它。这是您将遇到的问题之一，可能还有更多。如果您仍然想尝试使用 OpenCV 4 进行设置，您可能希望从堆栈溢出的这个问题开始。
如何设置和运行“拾取和放置”任务
我们希望开发我们的计算机视觉解决方案，并将其应用于真实的机器人场景中。因此，我们选择了在前面的教程中也使用的拾取和放置任务，其中我们使用 Moveit C++ 界面处理拾取和放置任务，以及如何使用 Moveit 和深度摄像头进行防撞。
下图显示了设置。左边的蓝色框应该放在右边的白板上。在第一个教程中，我们使用 Moveit 的C++接口对任务进行编程，并对盒子（起始位置）和板（目标位置）的位置进行硬编码。我们还告诉Moveit中间绿色障碍物的确切位置和大小，以便它可以找到绕过它的路径。在第二个教程中，我们添加了一个深度摄像头，可以自动检测中间的障碍物，而无需事先告诉 Moveit。现在我们想使用从相机获得的图像，并使用计算机视觉算法自动提取盒子和板的位置！
我们将首先完成设置并运行它，以便您可以看到程序的实际功能。然后，我将解释实现的所有细节，并引导您完成代码。
该项目由三个主要包组成：
- 适用于 UR5 机器人、抓手和 Kinect 深度摄像头的 Moveit 配置包。如果您执行了前面的教程，则应该已安装此包。如果没有，请转到有关如何创建 Moveit 配置包的教程的快速入门部分，然后按照步骤操作。之后，请查看避免碰撞教程中有关如何将 Kinect 3D 相机添加到环境的部分，并复制前两个步骤。
- OpenCV 节点，通过应用计算机视觉算法从相机图像中提取对象位置并将其发送到我们的应用程序。在本文的后面，我将详细解释确切的实现。现在请继续将此存储库克隆到 catkin 工作区的 src 文件夹中：
~/catkin_ws/src$ git clone https://github.com/dairal/opencv_services.git
3. 包含拾取和放置任务的实际实现的“拾取和放置”节点。这将是一个新的包，但实现将与我们在前两个教程中使用的实现非常相似。要安装软件包，还请将以下存储库克隆到 catkin 工作区的源文件夹中：
~/catkin_ws/src$ git clone https://github.com/dairal/ur5_pick_and_place_opencv.git
现在我们已经有了我们需要的所有软件包，继续构建您的 catkin 工作区：
~/catkin_ws/src$ catkin_make
为了运行任务，我们需要在 Linux 终端中执行三个命令。首先，我们需要启动我们的环境来模拟 Rviz 和 Gazebo 中的机器人。为此，我们使用两个附加参数运行ur5_gripper_moveit_config包的 demo_gazebo.launch 文件：
$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch world_name:=$(rospack find ur5_pick_and_place_opencv)/world/simple_pick_and_place_collision robot_spawn_position_z_axis:=1.21
希望您的环境看起来与上图相似，并且您会看到在 Rviz 中可视化的深度相机数据。其次，我们启动OpenCV节点。我们要运行的节点称为opencv_extract_object_positions。通过在新终端中执行以下命令来启动它：
$ rosrun opencv_services opencv_extract_object_positions
除了节点的输出之外，您还将看到一个自动显示在计算机上的图像（更准确地说，它是多个图像不断被新图像替换）。在这张图片上，你已经可以看到我们的计算机视觉算法的输出。您可能会看到在盒子和盘子的中心绘制的点。这些是我们从图像中提取的特征。opencv 节点已准备好将提取的仓位发送到我们的拾取和放置节点。因此，最后，在我们的第三个命令中，我们启动包含在ur5_pick_and_place_opencv包中的pick_and_place_opencv节点。因此，请执行：
$ rosrun ur5_pick_and_place_opencv pick_and_place_opencv
现在你应该能够看到机器人在盒子上方移动，用抓手捡起它，把它移到盘子上方，然后放在那里。手指按下，一切都对你有用。
如果你做过前面的教程，你可能会认为这很好，但我们之前已经可以这样做了，而无需将一些花哨的计算机视觉算法应用于相机图像。确实如此，但现在您可以尝试修改设置中蓝框和盘子的位置。但是，不要太疯狂。只需将盒子和板从其初始起始位置移动一点点，然后再次启动拾取和放置节点，稍后我们将讨论我们不能过多地改变位置的原因。 我承认它不是最强大的应用程序，有时可能会失败。需要作出更多努力，使其更加健壮和可靠。但是，该实现主要用于教育目的，我试图使其尽可能易于理解，因此让我们继续，我将引导您完成代码的每个细节。 创建 OpenCV 节点 这个节点应该为我们提供拾取和放置任务的开始和目标位置。在我们的例子中，这是蓝色盒子和盘子的位置。让我向您概述实现这一目标所需的步骤：
- 我们需要获取设置中 Kinect 相机发布的相机数据。这需要一些转换，因为OpenCV使用的图像格式与相机发布的图像格式不同。
- 我们将计算机视觉算法应用于图像。我将向您解释所选算法，解释我为什么选择它们以及如何使用它们。
- 之后，我们将能够从图像中提取边界及其中心。
- 中间步骤（我将在后面解释）是提取框和目标的对象位置所必需的。
- 此时，我们在从相机获得的图像中拥有2D起点和目标位置。但是，如果我们想向机器人发送位置命令，我们需要将 2D 图像位置转换为机器人坐标框中的 3D 位置。我们将使用 ROS tf2 包进行此转换。
- 获得最终结果后，我们希望将其发送到拾取和放置节点。我们在 OpenCV 节点中实现一个服务调用，可以从外部模块调用，并返回起点和目标位置。
现在您已经大致了解了必要的步骤，让我们开始查看实现。您可以在opencv_extract_object_positions.cpp文件中找到OpenCV节点的完整代码，您可以通过此链接在Github上查看该文件。 与往常一样，我们通过初始化节点并创建一个节点处理程序来开始我们的 main 函数： 1. 使用 cv_bridge 包在 ROS 和 OpenCV 之间转换相机图像 首先，我们需要获取 Kinect 相机发布的相机数据。对于映像订阅和发布，建议使用 image_transport 包。用法与订阅原始sensor_msgs/图像消息非常相似。我们包括以下套餐： 然后，我们定义一个名为的常量字符串，该字符串保存发布 Kinect 数据的主题的名称：IMAGE_TOPIC
接下来，我们创建一个与节点处理程序等效image_transport的对象，并使用它来创建订阅消息的订阅者。使用第二个参数，我们定义是否只想获取图像的子集（例如，10 意味着我们每获得第 10 个已发布的图像）。我们想要每个样本，所以我们定义.第三个参数是我们在收到新图像时要调用的函数的名称。ImageTransportIMAGE_TOPIC1image_cb
现在让我们看一下函数。ROS对中定义的图像使用自己的消息格式，而OpenCV使用不同的格式实现为。它们不兼容，但幸运的是，有cv_bridge包可以为我们进行格式之间的转换，因此我们很乐意在.cpp文件中包含cv_bridge：image_cbsensor_msgs/Imagecv::Mat
在函数内部，我们实例化一个 cv 图像指针 .然后我们调用函数，该函数接受传入的图像消息，并将消息的编码类型转换为输入参数。转换后，它会返回一个 cv 图像指针，我们将其分配给我们之前创建的。如果抛出异常，我们会捕获它。image_cbcv_ptrtoCvCopycv_ptr
最后，我们从深度摄像头接收的图像如下所示：
2. 将计算机视觉算法应用于图像
现在我们要从此图像中提取信息。有不同的方法可以做到这一点。您可以将计算机视觉算法视为一个工具箱，可用于获取所需的信息。您可以通过不同的方式组合这些算法，并且通常有多种方法可以实现您的目标。 在函数内部（第 66 行），我们要使用的第一种方法是灰度缩放。它使应用进一步的算法更容易。此外，我们不需要颜色信息，尽管可以通过从图像中提取某些颜色（分别为蓝色和白色）来考虑获取盒子和板的位置。这就是使用OpenCV完成灰度缩放的方式：apply_cv_algorithms OpenCV的功能将图像从一个色彩空间转换为另一个色彩空间。使用第三个参数，我们选择转换为灰度缩放的图像，然后将其存储在对象中。cvtColorcv::COLOR_BGR2GRAYimg_gray
接下来，我们采用边缘检测算法来检测物体的轮廓。我们使用 Canny 算法，根据强度梯度提取边缘。这意味着它将检测彼此相邻的像素之间的显着差异作为边缘。从上图中可以看出，盒子和板具有不同的亮度水平，但它们在背景下都很突出，这使得该算法特别适合。我们像这样应用算法： 该函数接受四个参数。前两个是输入和输出图像。第三个和第四个定义阈值并影响要解释为边的梯度的显著性水平。特别是第四个参数定义了这种敏感性。在我们的例子中，我们把这是一个相当高的值。对象和背景之间的强烈对比使我们能够设置如此高的值，其优点是我们可以“过滤掉”中间的碰撞框。cv::Canny350 我不会详细介绍Canny算法的工作原理，但如果您有兴趣，可以在这里找到更多信息。这是我们得到的结果：
3. 从图像中提取边界及其中心
即使生成的图片中的明亮区域看起来像一条实线，到目前为止，对计算机来说，它只是一堆具有陡峭强度梯度的像素。在extract_centroids函数（第 77 行）中，我们现在想要连接这些点以获得对象的实际边界。因此，我们使用OpenCV的函数，它只是将所有具有相同强度的像素连接到曲线上：findContours
让我们看一下函数采用的最后三个参数。我们选择轮廓检索模式。如果其他形状中存在形状，则此参数很重要。如果是这种情况，您可能想知道它们之间的关系，即哪种形状在哪个形状中。使用第四个参数，我们选择要存储多少有关这些关系的信息。然后将结果存储在第 81 行实例化的层次结构矩阵中。我们的对象是分开的，所以我们并不真正关心那部分，所以这只是给你一些上下文。findContourscv::RETR_TREE 使用最后一个参数，我们可以定义是否使用一些近似值来存储可以节省内存的边界。有了设置，我们没有。生成的等值线存储在矢量中。cv::CHAIN_APPROX_NONEcontours
现在我们找到了边界，但最终我们想知道物体的位置。我们如何做到这一点？通过计算等值线的中心： 该算法并不像看起来那么复杂。在第 86 行中，我们遍历我们发现的所有轮廓，可以计算图像时刻。图像时刻是特定形状中像素强度的加权平均值。从这些时刻M中，我们可以计算形状的中心（质心），如下所示： 我们对第 90 到 94 行中的所有时刻进行此计算。这就是我们为了获得对象的质心而需要做的一切。 通过以下几条线，我们绘制轮廓和质心，并在单独的窗口中显示它们： 这是我们得到的结果：
质心被绘制成小圆圈，我们可以看到物体的中心被很好地检测到。但是，在图像的底部，我们可以看到还检测到其他形状中心。我们得到结果之前的最后一步是选择正确的结果。
4. 提取对象位置
为了选择正确的质心，我们使用一种简单的方法。我们只是假设我们的起始位置（蓝色框）在图像的右上角，我们的目标位置（板）在左上角。 为此，我编写了一个简单的函数，该函数将质心的向量和用户定义的矩形作为输入。然后，它计算矩形区域内所有质心的平均值。为什么我们需要计算平均值？多个轮廓，因此，可能会为同一对象检测到多个质心。为了解决这个问题，我们只需取平均值即可。在图像中无法清楚地看到额外的质心，因为它们彼此非常接近，但它们显示在矢量中。
此函数被调用两次。一次是起始区域的搜索区域，一次是目标位置的区域： 我们只需选择左上角和右上角作为我们的搜索区域：
这种方法非常简单，当然还有更复杂的解决方案。例如，我们可以尝试根据物体的形状或颜色来检测物体。 5. 将 2D 像素转换为世界帧中的 3D 位置 让我们花点时间拍拍自己的肩膀。我们成功获得了2D图像中物体的x和y坐标！但是这些与机器人有什么关系呢？最后，我们希望向机器人控制一个3D位置，它应该在其中拾取或放置物体。因此，在将位置发送到拾取和放置节点之前，我们需要将 2D 图像中的像素位置转换为机器人框架中的 3D 位置。这需要两个步骤：
- 我们使用点云数据从 3D 像素信息中获取相机帧中的 2D 位置。
- 我们使用 tf3 包将相机帧中的 2D 位置转换为机器人帧。
为了获取相机帧中的 3D 位置，我们需要使用 Kinect 深度相机的点云数据。因此，我们在主函数中订阅它： 定义为 。我们获取数据的每个样本，我们的回调函数被调用。让我们立即看一下此函数的第一部分：POINT_CLOUD2_TOPIC"/camera1/depth/points"point_cloud_cb
当我们收到一个新的点云时，我们调用该函数两次，然后打印出结果。此函数将 2D 像素转换为相机帧中的 3D 位置，如下所示：pixel_to_3d_point 我们需要在与我们的 2D 图像像素相关的点云数据中找到深度信息。数据存储在一个数组中，我们必须找到正确的元素。该参数为我们提供了一行中的字节数，同时告诉我们一个点的字节数。pCloud.row_steppCloud.point_step 为了计算我们在第 154 行中的数组位置，我们首先将变量 v 乘以像素位置的 y 坐标。这意味着在数组中，我们向前跳转到包含像素的行开始的位置。
然后，我们将 u（像素的 x 坐标）乘以得到 .pCloud.row_steppCloud.point_steparrayPosition 每个 3D 点由三个坐标组成。这些坐标中的每一个都是存储在我们的字节数组中的浮点值。一个浮点数通常有 4 个字节，因此 y 坐标的偏移量为 4，因为它存储在 x 坐标之后。每个坐标的确切偏移量存储在数组中，在第 157 到 159 行中，我们将该偏移值添加到每个坐标的 中。现在我们将浮点数中的字节数从数据数组中的正确位置复制到坐标。然后我们把它们存储在我们的变量中。pCloud.fieldsarrayPositiongeometry_msgs::Pointp
在函数的第二部分中，我们将 3D 位置从相机帧转换为机器人（基础）帧：point_cloud_cb 幸运的是，有 tf2 转换库，它使帧之间的转换变得容易。在使用转换功能之前，我们包含一些库标头： 然后我们创建一个对象：tf_buffer 在 main 函数中，我们创建一个将对象作为参数的转换：listenertf_buffer 现在，我们已准备好使用转换功能。
让我们看一下函数中的实现：transform_between_frames 该函数采用 3D 点和我们要转换的帧。变换函数需要类型（类似于点对象，但具有附加信息）作为输入。geometry_msgs::PoseStamped 我们只是通过添加有关帧和时间的信息来构建 3D 点。然后我们可以调用我们之前创建的对象的函数。它需要构造的，我们想要转换为的帧和一些超时值。结果我们得到转换后的输出姿势，也是类型 ，我们只返回位置。input_pose_stampedtransformtf_bufferinput_pose_stampedgeometry_msgs::PoseStamped 很酷，我们终于有一个机器人可以理解的3D位置。
6. 将结果仓位作为服务呼叫发送 请记住，我们执行所有这些操作是为了自动检测拾取和放置任务的起点和目标位置。因此，我们需要将结果传达给我们的拾取和放置节点。到目前为止，我们主要使用主题在节点之间交换数据，这对于多对多和单向通信非常有用。但是，只有拾取和放置节点需要在开始运动之前根据请求获取提取的位置一次。为此，服务比主题更适合。 我们命名我们的服务，并在 OpenCV 包的 srv 文件夹中创建以下 box_and_target_position.srv 文件：opencv_services/box_and_target_position
我们还需要在 CMakeLists.txt 文件中添加该服务，以确保创建 opencv_node/box_and_target_position.h 头文件： 将此标头包含在我们的.cpp文件中后，我们可以使用该服务。为了调用它，我们在函数中通告它：main 当另一个节点调用服务时，将调用该函数：get_box_and_target_position
在其中，我们只需将存储在全局和变量中的提取位置分配给我们发送回请求服务的节点的响应。box_position_base_frametarget_position_base_frame 呼，那可是很多工作啊！OpenCV 节点中需要完成大量步骤，但现在我们终于可以将其用于拾取和放置任务了。
如何从拾取和放置节点调用 OpenCV 节点服务 正如我在开头提到的，我们将使用与上一教程中几乎相同的拾取和放置实现，并且仅将硬编码的位置值替换为我们从 OpenCV 节点接收的值。在本教程中，我将仅讨论与上一教程相比我们需要添加的行。如果您对拾取和放置任务的确切实现方式感兴趣，可以学习最后两个教程（使用 Moveit C++ 界面的拾取和放置任务和如何使用深度相机与 Moveit 避免碰撞）。
您还可以在 Github 存储库中找到新的拾取和放置节点。 pick_and_place_opencv.cpp文件包含完整的实现，包括描述性注释。 要使用该服务，我们需要将opencv_node添加为 CMakeList 中的依赖项.txt： 在package.xml： 生成的服务标头需要包含在.cpp文件中： 在拾取和放置节点的主函数中，我们为服务构造一个服务客户端，并在第 79 行创建服务对象。现在我们调用服务，如果成功，则打印出存储在 中的接收位置。box_and_target_position_srv_clientbox_and_target_positionsrv.response 最后，我们将值而不是硬编码位置分配给我们的盒子姿势： 并针对目标姿势：
结论
就是这样，这就是如何将OpenCV与ROS一起使用，将计算机视觉集成到机器人应用程序中的方式。您已经看到，从相机图像中提取信息并将其转换为机器人任务的有用数据需要相当多的工作。本教程中使用的方法肯定不是最优雅或最健壮的方法，但它向您展示了一种从头到尾如何做到这一点的方法，这就是目标。
OpenCV是一个强大的库，有更多的功能可供发现。在本教程中，我有意使用了经典的计算机视觉算法，但越来越多的机器学习方法被用于现代计算机视觉。在接下来的教程中，我们将讨论如何使用OpenCV在机器人应用程序中采用机器学习方法。在那之前，感谢您一直坚持到最后，并确保查看机器人教程概述中的其他教程。与往常一样，我很高兴在评论中收到您的反馈！
参考：
ROS 教程：如何在计算机视觉的机器人拾取和放置任务中使用 OpenCV - 机器人休闲 (roboticscasual.com)