ROS实战之节点发布/接收图片

ROS-节点发布/接收图片

ROS版本：indigo
摄像头：罗技C270（可以先用cheese来进行测试摄像头是否正常工作或者lsusb查看）

Indigo下OpenCV包的使用*
ROS从indigo开始就不再把opencv作为系统依赖包而是作为一个第三方包引入，如果直接使用会在rosmake编译阶段报错，no exist package “opencv2”，此时只需要在CMakeLists.txt中添加find_package(OpenCV 2 REQUIRED)，然后在manifest.xml中不能添加依赖包，此处添加的为系统依赖包。
Indigo下编程的细节
1） 节点：ros::NodeHandle n(“~”)
在设置节点句柄时，需要使用传值初始化来构造，否则无法通过rosrun xxx xxx _param:=xxx来进行传参，但使用了这种定义方式，发送主题会不成功，原因不详，可用argv[1]和istringstream来替代，可以使用到/dev/videox查看摄像头的设备号。
1) 在节点程序中，如果需要在rosrun运行节点的时候进行传参操作，只需要在程序中使用template n.param(const string&, T a, default value);即可
2) ROS中使用VideoCapture来存储摄像头动态获取到的图像，这个类的底层实现是cvCapture
3) ImageTransport对象能够以任何格式发送图像，感觉像是NodeHandler的派生类，这个不是特别清楚，其在使用时优于NodeHandler的是在advertise主题的时候无需指定模板类型，可以直接advertise（针对图像来说）
image_transport::Publisher为图片的发布者。
Image_transport::Subscriber为图片的订阅者。（不知道定义这个对象的作用是什么，但必须有）
其中advertise的作用是告诉主节点，本节点将会发送该主题的信息，相当于注册，而获得的Pulisher则起到内容发送者的作用。
image_transport::Publisher最终是以sensor_msgs::ImagePtr来发送主题的。
4) ROS图片转换的流程：VideoCapture->Mat->CvImage(ROS与Mat转换的中介)->toImageMsg->sensor_msgs::ImagePtr (ROS中使用的图像信息)
5) ros::spin：回调响应循环，有点像while(1){if(ros::ok())break;}的意思
ros::spinOnce：单次回调，可以根据自己定义的频率来进行主题的发布，通常与ros::Rate loop_rate()以及loop_rate.sleep()配合使用。loop_rate.sleep()是根据设定的频率来将程序挂起。
遇到的错误
a) 编译camera_publisher.cpp后，运行图片发送节点，出现ERROR：Tried to advertise a service that is already advertised in this node的错误
这里面可能的原因是在while(ros::ok())将advertise包含进去了，这样就会导致重复地向主节点注册主题，故报错。据wiki上解释，有时候自己编写的节点可能与系统本身所有的节点发送的主题起冲突，此时只需修改你自己的主题名即可。
源码
****img_deal

/*
 *  Author: OJ
 *  Date: 2016/08/26
 *  Function: deal with the image
 */
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

int imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
    try{
        imshow("img", cv_bridge::toCvShare(msg, "bgr8")->image);
    }catch(cv_bridge::Exception &e){
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
    return 0;
}

int main(int argc, char *argv[])
{
    /*初始化节点，并设定节点名*/
    ros::init(argc, argv, "img_deal");
    /*设置节点句柄*/  
    ros::NodeHandle n;

    /*创建显示窗口*/
    namedWindow("img");
    /*打开创建的窗口*/
    startWindowThread();

    /*设置图像接受的节点*/
    image_transport::ImageTransport it(n);
    /*设置图像订阅者*/
    image_transport::Subscriber sub = it.subscribe("camera/img", 1, imageCallback);

    /*回调响应循环*/
    ros::spin();

    /*关闭窗口*/
    destroyWindow("img");   

    return 0;
}

camera_publish****

/*
 *  Author: OJ
 *  Date: 2016/08/25
 *  Function: camera image publisher
 */
#include <ros/ros.h>
#include <iostream>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

class Camera_driver{
public:
    /*构造函数*/
    Camera_driver(ros::NodeHandle &n, int video_source = 0):
        it(n),
        cap(video_source){
            /*判断摄像头是否正常打开,若正常打开isOpened返回1*/
            if(!cap.isOpened())
            {
                ROS_ERROR("Cannot open the camera!\n");
            }
            /*设置主题名和缓冲区*/   
            pub_img = it.advertise("camera/img", 1);

            /*初始化CvImage智能指针，CvImage为Mat与ROS图像之间转换的载体*/
            frame = boost::make_shared<cv_bridge::CvImage>();
            /*设置ROS图片为BGR且每个像素点用1个字节来表示类似与CV_8U*/
            frame->encoding = sensor_msgs::image_encodings::BGR8;
        }

    /*图像发布函数*/
    int img_publish()
    {
        /*将摄像头获取到的图像存放在frame中的image*/
        cap >> frame->image;
        /*判断是否获取到图像，若获取到图像，将其转化为ROS图片*/
        if(!(frame->image.empty()))
        {
            frame->header.stamp = ros::Time::now();
            pub_img.publish(frame->toImageMsg());
        }

        return 0;       
    }

private:    
    /*设置图片节点*/
    image_transport::ImageTransport it;
    /*设置图片的发布者*/
    image_transport::Publisher pub_img;
    /*设置存放摄像头图像的变量*/    
    VideoCapture cap;
    /*设置cvImage的智能指针*/
    cv_bridge::CvImagePtr frame;
};

int main(int argc, char *argv[])
{
    /*初始化节点，并设定节点名*/
    ros::init(argc, argv, "img_publiser");
    /*设置节点句柄*/
    ros::NodeHandle n;

    /*判断输入参数是否完成*/
    if(argv[1] == NULL)
    {
        ROS_WARN("Please choose the camera you want to use !");
        return 1;
    }

    /*获取打开摄像机的设备号*/
    int video_source = 0;
    int default_p = 0;
    istringstream default_param(argv[1]);
    default_param >> default_p;
    n.param<int>("video_source", video_source, default_p);

    /*定义摄像机对象*/
    Camera_driver camera(n, video_source);

    /*设置主题的发布频率为5Hz*/
    ros::Rate loop_rate(5);
    /*图片节点进行主题的发布*/
    while(ros::ok())
    {
        camera.img_publish();
        ros::spinOnce();
        /*按照设定的频率来将程序挂起*/
        loop_rate.sleep();
    }

    return 0;
}