在ROS中，兩個節點分別實作影像的發布與訂閱功能，以及在其中一個節點實作影像的預處理

前言

最近正在學校實訓，學習的是智能機器人綜合設計，這周的任務量是兩節點影像的發布與訂閱，然后是影像預處理，
環境為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