ROS学习——读取摄像头数据image

在ROS工作空间的src文件夹下创建read_camera功能包，并在包内创建include、launch、src、cfg四个文件夹。
在cfg文件夹中创建param.yaml文件，并写入以下内容：

image_dev: /dev/video0
save_path: /home/huanyu/datasets/camera

save: false
visualization: true

在include文件夹中创建camera_manager.h文件，并写入以下内容：

#include
#include
#include 
#include 
#include
#include
#include
#include
#include
#include
#include
class CameraManager
{
public:
    CameraManager(ros::NodeHandle nh,std::string name,int hz,std::string path="./");
    cv::Mat* read_image(bool save);
 const cv::Mat& getRGB() const{return RGB_;}
    const cv::Mat& getGRY() const{return GRAY_;}
    const double get_delay() const{return delay_;}
bool save_image(std::string name,cv::Mat& image)
    {
         //std::cout << path_+name << std::endl;
         return cv::imwrite(path_+name,image);
    }
  void spin(bool ros_send = true ,bool save = false,bool visualization = false);
private:
 image_transport::ImageTransport nh_;
    image_transport::Publisher image_pub;
   //for visualization
    cv::Mat GRAY_;
    cv::Mat RGB_;
   cv::VideoCapture capture_;
   int size_[3];
   int hz_;
    double delay_;
    std::string device_name_;
    std::string path_;
};
在src文件夹中创建camera_manager.cpp文件，并写入以下内容：
#include"camera_manager.h"
using namespace std;
CameraManager::CameraManager(ros::NodeHandle nh,std::string name,int hz,std::string path):
nh_(nh),device_name_(name),hz_(hz),path_(path)
{
    delay_ = 1000.0/hz_;
   cout<<"delay: "<<delay_<<endl;
   capture_ = cv::VideoCapture(0);
    
    if(!capture_.isOpened())
        return;
 cv::Mat frame;
    capture_ >> frame;
    size_[0] = frame.size[0];
    size_[1] = frame.size[1];
    size_[2] = frame.size[2];
    image_pub = nh_.advertise("/sensor_msgs/image_gray",5);
}
cv::Mat* CameraManager::read_image(bool save)
{
    cv::Mat frame,gray;
    capture_ >> frame;
    ros::Time timestamp = ros::Time::now();
    std::string time_second = std::to_string(timestamp.toSec()*1e9);
    std::string image_name(time_second);
    image_name.erase(19);
    image_name.append(".png");
    if(frame.empty())
    {
        //std::cout << "frequency is too high"<
        return nullptr;
    }
    cv::cvtColor(frame, gray, CV_BGR2GRAY);
    RGB_ = frame;
    GRAY_ = gray;
    if(save)
    {
        save_image(image_name, gray);
    }
    return &RGB_;
}
void CameraManager::spin(bool ros_send ,bool save,bool visualization)
{    
    sensor_msgs::ImagePtr msg;
    while(ros::ok())
    {
        cv::Mat* imagePtr = read_image(save);
        if(visualization) imshow("image_gray",*imagePtr);
        if(imagePtr&&ros_send)
        {
            msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", *imagePtr).toImageMsg();
            image_pub.publish(msg);
        }
        cv::waitKey(delay_);
    }
}

在launch文件夹中创建read_image.launch文件，并写入以下内容：

<launch>
    <node pkg = "read_camera" type="read_image" name="read_image" output="screen">
        <rosparam file="$(find read_camera)/cfg/param.yaml" command="load"/>
    </node>

    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>

下面还提供了保存照片的函数，需要的人自行编译save_image.cpp

#include
#include
#include
#include"camera_manager.h"
using namespace cv;
int main(int argc,char** argv)
{
    ros::init(argc,argv,"save_images");
    ros::NodeHandle nh;
    std::string device_name,save_path;
    bool save,visualization;
    ros::param::get("~/image_dev",device_name);
    ros::param::get("~/save_path",save_path);
    ros::param::get("~/visualization",visualization);
    CameraManager cm(nh,device_name,20,save_path);
    cm.spin(true,visualization,save);
}

需要注意的一点是，在读取摄像头数据的时候用到了OpenCV，所以在CMakeLists.txt文件find_package中加入OpenCV。
重新打开终端，输入rviz，运行rviz，然后修改image Topic为/sensor_msgs/image_gray，在rviz左下角就会显示摄像头的图像了。