mycobot_ros/mycobot_320/new_mycobot_320/src/opencv_camera.cpp

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream> // for converting the command line parameter to integer

int main(int argc, char **argv)
{
    // Check if video source has been passed as a parameter
    if (argv[1] == NULL)
    {
        ROS_INFO("argv[1]=NULL\n");
        return 1;
    }

    ros::init(argc, argv, "image_publisher"); // Initialize node
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic

    ros::Rate loop_rate(200); // refresh Hz.

    // Convert the passed as command line parameter index for the video device to an integer
    std::istringstream video_sourceCmd(argv[1]);
    int video_source;
    // Check if it is indeed a number
    if (!(video_sourceCmd >> video_source))
    {
        ROS_INFO("video_sourceCmd is %d\n", video_source);
        return 1;
    }

    cv::VideoCapture cap(video_source);
    // Check if video device can be opened with the given index
    if (!cap.isOpened())
    {
        ROS_INFO("can not opencv video device\n");
        return 1;
    }
    cv::Mat frame;
    sensor_msgs::ImagePtr msg;

    while (nh.ok())
    {
        cap >> frame;
        // cv::imshow("veiwer", frame);
        // Check if grabbed frame is actually full with some content
        if (!frame.empty())
        {
            msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
            pub.publish(msg);
            //cv::Wait(1);
        }
        ros::spinOnce();
        loop_rate.sleep();
        // if(cv::waitKey(2) >= 0)
        // break;
    }
}