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

最近来公司实习,开始接触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_DEPENDS
roscpp
std_msgs

sensor_msgs
)

include_directories(
include
${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)


### find files

file(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_DEPENDS
roscpp
std_msgs
)

include_directories(
include
${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)


### find files

file(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_a

cd ../..

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



阅读更多
换一批

没有更多推荐了,返回首页