ROS2中的视觉图像应用
实验介绍
在机器人技术中，视觉传感器是实现机器人自主导航、目标检测和环境感知的重要组成部分。ROS2（Robot Operating System 2）作为一个强大的机器人开发框架，提供了丰富的工具和库来处理视觉数据。本次实验旨在通过ROS2实现图像视觉应用，具体包括图像数据的获取、颜色特征提取、目标定位、目标追踪以及人脸检测等功能。
实验过程
视觉图像数据的获取
编写图像数据获取程序
首先在工作空间中创建一个软件包。打开一个新的终端窗口，输入如下指令，进入工作空间。
cd ~/ros2_ws/src
然后用如下指令创建一个名为“cv_pkg”的软件包(“cv”是“Computer Vision”的简称)。
ros2 pkg create cv_pkg
创建好软件包后，接下来在这个软件包中创建一个节点。
1.编写节点代码
先创建节点的源码文件。在VSCode中找到[cv_pkg]软件包，用鼠标右键单击它的[src]子文件夹，在弹出的快捷菜单中选择[新建文件]。此时会提示输入文件名，这里输入“cv_image.cpp”，然后按[Enter]键创建文件。
下面编写这个源码文件，其内容如下。
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
std::shared_ptr<rclcpp::Node> node;
void CamRGBCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
cv::Mat imgOriginal = cv_ptr->image;
cv::imshow("RGB", imgOriginal);
cv::waitKey(1);
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("cv_image_node");
auto rgb_sub = node->create_subscription<sensor_msgs::msg::Image>(
"/kinect2/qhd/image_raw", 1, CamRGBCallback);
cv::namedWindow("RGB");
rclcpp::spin(node);
cv::destroyAllWindows();
rclcpp::shutdown();
return 0;
}
上述代码可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cpp\9_cv_image.cpp 文件中的代码进行比对。
代码编写完成后，需要进行保存。保存成功后，编辑界面文件名后面的圆点符号会变成一个叉符号。
2.设置编译规则
节点源代码的编译规则写在 cv_pkg的 CMakeLists.txt 文件里。在 VSCode中打开这个文件，在这个文件里添加节点的编译规则。首先使用如下代码寻找节点代码中用到的依赖项。
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
然后使用如下代码添加节点源代码的编译规则。
add_executable(cv_image src/cv_image.cpp)
ament_target_dependencies(cv_image "rclcpp" "sensor_msgs" "cv_bridge" "OpenCV")
最后使用如下代码添加节点编译完成后的安装规则。
install(TARGETS cv_image
DESTINATION lib/${PROJECT_NAME})
这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\9_cv_image.txt 文件中的代码进行比对。
上述规则添加完毕后，一定要保存文件，否则规则无法生效。
3.修改软件包信息
在 VSCode 中打开[ev_pkg]下的[package.xml]文件，使用如下代码添加依赖项信息。
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>OpenCV</depend>
这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_package\9_cv_image.xml文件中的代码进行比对。
文件修改完成后，一定要保存文件，否则新的包信息无法生效。
4.编译软件包
修改完上述文件后，打开终端窗口，执行如下指令，进入工作空间。
cd ~/ros2_ws
然后执行如下指令，对工作空间中的所有软件包进行编译
colcon build
仿真运行图像数据获取程序
下面运行刚编写的节点。在运行前，确认已经下载了wpr_simulation2 仿真项目。然后，加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件。保持终端的当前路径依然在工作空间目录ros2_ws中，然后执行如下指令。
source install/setup.bash
然后启动带有机器人的仿真环境，执行如下指令。
ros2 launch wpr_simulation2 wpb_balls.launch.py
这时会启动仿真环境，里面有一台虚拟机器人。机器人的面前放置了数个不同颜色的球，机器人的头部相机俯视着这些球。
接下来运行刚才编写的节点 cv_image，在 Terminator 终端中，按组合键[Ctrl+Shift+O]，将终端分为上下两个子窗口。在新的终端窗口中先执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，运行刚才编写的节点。
ros2 run cv_pkg cv_image
节点运行起来之后，会弹出一个[RGB]窗口，显示机器人头部相机所看到的4个不同颜色球的图像。
为了测试这个图像是不是实时获取的，可以借助wpr_simulation2附带的程序让中间的橘色球动起来，以便进行对比观察。在Terminator 终端中，按组合键[Ctrl+Shift+O]，将分出第3个子窗口。在新的终端窗口中先执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行小球运动指令。
ros2 run wpr_simulation2 ball_random_move
执行之后，可以看到仿真窗口中的橘色球开始随机运动。
此时再切换到[RGB]窗口，可以看到图像中的橘色球也跟着运动说明这个采集到的图像是实时更新的。
OpenCV颜色特征提取和目标定位
在上面的实验里，实现了从机器人的头部相机获取机器人的视觉图像。这一次将继续深人，使用 0penCV实现机器人视觉中的颜色特征提取和目标定位功能。在编写代码前，先设计一下这个程序的实现思路:
1)对机器人视觉图像进行颜色空间转换，从RGB空间转换到HSV空间，排除光照影响
2)对转换后的图像进行二值化处理，将目标物分割提取出来。
3)对提取到的目标像素进行计算统计，得出目标物的质心坐标。
编写颜色特征提取和目标定位程序
首先在工作空间中创建一个名为“cv_pkg”的软件包，如果前面已经创建过，这里可以直接跳过。打开一个新的终端窗口，输入如下指令，进入工作空间。
cd ~/ros2_ws/src
然后执行如下指令，创建软件包
ros2 pkg create cv_pkg
创建好软件包后，接下来在这个软件包中创建一个节点。
1.编写节点代码
先创建这个节点的源代码文件。在VSCode中找到[ev_pkg]软件包用鼠标右键单击它的[src]子目录，在弹出的快捷菜单中选择[新建文件]。
此时会提示输人文件名，这里输入“cv_hsv.cpp”，然后按[Enter]键创建文件。
下面编写这个源代码文件，其内容如下。
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
std::shared_ptr<rclcpp::Node> node;
using namespace cv;
using namespace std;
static int iLowH = 10;
static int iHighH = 40;
static int iLowS = 90;
static int iHighS = 255;
static int iLowV = 1;
static int iHighV = 255;
void CamRGBCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
Mat imgOriginal = cv_ptr->image;
Mat imgHSV;
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
vector<Mat> hsvSplit;
split(imgHSV, hsvSplit);
equalizeHist(hsvSplit[2], hsvSplit[2]);
merge(hsvSplit, imgHSV);
Mat imgThresholded;
inRange(imgHSV,
Scalar(iLowH, iLowS, iLowV),
Scalar(iHighH, iHighS, iHighV),
imgThresholded);
Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);
morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);
int nTargetX = 0;
int nTargetY = 0;
int nPixCount = 0;
int nImgWidth = imgThresholded.cols;
int nImgHeight = imgThresholded.rows;
for (int y = 0; y < nImgHeight; y++)
{
for (int x = 0; x < nImgWidth; x++)
{
if (imgThresholded.data[y * nImgWidth + x] == 255)
{
nTargetX += x;
nTargetY += y;
nPixCount++;
}
}
}
if (nPixCount > 0)
{
nTargetX /= nPixCount;
nTargetY /= nPixCount;
printf("Target (%d, %d) PixelCount = %d\n", nTargetX, nTargetY, nPixCount);
Point line_begin = Point(nTargetX - 10, nTargetY);
Point line_end = Point(nTargetX + 10, nTargetY);
line(imgOriginal, line_begin, line_end, Scalar(255, 0, 0));
line_begin.x = nTargetX; line_begin.y = nTargetY - 10;
line_end.x = nTargetX; line_end.y = nTargetY + 10;
line(imgOriginal, line_begin, line_end, Scalar(255, 0, 0));
}
else
{
printf("Target disappeared...\n");
}
imshow("RGB", imgOriginal);
imshow("HSV", imgHSV);
imshow("Result", imgThresholded);
cv::waitKey(5);
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("cv_hsv_node");
auto rgb_sub = node->create_subscription<sensor_msgs::msg::Image>(
"/kinect2/qhd/image_raw", 1, CamRGBCallback);
namedWindow("Threshold", WINDOW_AUTOSIZE);
createTrackbar("LowH", "Threshold", &iLowH, 179);
createTrackbar("HighH", "Threshold", &iHighH, 179);
createTrackbar("LowS", "Threshold", &iLowS, 255);
createTrackbar("HighS", "Threshold", &iHighS, 255);
createTrackbar("LowV", "Threshold", &iLowV, 255);
createTrackbar("HighV", "Threshold", &iHighV, 255);
namedWindow("RGB");
namedWindow("HSV");
namedWindow("Result");
rclcpp::spin(node);
cv::destroyAllWindows();
rclcpp::shutdown();
return 0;
}
上述代码可以从 wpr_simulation2 的例程文件中找到。如果编译出错，可以与 wpr_simulation2\demo_cpp\9_cv_hsv.cpp 文件中的代码进行比对。
代码编写完成后，需要进行保存。保存成功后，编辑界面文件名后面的圆点符号会变成一个叉符号。
2.设置编译规则
节点源代码的编译规则写在cv_pkg的CMakeLists.txt 文件里。在VSCode 中打开这个文件，在这个文件里添加节点的编译规则。首先使用如下代码寻找节点代码中用到的依赖项。
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
然后使用如下代码添加节点源代码的编译规则。
add_executable(cv_hsv src/cv_hsv.cpp)
ament_target_dependencies(cv_hsv "rclcpp" "sensor_msgs" "cv_bridge" "OpenCV")
最后使用如下代码添加节点编译完成后的安装规则。
install(TARGETS cv_hsv
DESTINATION lib/${PROJECT_NAME})
这些内容可以从 wpr_simulation2的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\9_cv_hsv.txt 文件中的内容进行比对。
上述规则添加完毕后，一定要保存文件，否则编译规则无法生效，
3.修改软件包信息
在 VSCode 中打开[cv_pkg]下的[package.xml]文件，使用如下代码添加依赖项信息。
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>OpenCV</depend>
这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_package\9_cv_hsv.xml 文件中的内容进行比对。
文件修改完成后，一定要保存文件，否则新的包信息无法生效。
4.编译软件包
修改完上述文件后，打开终端窗口。执行如下指令，进入工作空间。
cd ~/ros2_ws
然后执行如下指令，对工作空间中的所有软件包进行编译
colcon build
仿真运行颜色特征提取和目标定位程序
下面运行刚编写的节点。在运行前，确认已经下载了wpr_simulation2仿真项目。然后，需要加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件。保持终端的当前路径依然在工作空间目录ros2_ws中，然后执行如下指令。
source install/setup.bash
然后执行如下指令，启动带有机器人的仿真环境
ros2 launch wpr_simulation2 wpb_balls.launch.py
这时会启动仿真窗口，里面有一台虚拟机器人。机器人的面前放置了数个不同颜色的机器人的头部相机俯视着这些球。
接下来运行刚才编写的节点cv_hsv。在Terminator 终端中，按组合键[Ctrl+Shift+O]，将终端分为上、下两个子窗口。在新的终端窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，运行刚编写的节点。
ros2 run cv_pkg cv_hsv
节点运行起来之后，会弹出以下4个窗口。
1)[RGB]窗口，显示的是机器人头部相机所看到的4个不同颜色球的图像。
2)[HSV]窗口，显示的是转换后的 HSV 图像。
3)[Result] 窗口，显示的是转换颜色描述并二值化后的结果。白色的部分是检测到目标物的像素区域，黑色的部分是被剔除掉的非目标物的像素区域。可以看到,目前是将橘色球作为追踪目标。
4)[Threshold]窗口，显示的是当前使用的 HSV 颜色阈值数值。可以直接拖动窗口中的滑块来改变阈值大小，在其他窗口中会实时显示阈值变化的效果。
切换到运行cv_hsv节点的终端窗口，可以看到追踪的目标物的中心坐标值。
机器人使用的相机是Kinect V2，它的 QHD图像分辨率是 960x540。颜色质心的坐标原点在图像的左上角，对照终端显示的质心坐标值和前面RGB图像中绘出的目标位置(蓝色十字标记)，可以看到最后计算的目标物质心坐标和图像显示结果大致相同。为了测试目标追踪的效果，可以借助wpr_simulation2附带的程序节点让中间的橘色球动起来。在Terminator 终端中，按组合键[Ctrl+Shift+O]，将分出第3个子窗口。在新窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后输入如下指令，启动球随机运动的节点。
ros2 run wpr_simulation2 ball_random_move
执行之后，可以看到仿真窗口中的橘色球开始随机运动
此时再切换到[Result]窗口，观察图像中橘色目标球移动时，颜色特征提取的效果。
基于视觉图像的目标追踪实现
在编写例程代码前，先设计一下这个程序的实现思路:
1)对机器人视觉图像进行颜色空间转换，从RGB 空间转换到 HSV 空间，排除光照影响。
2)对转换后的图像进行二值化处理，将目标物分割提取出来。
3)对提取到的目标像素进行计算统计，得出目标物的质心坐标。
4)根据目标位置计算机器人运动速度，完成目标追踪功能。
编写目标追踪程序
首先在工作空间中创建一个名为“cv_pkg”的软件包，如果前面已经创建过，这里可以直接跳过，打开一个新的终端窗口，输入如下指令，进入工作空间。
cd ~/ros2_ws/src
然后执行如下指令，创建软件包。
ros2 pkg create cv_pkg
创建好软件包后，接下来在这个软件包中创建一个节点。
1.编写节点代码
先创建这个节点的源码文件，在VSCode 中找到[cv_pkg]软件包,用鼠标右键单击它的[src]子目录，在弹出的快捷菜单中选择[新建文件…]，此时会提示输人文件名，这里输人“cv_follow.cpp”，然后按[Enter]键创建文件。
下面编写这个源码文件，其内容如下。
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <geometry_msgs/msg/twist.hpp>
std::shared_ptr<rclcpp::Node> node;
using namespace cv;
using namespace std;
static int iLowH = 10;
static int iHighH = 40;
static int iLowS = 90;
static int iHighS = 255;
static int iLowV = 1;
static int iHighV = 255;
geometry_msgs::msg::Twist vel_cmd;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub;
void Cam_RGB_Callback(const sensor_msgs::msg::Image::SharedPtr msg)
{
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
Mat imgOriginal = cv_ptr->image;
Mat imgHSV;
vector<Mat> hsvSplit;
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
split(imgHSV, hsvSplit);
equalizeHist(hsvSplit[2],hsvSplit[2]);
merge(hsvSplit,imgHSV);
Mat imgThresholded;
inRange(imgHSV,
Scalar(iLowH, iLowS, iLowV),
Scalar(iHighH, iHighS, iHighV),
imgThresholded);
Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);
morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);
int nTargetX = 0;
int nTargetY = 0;
int nPixCount = 0;
int nImgWidth = imgThresholded.cols;
int nImgHeight = imgThresholded.rows;
for (int y = 0; y < nImgHeight; y++)
{
for(int x = 0; x < nImgWidth; x++)
{
if(imgThresholded.data[y*nImgWidth + x] == 255)
{
nTargetX += x;
nTargetY += y;
nPixCount ++;
}
}
}
if(nPixCount > 0)
{
nTargetX /= nPixCount;
nTargetY /= nPixCount;
printf("Target ( %d , %d ) PixelCount = %d\n",nTargetX,nTargetY,nPixCount);
Point line_begin = Point(nTargetX-10,nTargetY);
Point line_end = Point(nTargetX+10,nTargetY);
line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);
line_begin.x = nTargetX; line_begin.y = nTargetY-10;
line_end.x = nTargetX; line_end.y = nTargetY+10;
line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);
float fVelFoward = (nImgHeight/2-nTargetY)*0.002;
float fVelTurn = (nImgWidth/2-nTargetX)*0.003;
vel_cmd.linear.x = fVelFoward;
vel_cmd.linear.y = 0;
vel_cmd.linear.z = 0;
vel_cmd.angular.x = 0;
vel_cmd.angular.y = 0;
vel_cmd.angular.z = fVelTurn;
}
else
{
printf("Target disappeared...\n");
vel_cmd.linear.x = 0;
vel_cmd.linear.y = 0;
vel_cmd.linear.z = 0;
vel_cmd.angular.x = 0;
vel_cmd.angular.y = 0;
vel_cmd.angular.z = 0;
}
vel_pub->publish(vel_cmd);
imshow("Result", imgThresholded);
imshow("RGB ", imgOriginal);
cv::waitKey(5);
}
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("cv_follow_node");
vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
auto sub = node->create_subscription<sensor_msgs::msg::Image>(
"/kinect2/qhd/image_raw",
10,
Cam_RGB_Callback);
namedWindow("RGB");
namedWindow("Result");
rclcpp::spin(node);
cv::destroyAllWindows();
rclcpp::shutdown();
return 0;
}
上述代码可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cpp\9_ev_follow.epp 文件中的代码进行比对。
代码编写完成后，需要进行保存。保存成功后，编辑界面文件名后面的圆点符号会变成一个叉符号。
2.设置编译规则
节点源代码的编译规则写在cv_pkg的CMakeLists.txt 文件里，在VSCode中打开这个文件，在这个文件里添加节点的编译规则。首先使用如下代码寻找节点代码中用到的依赖项。
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
find_package(geometry_msgs REQUIRED)
然后使用如下代码添加节点源代码的编译规则。
add_executable(cv_follow src/cv_follow.cpp)
ament_target_dependencies(cv_follow
"rclcpp" "sensor_msgs" "cv_bridge" "OpenCV" "geometry_msgs")
最后使用如下代码添加节点编译完成后的安装规则。
install(TARGETS cv_follow
DESTINATION lib/${PROJECT_NAME})
这些内容可以从 wpr simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\9_cv follow.txt 文件中的代码进行比对。
上述规则添加完毕后，一定要保存文件，否则规则无法生效。
3.修改软件包信息
在VSCode中打开[cv_pkg]下的[package.xml]文件，使用如下代码添加依赖项信息。
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>OpenCV</depend>
<depend>geometry_msgs</depend>
这些内容可以从 wpr_simulation2的例程文件中找到。如果编译报错，可以与wpr_simulation2\demo_package\9_cv_follow.xml文件中的代码进行比对。
文件修改后，一定要保存文件，否则新的包信息无法生效。
4.编译软件包
修改完上述文件后，打开终端窗口，执行如下指令，进入工作空间
cd ~/ros2_ws
然后执行如下指令，对工作空间中的所有软件包进行编译
colcon build
仿真运行目标追踪程序
下面运行刚编写的节点。在运行前，确认已经下载了wpr_simulation2 仿真项目。然后，需要加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件。保持终端的当前路径依然在工作空间目录ros2_ws中，然后执行如下指令。
source install/setup.bash
然后先执行如下指令，启动带有机器人的仿真环境
ros2 launch wpr_simulation2 wpb_balls.launch.py
这时会启动仿真窗口，里面有一台虚拟机器人。机器人的面前放置了数个不同颜色的球，机器人的头部相机俯视着这些球。
接下来运行刚才编写的节点 cv_hsv。在 Terminator 终端中，按组合键[Ctrl+Shift+O],将终端分为上、下两个子窗口，先在新的终端窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，运行刚才编写的节点。
ros2 run cv_pkg cv_follow
节点运行起来之后，会弹出以下两个窗口。
1)[RGB]窗口，显示的是机器人头部相机所看到的4个不同颜色球的图像。
2)[Result]窗口，显示的是转换颜色描述并二值化后的结果。白色的部分是检测到目标物的像素区域，黑色的部分是被剔除掉的非目标物的像素区域。可以看到，目前是将橘色球作为追踪目标。
切换到仿真窗口，可以看到里面的机器人对准橘色球，轻微向后移动，与橘色球保持固定距离。在运行cv_follow节点的终端窗口，可以看到追踪目标物的中心坐标值在不停地刷新。
为了测试机器人追踪目标球的效果，可以借助wpr_simulation2 附带的程序让橘色球动起来在 Terminator 终端中，按组合键[Ctrl+Shift+O]，将分出第3个子窗口。在新窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
新的窗口加载环境设置后，输入如下指令，启动球随机运动的节点.
ros2 run wpr_simulation2 ball_random_move
执行之后，可以看到仿真窗口中的橘色球开始随机运动，而机器人也追着橘色球在移动。
此时再切换到[RGB]窗口，观察蓝色十字标记对橘色目标球的追踪效果。
经过 前面3个实验，终于将识别检测和运动行为结合起来，形成了一个典型的视觉闭环控制系统。机器人与外部世界的交互，形式虽然多样，但是本质上都是这样一套“识别一定位一操作”的闭环控制系统。通过这样一个简单的例子，了解和学习这种实现思路，可以为将来构建更复杂的机器人系统奠定基础。
基于视觉图像的人脸检测实现
在前面的实验中，使用的都是颜色特征进行视觉识别。在视觉的应用中，还有使用图形特征进行识别的案例。这一节将来体验一下使用图形学特征进行人脸检测的效果。当然，对于初学者来说，从零开始编写一个人脸检测算法过于复杂，所以需要借助现成的人脸识别算法库来实现人脸检测。这个算法库的调用在wpr_simulaiton2 的face_detector.py节点中已经实现，直接使用即可。
face_detector.py节点会订阅话题“/face_detector_input”，作为人脸图像的输人。图像中的人脸被检测到后，其坐标值会被发布到话题“/face_position”中。所以，这个实验只需要编写一个节点，从相机的话题中获取图片，转发给face_detector. py 节点进行人脸检测，然后从“/face_position”话题获取人脸坐标结果即可。
在编写代码前，需要安装人脸检测节点face_detector.py的依赖项。打开一个新的终端窗口，输入如下指令，进入依赖项安装目录。
cd ~/ros2_ws/src/wpr_simulation2/scripts/
然后执行如下指令，安装人脸检测节点的依赖项。
./install_dep_face.sh
因为网络的原因，这些依赖项的安装时间会比较长，需要耐心等待。
编写人脸检测程序
首先在工作空间中创建一个名为“cv_pkg”的软件包，如果前面已经创建过，这里可以直接跳过。打开一个新的终端窗口，输入如下指令，进入工作空间。
cd ~/ros2_ws/src
然后执行如下指令，创建软件包。
ros2 pkg create cv_pkg
创建好软件包后，接下来在这个软件包中创建一个节点。
1.编写节点代码
先创建这个节点的源码文件。在VSCode中找到[ev_pkg]软件包，用鼠标右键单击它的[src]子目录，在弹出的快捷菜单中选择[新建文件]。此时会提示输人文件名，这里输入“cv_face_detect.cpp”，然后按[Enter] 键创建文件。
下面编写这个源码文件，其内容如下。
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/region_of_interest.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
std::shared_ptr<rclcpp::Node> node;
cv::Mat imgFace;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr frame_pub;
void CamRGBCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
imgFace = cv_ptr->image;
frame_pub->publish(*msg);
}
void FacePosCallback(const sensor_msgs::msg::RegionOfInterest::SharedPtr msg)
{
cv::rectangle(imgFace,
cv::Point(msg->x_offset, msg->y_offset),
cv::Point(msg->x_offset + msg->width, msg->y_offset + msg->height),
cv::Scalar(0, 0, 255),
2,
cv::LINE_8);
cv::imshow("Face", imgFace);
cv::waitKey(1);
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("cv_face_detect");
auto rgb_sub = node->create_subscription<sensor_msgs::msg::Image>(
"/kinect2/qhd/image_raw", 1, CamRGBCallback);
frame_pub = node->create_publisher<sensor_msgs::msg::Image>(
"/face_detector_input", 1);
auto face_sub = node->create_subscription<sensor_msgs::msg::RegionOfInterest>(
"/face_position", 1, FacePosCallback);
cv::namedWindow("Face");
rclcpp::spin(node);
cv::destroyAllWindows();
rclcpp::shutdown();
return 0;
}
这些内容可以从 wpr_simulation2的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\9_cv face_detect.txt 文件中的代码进行比对。
上述规则添加完毕后，一定要保存文件，否则规则无法生效。
2.设置编译规则
节点源代码的编译规则写在cv_pkg的CMakeLists.txt 文件里。在VSCode 中打开这个文件，在这个文件里添加节点的编译规则。首先使用如下代码寻找节点代码中用到的依赖项。
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
然后使用如下代码添加节点源代码的编译规则。
add_executable(cv_face_detect src/cv_face_detect.cpp)
ament_target_dependencies(cv_face_detect "rclcpp" "sensor_msgs" "cv_bridge" "OpenCV")
最后使用如下代码添加节点编译完成后的安装规则。
install(TARGETS cv_face_detect
DESTINATION lib/${PROJECT_NAME})
这些内容可以从 wpr simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\9_cv_face_detect.txt 文件中的代码进行比对。
上述规则添加完毕后，一定要保存文件，否则规则无法生效。
3.修改软件包信息
在VSCode 中打开[cv_pkg]下的[package.xml]文件，使用如下代码添加依赖项信息。
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>OpenCV</depend>
这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simule
tion2\demo_package\9_cv_face_detect. xml文件中的代码进行比对。
文件修改后，一定要保存文件，否则新的包信息无法生效。
4.编译软件包
修改完上述文件后，打开终端窗口。执行如下指令，进入工作空间。
cd ~/ros2_ws
然后执行如下指令，对工作空间中的所有软件包进行编译。
colcon build
仿真运行人脸检测程序
下面运行刚编写的节点。在运行前，确认已经下载了wpr_simulation2 仿真项目。然后，需要加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件。保持终端的当前路径依然在工作空间目录ros2_ws中，然后执行如下指令。
source install/setup.bash
然后执行如下指令，启动带有机器人的仿真环境。
ros2 launch wpr_simulation2 wpb_face.launch.py
这时会启动仿真窗口，里面有一台虚拟机器人，机器人的面前站着一位虚拟人模特。
接下来运行 wpr_simulaiton2 自带的人脸检测服务节点face_detector.py。在 Terminator 终端中，按组合键[Ctrl+Shift+O]，将终端分为上、下两个子窗口。先在新的终端窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，运行人脸检测服务节点face_detector.py。
ros2 run wpr_simulation2 face_detector.py
最后运行刚才编写的 cv_face_detect 节点。在 Terminator 终端中，按组合键[Ctrl+Shift+O]，从终端分出第3个子窗口。在新的终端窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，运行刚才编写的cv_face_detect 节点。
ros2 run cv_pkg cv_face_detect
节点运行起来之后，会弹出一个[Face]窗口，显示的是机器人头部相机所看到的实时图像。当图像中出现人脸时，会有一个红色的矩形框把人脸的位置标注在这个图像中。
此时还可以借助 wpr_simulation2 附带的键盘控制程序让机器人运动起来，通过不同视角去测人脸识别的鲁棒性。在 Terminator 终端中，再次按组合键[Ctrl+Shift+O]这时会分出第4个子窗口。在第4个窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，启动键盘控制节点。
ros2 run wpr_simulation2 keyboard_vel_cmd
保持 Terminator 终端窗口位于所有窗口的前边，且第4个窗口的标题栏为红色，这样才能让键盘控制节点始终能够接收到键盘按下的信号。
实验总结
在本次实验中，我们通过ROS2框架深入学习了图像视觉应用的实现，包括图像数据获取、颜色特征提取与目标定位、目标追踪以及人脸检测。我们成功创建了多个ROS2节点，实现了从图像数据的实时获取到目标物体的颜色特征提取和精确定位，再到基于视觉反馈的动态目标追踪，最后利用预训练的人脸识别模型实现了人脸检测与标注。通过这些实验，我们不仅掌握了ROS2和OpenCV的基本使用方法，还提高了问题解决能力和系统集成经验，为未来开发更复杂的机器人视觉应用奠定了坚实的基础。
扫一扫
395
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
