1、在工作空間下創建程序包
$ cd ~/catkin_ws/src$ catkin_create_pkg robot_vision roscpp std_msgs cv_bridge image_transport sensor_msgs
$ cd ..
2、在創建的程序包的src文件中創建一個文本文件，並命名為getImage.cpp。具體代碼和注釋如下：
$ gedit getImage.cpp
#include<ros/ros.h> //ros標准庫頭文件
#include<iostream> //C++標准輸入輸出庫
/* cv_bridge中包含CvBridge庫 */
#include<cv_bridge/cv_bridge.h>
/* ROS圖象類型的編碼函數 */
#include<sensor_msgs/image_encodings.h>
/* image_transport 頭文件用來在ROS系統中的話題上發布和訂閱圖象消息 */
#include<image_transport/image_transport.h>
//OpenCV2標准頭文件
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
static const std::string INPUT = "Input"; //定義輸入窗口名稱
static const std::string OUTPUT = "Output"; //定義輸出窗口名稱
//定義一個轉換的類
class ImageConverter {
private:
ros::NodeHandle nh_; //定義ROS節點句柄
image_transport::ImageTransport it_; //定義一個image_transport實例,用來發布和訂閱ROS系統的圖像
image_transport::Subscriber image_sub_; //定義ROS圖象接收器 ,訂閱主題的變量
image_transport::Publisher image_pub_; //定義ROS圖象發布器 ,發布主題的變量
public:
ImageConverter() :it_(nh_) //構造函數
{
image_sub_ = it_.subscribe("camera/image_raw", 1, &ImageConverter::convert_callback, this); //定義圖象接受器，訂閱話題是“camera/rgb/image_raw”
image_pub_ = it_.advertise("/image_converter/output_video", 1); //定義圖象發布器
//初始化輸入輸出窗口
cv::namedWindow(INPUT);
cv::namedWindow(OUTPUT);
}
~ImageConverter() //析構函數
{
cv::destroyWindow(INPUT);
cv::destroyWindow(OUTPUT);
}
/* 這是一個ROS和OpenCV的格式轉換回調函數，將圖象格式從sensor_msgs/Image ---> cv::Mat */
void convert_callback(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr; // 聲明一個CvImage指針的實例
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); //將ROS消息中的圖象信息提取，生成新cv類型的圖象，復制給CvImage指針
}
catch(cv_bridge::Exception& e) //異常處理
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
image_process(cv_ptr->image); //得到了cv::Mat類型的圖象，在CvImage指針的image中，將結果傳送給處理函數
}
/* 這是圖象處理的主要函數，一般會把圖像處理的主要程序寫在這個函數中。這里的例子只是一個彩色圖象到灰度圖象的轉化 */
void image_process(cv::Mat img)
{
cv::Mat img_out; cv::cvtColor(img, img_out, CV_RGB2GRAY); //轉換成灰度圖象
cv::imshow(INPUT, img);
cv::imshow(OUTPUT, img_out);
cv::waitKey(5);
}
};
//主函數
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_converter"); //ros節點的初始化
ImageConverter obj;
ros::spin();//ros::spin()進入自循環，當ros::ok()返回FALSE時，ros::spin()就立刻跳出循環
//這很可能是ros::shutdown()被調用，或者用戶按下了Ctrl+C退出組合鍵
}
3、編譯成可執行文件
在編寫程序后，這個文本程序在編譯成可執行文件之前是不能夠運行的。首先在建立的robot_vision的程序包中的CMakeLists.txt文件中加入如下代碼：
add_executable(getImage src/getImage.cpp) //將src中的文件添加成名字為getImage的可執行文件 target_link_libraries(getImage ${catkin_LIBRARIES}) //將相關的庫和可執行文件鏈接 add_dependencies(getImage robot_vision_generate_messages_cpp) //給可執行文件添加依賴包
返回到工作空間下編譯。catkin_make