ROS开发专栏---基于图像视觉的目标追踪实验--适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ：《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ：《freertos专栏》《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》《Ai智能体专栏）《ROS开发专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

前言

一、实验原理与整体实现思路

二、编写目标追踪节点源码

[三、配置 CMakeLists.txt](#三、配置 CMakeLists.txt)

前言

上两期博客我们已经完成相机图像采集、OpenCV 颜色特征提取与目标定位实验，已经可以精准识别仿真环境中的橙色小球并计算像素质心。

本期实验在此基础上进一步升级，把视觉识别和机器人运动控制结合，通过目标像素偏移量换算底盘线速度、角速度，自动发布 /cmd_vel 速度指令，实现机器人跟随橙色小球自动移动，完成视觉识别→定位→运动控制的闭环控制系统。

一、实验原理与整体实现思路

本实验核心思路分为四步：

订阅 Kinect 相机图像话题，将 ROS 图像转为 OpenCV 格式；

RGB 转 HSV 颜色空间，通过阈值分割提取橙色目标区域，结合形态学滤波去除噪点；

遍历目标像素计算几何质心，获取目标在画面中的坐标位置；

根据目标相对画面中心的偏移差值，换算机器人前后、旋转运动速度，发布速度指令实现自动跟随。如下图所示：

二、编写目标追踪节点源码

在cv_pkg/src目录下新建cv_follow.cpp，代码继承前面颜色识别逻辑，新增速度发布与运动解算功能。全部代码如下：

cpp 复制代码

#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;

// 橙色小球HSV阈值
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 CamRGBCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
    // ROS图像转OpenCV格式
    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);

    // HSV阈值分割提取橙色
    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.angular.z = fVelTurn;
    }
    else
    {
        // 丢失目标原地停止
        printf("Target disappeared...\n");
        vel_cmd.linear.x = 0;
        vel_cmd.angular.z = 0;
    }

    // 发布速度指令
    vel_pub->publish(vel_cmd);

    // 窗口显示
    imshow("RGB ", imgOriginal);
    imshow("Result", imgThresholded);
    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", 1, CamRGBCallback);

    // 初始化窗口
    namedWindow("RGB");
    namedWindow("Result");

    rclcpp::spin(node);
    cv::destroyAllWindows();
    rclcpp::shutdown();
    return 0;
}

三、配置 CMakeLists.txt

打开cv_pkg的CMakeLists.txt，追加本次节点编译配置，新增geometry_msgs依赖支持速度消息：

cpp 复制代码

cmake_minimum_required(VERSION 3.8)
project(cv_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 声明依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)

# 编译 cv_image.cpp，生成可执行节点 cv_image
add_executable(cv_image src/cv_image.cpp)
ament_target_dependencies(cv_image
  rclcpp
  sensor_msgs
  cv_bridge
  OpenCV
)

# 编译颜色识别节点
add_executable(cv_hsv src/cv_hsv.cpp)
ament_target_dependencies(cv_hsv
  rclcpp
  sensor_msgs
  cv_bridge
  OpenCV
)


# 编译目标追踪节点
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})




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

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

ament_package()

四、配置 package.xml

在原有依赖基础上，新增几何消息依赖，全部代码如下：

cpp 复制代码

<?xml version="1.0"?>
<package format="3">
  <name>cv_pkg</name>
  <version>0.0.0</version>
  <description>ROS2视觉功能包</description>
  <maintainer email="todo@todo.todo">todo</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>cv_bridge</depend>
  <depend>OpenCV</depend>
  <depend>geometry_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

四、编译与仿真完整运行

4.1、编译功能包

首先我们先编译功能包，打开终端，输入指令如下：

cpp 复制代码

cd ~/ros2_zice
colcon build --packages-select cv_pkg
source install/setup.bash

4.2、运行程序

1、启动Gazebo仿真

打开终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 launch wpr_simulation2 wpb_balls.launch.py

2、启动目标追踪节点

新建终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 run cv_pkg cv_follow

启动后会弹出两个 OpenCV 窗口（RGB 和 Result），如下图所示：

切换到仿真窗口，可以看到里面的机器人对准桔色球，轻微向后移动，与桔色球保持固定距离。如图所示，在运行cv_follow节点的终端窗口，可以看到追踪目标物的中心坐标值在不停地刷新。

如下图所示：

3、启动小球随机移动

为了测试机器人追踪目标球的效果，可以借助wpr_simulation2附带的程序让桔色球动起来。以便进行观察。打开第3个终端子窗口。输入以下指令：

cpp 复制代码

cd ~/ros2_zice
source install/setup.bash
ros2 run wpr_simulation2 ball_random_move

执行之后，可以看到仿真窗口里的桔色球开始随机运动，而机器人也追着桔色球在移动。如下图所示：

此时再切换到"RGB"窗口，观察蓝色十字标记是对桔色目标球的追踪效果，如下图所示：

总结

本期博客在颜色识别定位的基础上，搭建了视觉感知→坐标解算→运动控制的完整闭环系统。通过像素偏移量换算底盘速度，实现机器人对橙色小球的智能跟随，完整掌握 ROS2+OpenCV 视觉与底盘联动开发思路，也为后续智能巡检、视觉避障等复杂机器人应用打下坚实基础。