ROS：两个节点同时具有发布和订阅图像信息的功能

来源：互联网发布：mac 上不了网编辑：程序博客网时间：2024/05/29 17:58

最近来公司实习，开始接触ROS（机器人操作系统），以下为工作笔记，如有不足请各位大牛们指出

作用描述：

（1）建立两个节点image_node_a 和 image_node_b;

（2）节点image_node_a在一个topic——node_a下发布图像message;

（3）节点image_node_b订阅topic——node_a；

（4）然后节点image_node_b又把收到的信息转换为opencv格式的图像进行处理；

（5）接着节点image_node_b把处理后的信息又在topic——node_b下发布出去；

（6）最后，node_a订阅topic——node_b。

一、节点node_a

在工作空间～/catkin_ws/src下新建一个文件夹——image_node_a，该文件夹下包含一个src的文件夹、一个CMakeLists.txt、一个package.xml，具体内容如下所示：

1 src/image_node_a.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("node_a listener from node_b", cv_bridge::toCvShare(tem_msg, "mono8")->image);    cv::waitKey(30);  }  catch (cv_bridge::Exception& e)  {    ROS_ERROR("Could not convert from '%s' to 'mono8'.", 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("node_a", 1);  image_transport::Subscriber sub = it.subscribe("node_b",1,imageCallback);  //cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);  cv::Mat image = cv::imread("/home/ding/lena.jpeg", 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 CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)project(image_node_a)set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")if(WIN32 AND NOT CYGWIN)    set(HOME $ENV{PROFILE})else()    set(HOME $ENV{HOME})endif()find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs cv_bridge image_transport)find_package(OpenCV REQUIRED)catkin_package(CATKIN_DEPENDSroscppstd_msgssensor_msgs)include_directories(include${OpenCV_INCLUDE_DIRS}${catkin_INCLUDE_DIRS})### find filesfile(GLOB_RECURSE HEADER_FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h*)file(GLOB_RECURSE SOURCE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.c*)add_executable(image_node_a${HEADER_FILES}${SOURCE_FILES})target_link_libraries(image_node_a${catkin_LIBRARIES}${OpenCV_LIBRARIES})

3 package.xml

<?xml version="1.0"?><package>  <name>image_node_a</name>  <version>0.0.0</version>  <description>The image_node_a package</description>  <maintainer email="abc@ding.com">abc</maintainer>  <license>TODO</license>   <build_depend>message_generation</build_depend>    <run_depend>message_runtime</run_depend>   <build_depend>opencv2</build_depend>   <run_depend>opencv2</run_depend>  <buildtool_depend>catkin</buildtool_depend>     <build_depend>roscpp</build_depend>  <build_depend>sensor_msgs</build_depend>  <build_depend>std_msgs</build_depend>  <build_depend>cv_bridge</build_depend>  <build_depend>image_transport</build_depend>   <run_depend>roscpp</run_depend>  <run_depend>sensor_msgs</run_depend>  <run_depend>std_msgs</run_depend>  <run_depend>cv_bridge</run_depend>  <run_depend>image_transport</run_depend>  <export>  </export></package>

二、节点node_b

在工作空间～/catkin_ws/src下新建一个文件夹——image_node_b，该文件夹下包含一个src的文件夹、一个CMakeLists.txt、一个package.xml，具体内容如下所示：

1 image_node_b.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>            namespace enc = sensor_msgs::image_encodings;            /*准备再次发布的图像显示到本窗口*/      //static const char OUT_WINDOW[] = "Image Out";      /*读取订阅的图像并显示到本窗口*/      //static std::string IN_WINDOW = "Image In";            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("node_b", 1);                     /*订阅主题camera/image*/              image_sub_ = it_.subscribe("node_a", 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("node_b listener from node_a",cv_ptr->image);                         cv::waitKey(30);            }                    catch (cv_bridge::Exception& e)              {                  ROS_ERROR("cv_bridge exception is %s", e.what());                  return;              }                    if(cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)                cv::circle(cv_ptr->image, cv::Point(50,50), 10, CV_RGB(255,0,0));                    //cv::imshow(IN_WINDOW, cv_ptr->image);              cv::Mat img_out;                            /*convert RGB to GRAY*/                      cv::cvtColor(cv_ptr->image, img_out, CV_RGB2GRAY);              //cv::imshow(OUT_WINDOW, img_out);              cv::imshow("node_b talker to node_c",img_out);            cv::waitKey(3);                      /*转化成ROS图像msg发布到主题out*/              image_pub_.publish(cv_ptr->toImageMsg());          }                  };                  int main(int argc, char** argv)      {          ros::init(argc, argv, "image_node_b");          ImageConvertor ic;          ros::spin();                    return 0;      }

2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)project(image_node_b)set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")if(WIN32 AND NOT CYGWIN)    set(HOME $ENV{PROFILE})else()    set(HOME $ENV{HOME})endif()find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs cv_bridge image_transport)find_package(OpenCV REQUIRED)catkin_package(CATKIN_DEPENDSroscppstd_msgs)include_directories(include${OpenCV_INCLUDE_DIRS}${catkin_INCLUDE_DIRS})### find filesfile(GLOB_RECURSE HEADER_FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h*)file(GLOB_RECURSE SOURCE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.c*)add_executable(image_node_b${HEADER_FILES}${SOURCE_FILES})target_link_libraries(image_node_b${catkin_LIBRARIES}${OpenCV_LIBRARIES})

3 package.xml

<?xml version="1.0"?><package>  <name>image_node_b</name>  <version>0.0.0</version>  <description>The image_node_b package</description>  <maintainer email="abc@ding.com">abc</maintainer>  <license>TODO</license>   <build_depend>message_generation</build_depend>    <run_depend>message_runtime</run_depend>   <build_depend>opencv2</build_depend>   <run_depend>opencv2</run_depend>  <buildtool_depend>catkin</buildtool_depend>     <build_depend>roscpp</build_depend>  <build_depend>sensor_msgs</build_depend>  <build_depend>std_msgs</build_depend>  <build_depend>cv_bridge</build_depend>  <build_depend>image_transport</build_depend>   <run_depend>roscpp</run_depend>  <run_depend>sensor_msgs</run_depend>  <run_depend>std_msgs</run_depend>  <run_depend>cv_bridge</run_depend>  <run_depend>image_transport</run_depend>  <export>  </export></package>

三、编译节点

分别按照如下步骤在终端上运行：

export ROS_PACKAGE_PATH=~/catkin_ws/src:$ROS_PACKAGE_PATH roscd image_node_acd ../..catkin_make

四、运行节点

1 打开一个新的终端

roscore

2 再打开一个新的终端

cd ~/catkin_ws
source ./devel/setup.bash
rosrun image_node_a image_node_a

3 再打开一个新的终端

cd ~/catkin_ws
source ./devel/setup.bash
rosrun image_node_b image_node_b

到此终于大功告成了，你可以看到如下的结果：

参考文献：

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

0 0