在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-2allsobaiduweb~default-6-70168877.pc_search_result_hbase_insert&spm=1018.2226.3001.4187