ROS：两个节点分别实现图像的发布与订阅功能，以及在其中一个节点实现图像的预处理

Skyblueee_

已于 2024-05-16 10:05:58 修改

阅读量1.4k

点赞数 4

分类专栏： ROS的应用文章标签： opencv c++ 计算机视觉 ubuntu

于 2021-09-18 22:16:17 首次发布

本文链接：https://blog.csdn.net/qq978744267/article/details/120364893

版权

ROS的应用专栏收录该内容

2 篇文章 1 订阅

订阅专栏

在ROS中，两个节点分别实现图像的发布与订阅功能，以及在其中一个节点实现图像的预处理

文章目录

- 在ROS中，两个节点分别实现图像的发布与订阅功能，以及在其中一个节点实现图像的预处理
前言
一、功能实现步骤详情
二、节点工作环境
三、两节点代码详情
- 1.pic2msg.cpp
- 2.msg2video.cpp
四、编译，并运行节点
五、参考资料

前言

最近正在学校实训，学习的是智能机器人综合设计，这周的任务量是两节点图像的发布与订阅，然后是图像预处理。
环境为ubuntu14.04

一、功能实现步骤详情

（1）创建两个节点，pic2msg 和 video2msg1；

     第一个节点pic2msg在代码pic2msg.cpp 话题（topic）中发布图像message;
     第二个节点msg2video在代码msg2video.cpp话题（topic）中订阅图像message；

（2）节点msg2video把订阅的图片信息（message）转换为opencv格式的图像进行预处理，之后节点msg2video把预处理后的图片信息（message）在话题（topic）msg2video.cpp下发布；

（3）pic2msg.cpp订阅video2msg1.cpp中预处理后的图像信息；

二、节点工作环境

在工作空间～home/catkin_ws/src下新建文件夹the_image_transport，在此文件夹下新建src（文件夹）、CMakeList.cpp（构建），package.xml（依赖），如图所示：
在这里插入图片描述

1.src文件夹下的文件

两个节点pic2msg 和 video2msg1，1.png预处理的图片。
在这里插入图片描述

2.CMakeLists.txt

代码如下（示例）：

cmake_minimum_required(VERSION 2.8.3)
project(the_image_transport)

## Add support for C++11, supported in ROS Kinetic and newer
# add_definitions(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  image_transport
)

## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES the_image_transport
#  CATKIN_DEPENDS cv_bridge image_transport
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/the_image_transport.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/the_image_transport_node.cpp)
add_executable(${PROJECT_NAME}_video2msg src/video2msg.cpp) #摄像头转化为图片
add_executable(${PROJECT_NAME}_msg2video src/msg2video.cpp) #得到传感器的topic
add_executable(${PROJECT_NAME}_msg2video1 src/msg2video1.cpp) #得到传感器的topic
add_executable(${PROJECT_NAME}_pic2msg src/pic2msg.cpp)     

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )
target_link_libraries(${PROJECT_NAME}_video2msg
   ${catkin_LIBRARIES}
 )
target_link_libraries(${PROJECT_NAME}_msg2video
   ${catkin_LIBRARIES}
 )
target_link_libraries(${PROJECT_NAME}_msg2video1
   ${catkin_LIBRARIES}
 )
target_link_libraries(${PROJECT_NAME}_pic2msg
   ${catkin_LIBRARIES}
 )

3.package.xml

代码如下（示例）：

<?xml version="1.0"?>
<package>
  <name>the_image_transport</name>
  <version>0.0.0</version>
  <description>The the_image_transport package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="ros@todo.todo">ros</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>
  
  <!--   <test_depend>gtest</test_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  <run_depend>cv_bridge</run_depend>
  <run_depend>image_transport</run_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

三、两节点代码详情

1.pic2msg.cpp

代码如下（示例）：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
 
void imageCallback(const sensor_msgs::ImageConstPtr& tem_msg)
{
  try
  {
    cv::imshow("canny image->pub", cv_bridge::toCvShare(tem_msg, "bgr8")->image);
    cv::waitKey(30);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", tem_msg->encoding.c_str());
  }
}
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_node_a");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("camera/image", 1);
  image_transport::Subscriber sub = it.subscribe("camera/rgb/image_raw",1,imageCallback);
 
  //cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
  cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
  cv::waitKey(30);
  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
 
  ros::Rate loop_rate(5);
  while (nh.ok()) {
    pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
}

2.msg2video.cpp

代码如下（示例）：

#include "ros/ros.h"  
#include "image_transport/image_transport.h"  
#include "cv_bridge/cv_bridge.h"  
#include "sensor_msgs/image_encodings.h"  
#include <opencv2/imgproc/imgproc.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <iostream>  
#include <cstring> //std::string std::to_string

using namespace std;
using namespace cv;
    
namespace enc = sensor_msgs::image_encodings;  
        
class ImageConvertor  
{  
  ros::NodeHandle nh_;  
  image_transport::ImageTransport it_;  
  image_transport::Subscriber image_sub_;  
  image_transport::Publisher image_pub_;  
          
public:  
    ImageConvertor():it_(nh_)
    {  
      //发布话题out  
      image_pub_ = it_.advertise("camera/rgb/image_raw", 1); 
      //订阅话题camera/image
      image_sub_ = it_.subscribe("camera/image", 1, &ImageConvertor::ImageCb, this);  
       //cv::namedWindow(OUT_WINDOW, CV_WINDOW_AUTOSIZE);  
       //cv::namedWindow(IN_WINDOW, CV_WINDOW_AUTOSIZE);  
     }  
      
    ~ImageConvertor()  
    {  
        //cv::destroyWindow(IN_WINDOW);  
        //cv::destroyWindow(OUT_WINDOW);  
    }  
      
    void ImageCb(const sensor_msgs::ImageConstPtr& msg)  
    {  
      cv_bridge::CvImagePtr cv_ptr;  
      try
      {  
         /*转化成CVImage*/  
          cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);  
          cv::imshow("pub->sub",cv_ptr->image);
          cv::waitKey(30);
       }  
      
       catch (cv_bridge::Exception& e)  
       {  
           ROS_ERROR("cv_bridge exception is %s", e.what());  
           return;  
       }  
      
     // Draw an example circle on the video stream
     if (cv_ptr->image.rows > 40 && cv_ptr->image.cols > 60)
      {
 
	hello(cv_ptr->image);
    	image_pub_.publish(cv_ptr->toImageMsg());
 
      }  
   }
    //OpenCV的边缘检测程序
    void detect_edges(cv::Mat img)
    {
   	cv::Mat src, src_gray;
	cv::Mat dst,dst1, detected_edges;
 
	int edgeThresh = 1;
	int lowThreshold = 200;
	int highThreshold =300;
	int kernel_size = 5;
 
	img.copyTo(src);
 
	cv::cvtColor( src, src_gray, CV_BGR2GRAY );
        cv::blur( src_gray, detected_edges, cv::Size(5,5) );
	cv::Canny( detected_edges, detected_edges, lowThreshold, highThreshold, kernel_size );
 
  	dst = cv::Scalar::all(0);
  	img.copyTo( dst, detected_edges);
	dst.copyTo(img);
 
    	cv::imshow("sub->canny", dst);
    	cv::waitKey(3);
    }	

   void hello(cv::Mat img)
   {
   	cv::Mat src, src_gray,img_value;
	cv::Mat dst,dst1, detected_edge;

    RNG g_rng(12345);//随机数生成器

	vector<vector<Point> > contours;
	vector<Vec4i> hierarchy;

	img.copyTo(src);
	//转换到灰度图 
	cv::cvtColor(src, src_gray, CV_BGR2GRAY);
        blur(src_gray,  src_gray, cv::Size(3, 3), Point(-1, -1));
	//二值化
        cv::threshold(src_gray, img_value, 100, 255, CV_THRESH_BINARY_INV);

	// 找出轮廓
	findContours(img_value, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
 
	// 多边形逼近轮廓 + 获取矩形和圆形边界框
	vector<vector<Point> > contours_poly(contours.size());
	vector<Rect> boundRect(contours.size());
	vector<Point2f>center(contours.size());
	vector<float>radius(contours.size());
 
	//一个循环，遍历所有部分，进行本程序最核心的操作
	for (unsigned int i = 0; i < contours.size(); i++)
	{
		approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);//用指定精度逼近多边形曲线 
		boundRect[i] = boundingRect(Mat(contours_poly[i]));//计算点集的最外面（up-right）矩形边界
		minEnclosingCircle(contours_poly[i], center[i], radius[i]);//对给定的 2D点集，寻找最小面积的包围圆形 
	}
	// 绘制多边形轮廓 + 包围的矩形框 + 圆形框
	Mat drawing = Mat::zeros(img_value.size(), CV_8UC3);
	for (int unsigned i = 0; i < contours.size(); i++)
	{
		Scalar color = Scalar(g_rng.uniform(0, 255), g_rng.uniform(0, 255), g_rng.uniform(0, 255));//随机设置颜色
		drawContours(drawing, contours_poly, i, color, 1, 8, vector<Vec4i>(), 0, Point());//绘制轮廓
		rectangle(drawing, boundRect[i].tl(), boundRect[i].br(), color, 2, 8, 0);//绘制矩形
		circle(drawing, center[i], (int)radius[i], color, 2, 8, 0);//绘制圆
        }
        img.copyTo(dst,drawing);
	dst.copyTo(img);
    	cv::imshow("text", dst);
    	cv::waitKey(3);
   }
};  
      
int main(int argc, char** argv)  
{  
  ros::init(argc, argv, "image_listener");  
  ImageConvertor ic;  
  ros::spin();       
  return 0;  
}

四、编译，并运行节点

1.在工作空间打开一个终端，输入指令：catkin_make

ros@ubuntu:~/catkin_ws$ catkin_make

在这里插入图片描述

2.打开新终端，运行roscore

输入指令：roscore

ros@ubuntu:~$ roscore

在这里插入图片描述

3. 在绝对路径打开新终端，运行第一个节点pic2msg

输入指令：rosrun the_image_transport the_image_transport_pic2msg ./1.png

ros@ubuntu:~/catkin_ws/src/the_image_transport/src$ rosrun the_image_transport the_image_transport_pic2msg ./1.png

在这里插入图片描述

4.在绝对路径打开新终端，运行第二个节点msg2video

输入指令：rosrun the_image_transport the_image_transport_msg2video1

ros@ubuntu:~/catkin_ws/src/the_image_transport/src$ rosrun the_image_transport the_image_transport_msg2video1

在这里插入图片描述

5.运行结果

在这里插入图片描述

6.在rviz可查看新旧话题发布情况

在新终端输入指令：rviz

ros@ubuntu:~$ rviz

在这里插入图片描述
分别添加新旧话题：

最终效果图：

五、参考资料

图片来源:小破站-Ja039up主（超级无敌可爱）

1 http://blog.csdn.net/github_30605157/article/details/50990493

2 http://blog.csdn.net/x_r_su/article/details/52704193

3 http://wiki.ros.org/image_transport/Tutorials

4 http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
5.https://www.cnblogs.com/xingkongcanghai/p/11197111.html
6.https://blog.csdn.net/ding977921830/article/details/70168877?ops_request_misc=&request_id=&biz_id=102&utm_term=ros%20%E5%90%8C%E6%97%B6%E8%AE%A2%E9%98%85%E5%8F%91%E5%B8%83&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduweb~default-6-70168877.pc_search_result_hbase_insert&spm=1018.2226.3001.4187