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;
}