vpROSGrabber.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 * $Id: vpROSGrabber.cpp 3530 2012-01-03 10:52:12Z fpasteau $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Camera video capture for ROS image_transort_compressed.
 *
 * Authors:
 * Francois Pasteau
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp_ros/vpROSGrabber.h>

#if defined(VISP_HAVE_OPENCV)

#include <visp/vpImageConvert.h>
#include <visp/vpFrameGrabberException.h>
#include <sensor_msgs/CompressedImage.h>
#include <cv_bridge/cv_bridge.h>

#include <iostream>
#include <math.h>

vpROSGrabber::vpROSGrabber() :
  isInitialized(false),
  mutex_image(true),
  mutex_param(true),
  first_img_received(false),
  first_param_received(false),
  _rectify(true),
  flip(false),
  _topic_image("image"),
  _topic_info("camera_info"),
  _master_uri("http://127.0.0.1:11311"),
  _nodespace(""),
  _image_transport("raw"),
  _sec(0),
  _nsec(0)
{

}


vpROSGrabber::~vpROSGrabber( )
{
  close();
}



void vpROSGrabber::open(int argc, char **argv){

  if(!isInitialized){
    std::string str;
    if(!ros::isInitialized()) ros::init(argc, argv, "visp_node", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;
    if(_image_transport == "raw"){
      if (ros::param::get("~image_transport",  str)){
        _image_transport = str;
      }else{
        _image_transport = "raw";
        ros::param::set("~image_transport", "raw");
      }
    }
    if(_image_transport == "raw") {
      std::cout << "Subscribe to raw image on " << _nodespace + _topic_image << " topic" << std::endl;
      image_data = n->subscribe(_nodespace + _topic_image, 1, &vpROSGrabber::imageCallbackRaw,this,ros::TransportHints().tcpNoDelay());
    }
    else {
      std::cout << "Subscribe to image on " << _nodespace + _topic_image << " topic" << std::endl;
      image_data = n->subscribe(_nodespace + _topic_image, 1, &vpROSGrabber::imageCallback,this,ros::TransportHints().tcpNoDelay());
    }

    std::cout << "Subscribe to camera_info on " << _nodespace + _topic_info << " topic" << std::endl;
    image_info = n->subscribe(_nodespace + _topic_info, 1, &vpROSGrabber::paramCallback,this,ros::TransportHints().tcpNoDelay());

    spinner = new ros::AsyncSpinner(1);
    spinner->start();
    usWidth = -1;
    usHeight = -1;
    isInitialized = true;
  }
}


void vpROSGrabber::open(){
  if(ros::isInitialized() && ros::master::getURI() != _master_uri){
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
                                   "ROS grabber already initialised with a different master_URI (" + ros::master::getURI() +" != " + _master_uri + ")") );
  }
  if(!isInitialized){
    int argc = 2;
    char *argv[2];
    argv[0] = new char [255];
    argv[1] = new char [255];

    std::string exe = "ros.exe", arg1 = "__master:=";
    strcpy(argv[0], exe.c_str());
    arg1.append(_master_uri);
    strcpy(argv[1], arg1.c_str());
    open(argc, argv);

    // Wait for a first image
    while( !first_img_received) vpTime::wait(40);

    delete [] argv[0];
    delete [] argv[1];
  }
}


void vpROSGrabber::open(vpImage<unsigned char> &I)
{
  open();
  acquire(I);
}


void vpROSGrabber::open(vpImage<vpRGBa> &I)
{
  open();
  acquire(I);
}




void vpROSGrabber::acquire(vpImage<unsigned char> &I, struct timespec &timestamp)
{  
  if (isInitialized==false) {
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
  }
  while(!mutex_image || !first_img_received);
  mutex_image = false;
  timestamp . tv_sec = _sec;
  timestamp . tv_nsec = _nsec;
  vpImageConvert::convert(data, I, flip);
  first_img_received = false;
  mutex_image = true;
}



bool vpROSGrabber::acquireNoWait(vpImage<unsigned char> &I, struct timespec &timestamp)
{
  bool new_image = false;
  if (isInitialized==false) {
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
  }
  while(!mutex_image);
  mutex_image = false;
  timestamp . tv_sec = _sec;
  timestamp . tv_nsec = _nsec;
  vpImageConvert::convert(data, I, flip);
  new_image = first_img_received;
  first_img_received = false;
  mutex_image = true;
  return new_image;
}


void vpROSGrabber::acquire(vpImage<vpRGBa> &I, struct timespec &timestamp)
{
  if (isInitialized==false) {
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
  }
  while(!mutex_image || !first_img_received);
  mutex_image = false;
  timestamp . tv_sec = _sec;
  timestamp . tv_nsec = _nsec;
  vpImageConvert::convert(data, I, flip);
  first_img_received = false;
  mutex_image = true;
}



bool vpROSGrabber::acquireNoWait(vpImage<vpRGBa> &I, struct timespec &timestamp)
{
  bool new_image = false;
  if (isInitialized==false) {
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
  }
  while(!mutex_image);
  mutex_image = false;
  timestamp . tv_sec = _sec;
  timestamp . tv_nsec = _nsec;
  vpImageConvert::convert(data, I, flip);
  new_image = first_img_received;
  first_img_received = false;
  mutex_image = true;
  return new_image;
}



cv::Mat vpROSGrabber::acquire(struct timespec &timestamp)
{
  cv::Mat retour;
  if (isInitialized==false) {
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
  }
  while(!mutex_image || !first_img_received);
  mutex_image = false;
  timestamp . tv_sec = _sec;
  timestamp . tv_nsec = _nsec;
  retour = data.clone();
  first_img_received = false;
  mutex_image = true;
  return retour;
}



void vpROSGrabber::acquire(vpImage<unsigned char> &I)
{
  struct timespec timestamp;
  acquire(I, timestamp);
}



bool vpROSGrabber::acquireNoWait(vpImage<unsigned char> &I)
{
  struct timespec timestamp;
  return acquireNoWait(I, timestamp);
}


void vpROSGrabber::acquire(vpImage<vpRGBa> &I)
{
  struct timespec timestamp;
  acquire(I, timestamp);
}



bool vpROSGrabber::acquireNoWait(vpImage<vpRGBa> &I)
{
  struct timespec timestamp;
  return acquireNoWait(I, timestamp);
}



cv::Mat vpROSGrabber::acquire()
{
  struct timespec timestamp;
  return acquire(timestamp);
}


void vpROSGrabber::close(){
  if(isInitialized){
    isInitialized = false;
    spinner->stop();
    delete spinner;
    delete n;
  }
}


void vpROSGrabber::setFlip(bool flipType)
{
  flip = flipType;
}


void vpROSGrabber::setRectify(bool rectify)
{
  _rectify = rectify;
}


void vpROSGrabber::getWidth(unsigned short &width) const
{
  width = getWidth();
}


void vpROSGrabber::getHeight(unsigned short &height)const
{
  height = getHeight();
}


unsigned short vpROSGrabber::getWidth()const
{
  return usWidth;
}

unsigned short vpROSGrabber::getHeight()const
{
  return usHeight;
}


void vpROSGrabber::setCameraInfoTopic(std::string topic_name)
{
  _topic_info = topic_name;
}


void vpROSGrabber::setImageTopic(std::string topic_name)
{
  _topic_image = topic_name;
}


void vpROSGrabber::setMasterURI(std::string master_uri)
{
  _master_uri = master_uri;
}

void vpROSGrabber::setNodespace(std::string nodespace)
{
  _nodespace = nodespace;
}


void setImageTransport(std::string image_transport);

void vpROSGrabber::setImageTransport(std::string image_transport)
{
  _image_transport = image_transport;
}

bool vpROSGrabber::getCameraInfo(vpCameraParameters &cam){
  if (! isInitialized) {
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
  }

  // Test: if we get an image (first_img_received=true) we should have the camera parameters (first_param_received=true) if they are available
  if (first_img_received && !first_param_received)
    return false;
  while(!mutex_param || !first_param_received);
  mutex_param = false;
  cam = _cam;
  mutex_param = true;

  return true;
}


void vpROSGrabber::imageCallback(const sensor_msgs::CompressedImage::ConstPtr& msg){

  cv::Mat data_t = cv::imdecode(msg->data,1);
  cv::Size data_size = data_t.size();

  while(!mutex_image);
  mutex_image = false;
  if(_rectify && p.initialized()){
    p.rectifyImage(data_t,data);
  }else{
    data_t.copyTo(data);
  }
  usWidth = data_size.width;
  usHeight = data_size.height;
  _sec = msg->header.stamp.sec;
  _nsec = msg->header.stamp.nsec;
  first_img_received = true;
  mutex_image = true;
}


void vpROSGrabber::imageCallbackRaw(const sensor_msgs::Image::ConstPtr& msg){
  while(!mutex_image);
  mutex_image = false;
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvShare(msg, "bgr8");
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  if(_rectify && p.initialized()){
    p.rectifyImage(cv_ptr->image,data);
  }else{
    cv_ptr->image.copyTo(data);
  }
  cv::Size data_size = data.size();
  usWidth = data_size.width;
  usHeight = data_size.height;
  _sec = msg->header.stamp.sec;
  _nsec = msg->header.stamp.nsec;
  first_img_received = true;
  mutex_image = true;
}

void vpROSGrabber::paramCallback(const sensor_msgs::CameraInfo::ConstPtr& msg){
  if (_rectify) {
    while(!mutex_param);
    mutex_param = false;
    _cam = visp_bridge::toVispCameraParameters(*msg);
    p.fromCameraInfo(msg);
    first_param_received = true;
    mutex_param = true;
  }
}

#endif