ROS1 cv_bridge实践

目录

0. 概述

1. 相关api源码声明&使用

1.1 Opencv图像转ROS图像消息

1.2 ROS图像消息转Opencv图像

2. 实践

2.1 创建ROS工作空间

2.2 创建图像发送节点和接收节点

2.3 实现图像的发送节点

2.4 实现图像的接收节点

3. 相关链接

---

0. 概述

        Opencv以其丰富的图像处理算法广泛应用于图像处理中，而Opencv主要使用cv::Mat封装图像数据，该数据是无法直接在ROS中传输的，需要对其进行编码成ROS可传输的sensor_msgs::Image格式；cv_bridge的主要作用便是实现该转换过程。通过cv_bridge的api可以很方便的将Opencv的cv::Mat转换为cv_bridge的数据格式CvImage，然后调用自己的成员函数toImageMsg()将CvImage转换为sensor_msgs::Image。        

        cv_bridge是ROS图像消息和Opencv图像数据转换的一个功能包，就像该功能包的名字一样，cv_bridge在Opencv和ROS联合编程中起到桥梁的作用。

1. 相关api源码声明&使用

1.1 Opencv图像转ROS图像消息

        在CvImage类中执行的OpenCV图像转换为ROS消息的成员函数为 toImageMsg()，其源码声明为：

/**
 * \brief Image message class that is interoperable with sensor_msgs/Image but uses a
 * more convenient cv::Mat representation for the image data.
 */
class CvImage
{
public:
  std_msgs::Header header; //!< ROS header
  std::string encoding;    //!< Image encoding ("mono8", "bgr8", etc.)
  cv::Mat image;           //!< Image data for use with OpenCV

  /**
   * \brief Empty constructor.
   */
  CvImage() {}

  /**
   * \brief Constructor.
   */
  CvImage(const std_msgs::Header& header, const std::string& encoding,
          const cv::Mat& image = cv::Mat())
    : header(header), encoding(encoding), image(image)
  {
  }
  
  /**
   * \brief Convert this message to a ROS sensor_msgs::Image message.
   *
   * The returned sensor_msgs::Image message contains a copy of the image data.
   */
  sensor_msgs::ImagePtr toImageMsg() const;

  /**
   * dst_format is compress the image to desire format.
   * Default value is empty string that will convert to jpg format.
   * can be: jpg, jp2, bmp, png, tif at the moment
   * support this format from opencv:
   * http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
   */
  sensor_msgs::CompressedImagePtr toCompressedImageMsg(const Format dst_format = JPG) const;

  /**
   * \brief Copy the message data to a ROS sensor_msgs::Image message.
   *
   * This overload is intended mainly for aggregate messages such as stereo_msgs::DisparityImage,
   * which contains a sensor_msgs::Image as a data member.
   */
  void toImageMsg(sensor_msgs::Image& ros_image) const;

  /**
   * dst_format is compress the image to desire format.
   * Default value is empty string that will convert to jpg format.
   * can be: jpg, jp2, bmp, png, tif at the moment
   * support this format from opencv:
   * http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
   */
  void toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, const Format dst_format = JPG) const;

  typedef boost::shared_ptr<CvImage> Ptr;
  typedef boost::shared_ptr<CvImage const> ConstPtr;

protected:
  boost::shared_ptr<void const> tracked_object_; // for sharing ownership

  /// @cond DOXYGEN_IGNORE
  friend
  CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
                            const boost::shared_ptr<void const>& tracked_object,
                            const std::string& encoding);
  /// @endcond
};

        Opencv cv::Mat转ROS的sensor_msgs::ImagePtr用到的主要是cv_bridge::CvImage的toImageMsg()成员函数，具体转换过程如下所示： 

cv::Mat image;
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

1.2 ROS图像消息转Opencv图像

        ROS的sensor_msgs::ImageConstPtr转Opencv的cv::Mat用的主要api包括toCvCopy()和toCvShare()，具体声明如下所示：

/**
 * \brief Convert a sensor_msgs::Image message to an OpenCV-compatible CvImage, copying the
 * image data.
 *
 * \param source   A shared_ptr to a sensor_msgs::Image message
 * \param encoding The desired encoding of the image data, one of the following strings:
 *    - \c "mono8"
 *    - \c "bgr8"
 *    - \c "bgra8"
 *    - \c "rgb8"
 *    - \c "rgba8"
 *    - \c "mono16"
 *
 * If \a encoding is the empty string (the default), the returned CvImage has the same encoding
 * as \a source.
 */
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
                    const std::string& encoding = std::string());

/**
 * \brief Convert a sensor_msgs::Image message to an OpenCV-compatible CvImage, copying the
 * image data.
 *
 * \param source   A sensor_msgs::Image message
 * \param encoding The desired encoding of the image data, one of the following strings:
 *    - \c "mono8"
 *    - \c "bgr8"
 *    - \c "bgra8"
 *    - \c "rgb8"
 *    - \c "rgba8"
 *    - \c "mono16"
 *
 * If \a encoding is the empty string (the default), the returned CvImage has the same encoding
 * as \a source.
 * If the source is 8bit and the encoding 16 or vice-versa, a scaling is applied (65535/255 and
 * 255/65535 respectively). Otherwise, no scaling is applied and the rules from the convertTo OpenCV
 * function are applied (capping): http://docs.opencv.org/modules/core/doc/basic_structures.html#mat-convertto
 */
CvImagePtr toCvCopy(const sensor_msgs::Image& source,
                    const std::string& encoding = std::string());

/**
 * \brief Convert an immutable sensor_msgs::Image message to an OpenCV-compatible CvImage, sharing
 * the image data if possible.
 *
 * If the source encoding and desired encoding are the same, the returned CvImage will share
 * the image data with \a source without copying it. The returned CvImage cannot be modified, as that
 * could modify the \a source data.
 *
 * \param source   A shared_ptr to a sensor_msgs::Image message
 * \param encoding The desired encoding of the image data, one of the following strings:
 *    - \c "mono8"
 *    - \c "bgr8"
 *    - \c "bgra8"
 *    - \c "rgb8"
 *    - \c "rgba8"
 *    - \c "mono16"
 *
 * If \a encoding is the empty string (the default), the returned CvImage has the same encoding
 * as \a source.
 */
 CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
                          const std::string& encoding = std::string());
                          
 /**
 * \brief Convert an immutable sensor_msgs::Image message to an OpenCV-compatible CvImage, sharing
 * the image data if possible.
 *
 * If the source encoding and desired encoding are the same, the returned CvImage will share
 * the image data with \a source without copying it. The returned CvImage cannot be modified, as that
 * could modify the \a source data.
 *
 * This overload is useful when you have a shared_ptr to a message that contains a
 * sensor_msgs::Image, and wish to share ownership with the containing message.
 *
 * \param source         The sensor_msgs::Image message
 * \param tracked_object A shared_ptr to an object owning the sensor_msgs::Image
 * \param encoding       The desired encoding of the image data, one of the following strings:
 *    - \c "mono8"
 *    - \c "bgr8"
 *    - \c "bgra8"
 *    - \c "rgb8"
 *    - \c "rgba8"
 *    - \c "mono16"
 *
 * If \a encoding is the empty string (the default), the returned CvImage has the same encoding
 * as \a source.
 */
CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
                          const boost::shared_ptr<void const>& tracked_object,
                          const std::string& encoding = std::string());

具体有如下使用方法：

// sensor_msgs::ImageConstPtr To cv::Mat
sensor_msgs::ImageConstPtr msg;
cv_bridge::CvImagePtr cv_ptr;
try
{
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch(cv_bridge::Exception &e)
{
    ROS_ERROR("cv_bridge exception %s", e.what());
}

2. 实践

        实践内容为使用ROS编写两个节点，发送者节点和接收者节点，发送者节点负责通过Opencv读取摄像头数据并将读取到的图像数据使用cv_bridge转换为可以在ROS节点间传输的图像数据；接收者节点订阅发布者节点的图像的数据并将ROS图像数据格式通过cv_bridge转换为Opencv的cv::Mat可视化。

cv_bridge编程实践

2.1 创建ROS工作空间

mkdir -p cv_bridge_test/src

2.2 创建图像发送节点和接收节点

cd v_bridge_test/src
catkin_create_pkg camera_pub roscpp rospy std_msgs sensor_msgs cv_bridge image_transport
catkin_create_pkg camera_sub roscpp rospy std_msgs sensor_msgs cv_bridge image_transport

2.3 实现图像的发送节点

// v_bridge_test/src/camera_pub/src/camera_pub.cpp
#include<iostream>
#include<ros/ros.h>
#include<opencv2/opencv.hpp>
#include<image_transport/image_transport.h>
#include<cv_bridge/cv_bridge.h>

class CameraDriver
{
public:
    CameraDriver():
        it(nh_),
        capture_(0)
    {
        image_pub_ = it_.advertise("/image_raw", 10);
        this->start_capture();
    }

    ~CameraDriver(){}

    void start_capture()
    {
        if(!capture_.isOpened())
        {
            ROS_ERROR("fail to open camera");
            ros::shutdown();
            return;
        }
        
        while(true)
        {
            cv::Mat image;
            capture_>>image;
            if(image.empty())
            {
                ROS_ERROR("NULL image");
                continue;
            }
            
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
            image_pub_.publish(std::move(msg));
            cv::waitKey(3);
        }
    }

private:
    cv::VideoCapture capture_;
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Publisher image_pub_;
};

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "CameraSource");
    CameraDriver cd;
    ros::spin();
    return 0;
}

修改CMakeLists.txt

# v_bridge_test/src/camera_pub/CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(camera_pub)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  cv_bridge
  image_transport
)

# Opencv
find_package(OpenCV REQUIRED)
message(STATUS "opencv version ${OpenCV_VERSION}")

catkin_package(CATKIN_DEPENDS
  cv_bridge
  image_transport
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME}_node src/camera_pub.cpp)
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
)

实现一个launch文件用以启动节点

<!--cv_bridge_test/src/camera_pub/launch/camera_pub.launch-->
<launch>
    <node pkg="camera_pub" type="camera_pub_node" name="camera_pub_node">
    </node>
</launch>

2.4 实现图像的接收节点

// cv_bridge_test/src/camera_sub/camera_sub.cpp
#include<iostream>
#include<ros/ros.h>
#include<opencv2/opencv.hpp>
#include<cv_bridge/cv_bridge.h>
#include<image_transport/image_transport.h>

static const std::string CV_WINDOW = "camera_player";

class CameraListener
{
public:
    CameraListener():
        it_(nh_)
    {
        image_sub_ = it_.subcribe("/image_raw", 1, &CameraListener::imageCb, this);
        cv::namedWindow(CV_WINDOW);
    }
    
    ~CameraListener()
    {
        cv::destroyWindow(CV_WINDOW);
    }

    void imageCb(const sensor_msgs::ImageConstPtr& msg)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, std_msgs::image_encodings::BGR8);
        }
        catch(cv_bridge::Exception &e)
        {
            ROS_ERROR("cv_bridge exception %s", e.what());
            return;
        }
    
        cv::imshow(CV_WINDOW, cv_ptr->image);
        cv::waitKey(3);
    }

private:
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;    
};

修改CMakeLists.txt

# cv_bridge_test/src/camera_pub/CMakelists.txt
cmake_minimum_required(VERSION 3.0.2)
project(camera_sub)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  cv_bridge
  image_transport
)

find_package(OpenCV REQUIRED)
message(STATUS "Opencv version ${OpenCV_VERSION}")

catkin_package(CATKIN_DEPENDS
  cv_bridge
  image_transport
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME}_node src/camera_sub.cpp)
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
)

实现一个launch文件用以启动节点

<!--cv_bridge_test/src/camera_pub/launch/camera_sub.launch-->
<launch>
    <node pkg="camera_sub" type="camera_sub_node" name="camera_sub_node">
    </node>
</launch>

3. 相关链接

• 【ros学习笔记】使用cv_bridge连接ROS和OpenCV - 灰信网（软件开发博客聚合）

• https://github.com/ros/ros_comm