vpROSGrabber.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2021 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 <visp3/core/vpFrameGrabberException.h>
#include <visp3/core/vpImageConvert.h>
 
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/CompressedImage.h>
 
#include <iostream>
#include <math.h>
 
vpROSGrabber::vpROSGrabber()
  : m_isInitialized( false )
  , m_flip( false )
  , m_rectify( true )
  , m_mutex_image( true )
  , m_mutex_param( true )
  , m_first_img_received( false )
  , m_first_param_received( false )
  , m_sec( 0 )
  , m_nsec( 0 )
  , m_master_uri( "http://localhost:11311" )
  , m_topic_image( "image" )
  , m_topic_cam_info( "camera_info" )
  , m_nodespace( "" )
  , m_image_transport( "raw" )
{
}
 
vpROSGrabber::~vpROSGrabber() { close(); }
 
void
vpROSGrabber::open( int argc, char **argv )
{
 
  if ( !m_isInitialized )
  {
    std::string str;
    if ( !ros::isInitialized() )
      ros::init( argc, argv, "visp_node", ros::init_options::AnonymousName );
    m_n = new ros::NodeHandle;
    if ( m_image_transport == "raw" )
    {
      if ( ros::param::get( "~image_transport", str ) )
      {
        m_image_transport = str;
      }
      else
      {
        m_image_transport = "raw";
        ros::param::set( "~image_transport", "raw" );
      }
    }
    if ( m_image_transport == "raw" )
    {
      std::cout << "Subscribe to raw image on " << m_nodespace + m_topic_image << " topic" << std::endl;
      m_img_sub = m_n->subscribe( m_nodespace + m_topic_image, 1, &vpROSGrabber::imageCallbackRaw, this,
                                  ros::TransportHints().tcpNoDelay() );
    }
    else
    {
      std::cout << "Subscribe to image on " << m_nodespace + m_topic_image << " topic" << std::endl;
      m_img_sub = m_n->subscribe( m_nodespace + m_topic_image, 1, &vpROSGrabber::imageCallback, this,
                                  ros::TransportHints().tcpNoDelay() );
    }
 
    std::cout << "Subscribe to camera_info on " << m_nodespace + m_topic_cam_info << " topic" << std::endl;
    m_cam_info_sub = m_n->subscribe( m_nodespace + m_topic_cam_info, 1, &vpROSGrabber::paramCallback, this,
                                     ros::TransportHints().tcpNoDelay() );
 
    m_spinner = new ros::AsyncSpinner( 1 );
    m_spinner->start();
    m_width         = 0;
    m_height        = 0;
    m_isInitialized = true;
  }
}
 
void
vpROSGrabber::open()
{
  if ( ros::isInitialized() && ros::master::getURI() != m_master_uri )
  {
    close();
    throw( vpFrameGrabberException( vpFrameGrabberException::initializationError,
                                    "ROS grabber already initialised with a different master_URI (" +
                                        ros::master::getURI() + " != " + m_master_uri + ")" ) );
  }
  if ( !m_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( m_master_uri );
    strcpy( argv[1], arg1.c_str() );
    open( argc, argv );
 
    // Wait for a first image
    while ( !m_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 ( m_isInitialized == false )
  {
    close();
    throw( vpFrameGrabberException( vpFrameGrabberException::initializationError, "Grabber not initialized." ) );
  }
  while ( !m_mutex_image || !m_first_img_received )
    ;
  m_mutex_image     = false;
  timestamp.tv_sec  = m_sec;
  timestamp.tv_nsec = m_nsec;
  vpImageConvert::convert( m_img, I, m_flip );
  m_first_img_received = false;
  m_mutex_image        = true;
}
 
void
vpROSGrabber::acquire( vpImage< unsigned char > &I, double &timestamp_second )
{
  struct timespec timestamp;
  acquire( I, timestamp );
  timestamp_second = timestamp.tv_sec + static_cast< double >( timestamp.tv_nsec ) * 1e-9;
}
 
bool
vpROSGrabber::acquireNoWait( vpImage< unsigned char > &I, struct timespec &timestamp )
{
  bool new_image = false;
  if ( m_isInitialized == false )
  {
    close();
    throw( vpFrameGrabberException( vpFrameGrabberException::initializationError, "Grabber not initialized." ) );
  }
  while ( !m_mutex_image )
    ;
  m_mutex_image     = false;
  timestamp.tv_sec  = m_sec;
  timestamp.tv_nsec = m_nsec;
  vpImageConvert::convert( m_img, I, m_flip );
  new_image            = m_first_img_received;
  m_first_img_received = false;
  m_mutex_image        = true;
  return new_image;
}
 
void
vpROSGrabber::acquire( vpImage< vpRGBa > &I, struct timespec &timestamp )
{
  if ( m_isInitialized == false )
  {
    close();
    throw( vpFrameGrabberException( vpFrameGrabberException::initializationError, "Grabber not initialized." ) );
  }
  while ( !m_mutex_image || !m_first_img_received )
    ;
  m_mutex_image     = false;
  timestamp.tv_sec  = m_sec;
  timestamp.tv_nsec = m_nsec;
  vpImageConvert::convert( m_img, I, m_flip );
  m_first_img_received = false;
  m_mutex_image        = true;
}
 
void
vpROSGrabber::acquire( vpImage< vpRGBa > &I, double &timestamp_second )
{
  struct timespec timestamp;
  acquire( I, timestamp );
  timestamp_second = timestamp.tv_sec + static_cast< double >( timestamp.tv_nsec ) * 1e-9;
}
 
bool
vpROSGrabber::acquireNoWait( vpImage< vpRGBa > &I, struct timespec &timestamp )
{
  bool new_image = false;
  if ( m_isInitialized == false )
  {
    close();
    throw( vpFrameGrabberException( vpFrameGrabberException::initializationError, "Grabber not initialized." ) );
  }
  while ( !m_mutex_image )
    ;
  m_mutex_image     = false;
  timestamp.tv_sec  = m_sec;
  timestamp.tv_nsec = m_nsec;
  vpImageConvert::convert( m_img, I, m_flip );
  new_image            = m_first_img_received;
  m_first_img_received = false;
  m_mutex_image        = true;
  return new_image;
}
 
cv::Mat
vpROSGrabber::acquire( struct timespec &timestamp )
{
  cv::Mat img;
  if ( m_isInitialized == false )
  {
    close();
    throw( vpFrameGrabberException( vpFrameGrabberException::initializationError, "Grabber not initialized." ) );
  }
  while ( !m_mutex_image || !m_first_img_received )
    ;
  m_mutex_image        = false;
  timestamp.tv_sec     = m_sec;
  timestamp.tv_nsec    = m_nsec;
  img                  = m_img.clone();
  m_first_img_received = false;
  m_mutex_image        = true;
  return img;
}
 
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 ( m_isInitialized )
  {
    m_isInitialized = false;
    m_spinner->stop();
    delete m_spinner;
    delete m_n;
  }
}
 
void
vpROSGrabber::setFlip( bool flipType )
{
  m_flip = flipType;
}
 
void
vpROSGrabber::setRectify( bool rectify )
{
  m_rectify = rectify;
}
 
void
vpROSGrabber::getWidth( unsigned int width ) const
{
  width = getWidth();
}
 
void
vpROSGrabber::getHeight( unsigned int height ) const
{
  height = getHeight();
}
 
unsigned int
vpROSGrabber::getWidth() const
{
  return m_width;
}
 
unsigned int
vpROSGrabber::getHeight() const
{
  return m_height;
}
 
void
vpROSGrabber::setCameraInfoTopic( const std::string &topic_name )
{
  m_topic_cam_info = topic_name;
}
 
void
vpROSGrabber::setImageTopic( const std::string &topic_name )
{
  m_topic_image = topic_name;
}
 
void
vpROSGrabber::setMasterURI( const std::string &master_uri )
{
  m_master_uri = master_uri;
}
 
void
vpROSGrabber::setNodespace( const std::string &nodespace )
{
  m_nodespace = nodespace;
}
 
void
setImageTransport( const std::string image_transport );
 
void
vpROSGrabber::setImageTransport( const std::string &image_transport )
{
  m_image_transport = image_transport;
}
 
bool
vpROSGrabber::getCameraInfo( vpCameraParameters &cam )
{
  if ( !m_isInitialized )
  {
    close();
    throw( vpFrameGrabberException( vpFrameGrabberException::initializationError, "Grabber not initialized." ) );
  }
 
  // Test: if we get an image (m_first_img_received=true) we should have the camera parameters
  // (m_first_param_received=true) if they are available
  if ( m_first_img_received && !m_first_param_received )
    return false;
  while ( !m_mutex_param || !m_first_img_received )
    ;
  m_mutex_param = false;
  cam           = m_cam;
  m_mutex_param = true;
 
  return true;
}
 
void
vpROSGrabber::imageCallback( const sensor_msgs::CompressedImage::ConstPtr &msg )
{
  cv::Mat img        = cv::imdecode( msg->data, 1 );
  cv::Size data_size = img.size();
 
  while ( !m_mutex_image )
    ;
  m_mutex_image = false;
  if ( m_rectify && m_p.initialized() )
  {
    m_p.rectifyImage( img, m_img );
  }
  else
  {
    img.copyTo( m_img );
  }
  m_width              = static_cast< unsigned int >( data_size.width );
  m_height             = static_cast< unsigned int >( data_size.height );
  m_sec                = msg->header.stamp.sec;
  m_nsec               = msg->header.stamp.nsec;
  m_first_img_received = true;
  m_mutex_image        = true;
}
 
void
vpROSGrabber::imageCallbackRaw( const sensor_msgs::Image::ConstPtr &msg )
{
  while ( !m_mutex_image )
    ;
  m_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 ( m_rectify && m_p.initialized() )
  {
    m_p.rectifyImage( cv_ptr->image, m_img );
  }
  else
  {
    cv_ptr->image.copyTo( m_img );
  }
  cv::Size data_size   = m_img.size();
  m_width              = static_cast< unsigned int >( data_size.width );
  m_height             = static_cast< unsigned int >( data_size.height );
  m_sec                = msg->header.stamp.sec;
  m_nsec               = msg->header.stamp.nsec;
  m_first_img_received = true;
  m_mutex_image        = true;
}
 
void
vpROSGrabber::paramCallback( const sensor_msgs::CameraInfo::ConstPtr &msg )
{
  if ( m_rectify )
  {
    while ( !m_mutex_image )
      ;
    m_mutex_image = false;
    m_cam         = visp_bridge::toVispCameraParameters( *msg );
    m_p.fromCameraInfo( msg );
    m_first_param_received = true;
    m_mutex_image          = true;
  }
}
 
#endif