tutorial-ros-grabber.cpp

#include <visp/vpDisplayX.h>
#include <visp/vpImage.h>
#include <visp_ros/vpROSGrabber.h>

int main(int argc, const char** argv)
{
  try {
    bool opt_use_camera_info = false;
    for (int i=0; i<argc; i++) {
      if (std::string(argv[i]) == "--use-camera-info")
        opt_use_camera_info = true;
      else if (std::string(argv[i]) == "--help") {
        std::cout << "Usage: " << argv[0]
                  << " [--use-camera-info] [--help]"
                  << std::endl;
        return 0;
      }
    }
    std::cout << "Use camera info: " << ((opt_use_camera_info == true) ? "yes" : "no") << std::endl;

    //vpImage<unsigned char> I; // Create a gray level image container
    vpImage<vpRGBa> I; // Create a color image container
    vpROSGrabber g; // Create a grabber based on ROS

    g.setImageTopic("/camera/image_raw");
    if (opt_use_camera_info) {
      g.setCameraInfoTopic("/camera/camera_info");
      g.setRectify(true);
    }

    g.open(I);
    std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl;

#ifdef VISP_HAVE_X11
    vpDisplayX d(I);
#else
    std::cout << "No image viewer is available..." << std::endl;
#endif

    while(1) {
      g.acquire(I);
      vpDisplay::display(I);
      vpDisplay::displayText(I, 20, 20, "A click to quit...", vpColor::red);
      vpDisplay::flush(I);
      if (vpDisplay::getClick(I, false))
        break;
    }
  }
  catch(vpException e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
}