1 #include <ros/ros.h>
   2 #include <image_transport/image_transport.h>
   3 #include <opencv2/highgui/highgui.hpp>
   4 #include <cv_bridge/cv_bridge.h>
   5 #include <sstream> // for converting the command line parameter to integer
   6 
   7 int main(int argc, char** argv){
   8   // Check if video source has been passed as a parameter
   9   if(argv[1] == NULL) return 1;
  10 
  11   ros::init(argc, argv, "image_publisher");
  12   ros::NodeHandle nh;
  13   image_transport::ImageTransport it(nh);
  14   image_transport::Publisher pub = it.advertise("cam_pub", 1);
  15   int cam_device;
  16   //condition to verify if the camera device was passed as the first
  17   //terminal parameter "argv[1]" or as the "cam_device" parameter in the .launch.
  18   //id 0 device is passed as default
  19   if (argc < 2)
  20      nh.param("cam_device", cam_device, 0);
  21   else{
  22      // Convert the passed as command line parameter index
  23      //for the video device to an integer
  24      std::istringstream cam_deviceCmd(argv[1]);
  25      // Check if it is indeed a number
  26      if(!(cam_deviceCmd >> cam_device)) return 1;
  27   }
  28   //initialize a VideoCapture variable to get camera data
  29   cv::VideoCapture cap(cam_device);
  30   // Check if video device can be opened with the given index
  31   if(!cap.isOpened()) return 1;
  32   cv::Mat frame;
  33   sensor_msgs::ImagePtr msg;
  34 
  35   int image_show;
  36   nh.param("image_show", image_show, 0);
  37 
  38   while (nh.ok()) {
  39      cap >> frame;
  40      //show the image
  41      if (image_show){
  42         cv::imshow("Cam View", frame);
  43         cv::waitKey(30);
  44      }
  45      //Check if grabbed frame is actually full with some content,
  46      //then publish the image
  47      if(!frame.empty()) {
  48         msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
  49         pub.publish(msg);
  50      }
  51   }
  52 }

   1 #include <string>
   2 #include <ros/ros.h>
   3 #include <image_transport/image_transport.h>
   4 #include <opencv2/highgui/highgui.hpp>
   5 #include <cv_bridge/cv_bridge.h>
   6 #include "picture_server/image_cmd.h"
   7 #include <sstream>
   8 //Class creation to allow the use of camera callback msg in the service
   9 class PictureServer{
  10    cv::Mat picture;
  11 
  12 public:
  13    //callback to get camera data through "image_pub" topic
  14    void imageCallback(const sensor_msgs::ImageConstPtr& msg){
  15       try{
  16          picture = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
  17       }
  18       catch (cv_bridge::Exception& e){
  19          ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  20       }
  21    }
  22    // service callback that receives "angle" (int representing image name),
  23    // "path" (path to save image data) and
  24    // "cmd" (comand confirming if the camera data
  25    // should be saved). The service response should return a "result" returning 1
  26    // if the data was correctly saved
  27    bool check_and_print(picture_server::image_cmd::Request &req,
  28                         picture_server::image_cmd::Response &res){
  29       if (req.cmd){
  30          //image name composed by path (finished with "/")+ capture angle+extension
  31          std::string im_name = req.path + req.num_name+ ".png";
  32          //checking if the picture has a valid content,
  33          //otherwise system would failed and stop trying to write the image
  34          if(!picture.empty()){
  35             if (!cv::imwrite (im_name, picture)){
  36                res.result = 0;
  37                std::cout<<"Image can not be saved as '"<<im_name<<"'\n";
  38             }else{
  39                // represent success to save the image
  40                std::cout<<"Image saved in '"<<im_name<<"'\n";
  41                res.result = 1;
  42             }
  43          }else{
  44             // represent fail to save the image
  45             res.result = 0;
  46             ROS_ERROR("Failed to save image\n");
  47          }
  48       }else{
  49          // represent that server was called, but image was not requested
  50          res.result = 2;
  51       }
  52    }
  53 };
  54 
  55 int main(int argc, char **argv)
  56 {
  57    PictureServer mi;
  58    ros::init(argc, argv, "Img_Ctrl_server");
  59    ros::NodeHandle nh;
  60    image_transport::ImageTransport it(nh);
  61    ros::ServiceServer service = nh.advertiseService("image_cmd", &PictureServer::check_and_print, &mi);
  62   image_transport::Subscriber sub = it.subscribe("cam_pub", 1, &PictureServer::imageCallback, &mi);
  63 
  64    ros::spin();
  65 }