vpROSGrabber.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: vpROSGrabber.cpp 3530 2012-01-03 10:52:12Z fpasteau $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
00007  *
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  *
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  *
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  *
00034  * Description:
00035  * Camera video capture for ROS image_transort_compressed.
00036  *
00037  * Authors:
00038  * Francois Pasteau
00039  * Fabien Spindler
00040  *
00041  *****************************************************************************/
00042 
00048 #include <visp_ros/vpROSGrabber.h>
00049 
00050 #if defined(VISP_HAVE_OPENCV)
00051 
00052 #include <visp/vpImageConvert.h>
00053 #include <visp/vpFrameGrabberException.h>
00054 #include <sensor_msgs/CompressedImage.h>
00055 #include <cv_bridge/cv_bridge.h>
00056 
00057 #include <iostream>
00058 #include <math.h>
00059 
00063 vpROSGrabber::vpROSGrabber() :
00064   isInitialized(false),
00065   mutex_image(true),
00066   mutex_param(true),
00067   first_img_received(false),
00068   first_param_received(false),
00069   _rectify(true),
00070   flip(false),
00071   _topic_image("image"),
00072   _topic_info("camera_info"),
00073   _master_uri("http://localhost:11311"),
00074   _nodespace(""),
00075   _image_transport("raw"),
00076   _sec(0),
00077   _nsec(0)
00078 {
00079 
00080 }
00081 
00082 
00088 vpROSGrabber::~vpROSGrabber( )
00089 {
00090   close();
00091 }
00092 
00093 
00094 
00106 void vpROSGrabber::open(int argc, char **argv){
00107 
00108   if(!isInitialized){
00109     std::string str;
00110     if(!ros::isInitialized()) ros::init(argc, argv, "visp_node", ros::init_options::AnonymousName);
00111     n = new ros::NodeHandle;
00112     if(_image_transport == "raw"){
00113       if (ros::param::get("~image_transport",  str)){
00114         _image_transport = str;
00115       }else{
00116         _image_transport = "raw";
00117         ros::param::set("~image_transport", "raw");
00118       }
00119     }
00120     if(_image_transport == "raw") {
00121       std::cout << "Subscribe to raw image on " << _nodespace + _topic_image << " topic" << std::endl;
00122       image_data = n->subscribe(_nodespace + _topic_image, 1, &vpROSGrabber::imageCallbackRaw,this,ros::TransportHints().tcpNoDelay());
00123     }
00124     else {
00125       std::cout << "Subscribe to image on " << _nodespace + _topic_image << " topic" << std::endl;
00126       image_data = n->subscribe(_nodespace + _topic_image, 1, &vpROSGrabber::imageCallback,this,ros::TransportHints().tcpNoDelay());
00127     }
00128 
00129     std::cout << "Subscribe to camera_info on " << _nodespace + _topic_info << " topic" << std::endl;
00130     image_info = n->subscribe(_nodespace + _topic_info, 1, &vpROSGrabber::paramCallback,this,ros::TransportHints().tcpNoDelay());
00131 
00132     spinner = new ros::AsyncSpinner(1);
00133     spinner->start();
00134     usWidth = -1;
00135     usHeight = -1;
00136     isInitialized = true;
00137   }
00138 }
00139 
00140 
00150 void vpROSGrabber::open(){
00151   if(ros::isInitialized() && ros::master::getURI() != _master_uri){
00152     close();
00153     throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
00154                                    "ROS grabber already initialised with a different master_URI (" + ros::master::getURI() +" != " + _master_uri + ")") );
00155   }
00156   if(!isInitialized){
00157     int argc = 2;
00158     char *argv[2];
00159     argv[0] = new char [255];
00160     argv[1] = new char [255];
00161 
00162     std::string exe = "ros.exe", arg1 = "__master:=";
00163     strcpy(argv[0], exe.c_str());
00164     arg1.append(_master_uri);
00165     strcpy(argv[1], arg1.c_str());
00166     open(argc, argv);
00167 
00168     // Wait for a first image
00169     while( !first_img_received) vpTime::wait(40);
00170 
00171     delete [] argv[0];
00172     delete [] argv[1];
00173   }
00174 }
00175 
00176 
00186 void vpROSGrabber::open(vpImage<unsigned char> &I)
00187 {
00188   open();
00189   acquire(I);
00190 }
00191 
00192 
00202 void vpROSGrabber::open(vpImage<vpRGBa> &I)
00203 {
00204   open();
00205   acquire(I);
00206 }
00207 
00208 
00209 
00210 
00222 void vpROSGrabber::acquire(vpImage<unsigned char> &I, struct timespec &timestamp)
00223 {  
00224   if (isInitialized==false) {
00225     close();
00226     throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
00227   }
00228   while(!mutex_image || !first_img_received);
00229   mutex_image = false;
00230   timestamp . tv_sec = _sec;
00231   timestamp . tv_nsec = _nsec;
00232   vpImageConvert::convert(data, I, flip);
00233   first_img_received = false;
00234   mutex_image = true;
00235 }
00236 
00237 
00238 
00253 bool vpROSGrabber::acquireNoWait(vpImage<unsigned char> &I, struct timespec &timestamp)
00254 {
00255   bool new_image = false;
00256   if (isInitialized==false) {
00257     close();
00258     throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
00259   }
00260   while(!mutex_image);
00261   mutex_image = false;
00262   timestamp . tv_sec = _sec;
00263   timestamp . tv_nsec = _nsec;
00264   vpImageConvert::convert(data, I, flip);
00265   new_image = first_img_received;
00266   first_img_received = false;
00267   mutex_image = true;
00268   return new_image;
00269 }
00270 
00271 
00283 void vpROSGrabber::acquire(vpImage<vpRGBa> &I, struct timespec &timestamp)
00284 {
00285   if (isInitialized==false) {
00286     close();
00287     throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
00288   }
00289   while(!mutex_image || !first_img_received);
00290   mutex_image = false;
00291   timestamp . tv_sec = _sec;
00292   timestamp . tv_nsec = _nsec;
00293   vpImageConvert::convert(data, I, flip);
00294   first_img_received = false;
00295   mutex_image = true;
00296 }
00297 
00298 
00299 
00314 bool vpROSGrabber::acquireNoWait(vpImage<vpRGBa> &I, struct timespec &timestamp)
00315 {
00316   bool new_image = false;
00317   if (isInitialized==false) {
00318     close();
00319     throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
00320   }
00321   while(!mutex_image);
00322   mutex_image = false;
00323   timestamp . tv_sec = _sec;
00324   timestamp . tv_nsec = _nsec;
00325   vpImageConvert::convert(data, I, flip);
00326   new_image = first_img_received;
00327   first_img_received = false;
00328   mutex_image = true;
00329   return new_image;
00330 }
00331 
00332 
00333 
00344 cv::Mat vpROSGrabber::acquire(struct timespec &timestamp)
00345 {
00346   cv::Mat retour;
00347   if (isInitialized==false) {
00348     close();
00349     throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
00350   }
00351   while(!mutex_image || !first_img_received);
00352   mutex_image = false;
00353   timestamp . tv_sec = _sec;
00354   timestamp . tv_nsec = _nsec;
00355   retour = data.clone();
00356   first_img_received = false;
00357   mutex_image = true;
00358   return retour;
00359 }
00360 
00361 
00362 
00372 void vpROSGrabber::acquire(vpImage<unsigned char> &I)
00373 {
00374   struct timespec timestamp;
00375   acquire(I, timestamp);
00376 }
00377 
00378 
00379 
00391 bool vpROSGrabber::acquireNoWait(vpImage<unsigned char> &I)
00392 {
00393   struct timespec timestamp;
00394   return acquireNoWait(I, timestamp);
00395 }
00396 
00397 
00407 void vpROSGrabber::acquire(vpImage<vpRGBa> &I)
00408 {
00409   struct timespec timestamp;
00410   acquire(I, timestamp);
00411 }
00412 
00413 
00414 
00426 bool vpROSGrabber::acquireNoWait(vpImage<vpRGBa> &I)
00427 {
00428   struct timespec timestamp;
00429   return acquireNoWait(I, timestamp);
00430 }
00431 
00432 
00433 
00442 cv::Mat vpROSGrabber::acquire()
00443 {
00444   struct timespec timestamp;
00445   return acquire(timestamp);
00446 }
00447 
00448 
00449 void vpROSGrabber::close(){
00450   if(isInitialized){
00451     isInitialized = false;
00452     spinner->stop();
00453     delete spinner;
00454     delete n;
00455   }
00456 }
00457 
00458 
00468 void vpROSGrabber::setFlip(bool flipType)
00469 {
00470   flip = flipType;
00471 }
00472 
00473 
00481 void vpROSGrabber::setRectify(bool rectify)
00482 {
00483   _rectify = rectify;
00484 }
00485 
00486 
00493 void vpROSGrabber::getWidth(unsigned short &width) const
00494 {
00495   width = getWidth();
00496 }
00497 
00498 
00506 void vpROSGrabber::getHeight(unsigned short &height)const
00507 {
00508   height = getHeight();
00509 }
00510 
00511 
00518 unsigned short vpROSGrabber::getWidth()const
00519 {
00520   return usWidth;
00521 }
00522 
00529 unsigned short vpROSGrabber::getHeight()const
00530 {
00531   return usHeight;
00532 }
00533 
00534 
00542 void vpROSGrabber::setCameraInfoTopic(std::string topic_name)
00543 {
00544   _topic_info = topic_name;
00545 }
00546 
00547 
00555 void vpROSGrabber::setImageTopic(std::string topic_name)
00556 {
00557   _topic_image = topic_name;
00558 }
00559 
00560 
00568 void vpROSGrabber::setMasterURI(std::string master_uri)
00569 {
00570   _master_uri = master_uri;
00571 }
00572 
00580 void vpROSGrabber::setNodespace(std::string nodespace)
00581 {
00582   _nodespace = nodespace;
00583 }
00584 
00585 
00586 void setImageTransport(std::string image_transport);
00587 
00597 void vpROSGrabber::setImageTransport(std::string image_transport)
00598 {
00599   _image_transport = image_transport;
00600 }
00601 
00610 bool vpROSGrabber::getCameraInfo(vpCameraParameters &cam){
00611   if (! isInitialized) {
00612     close();
00613     throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
00614   }
00615 
00616   // Test: if we get an image (first_img_received=true) we should have the camera parameters (first_param_received=true) if they are available
00617   if (first_img_received && !first_param_received)
00618     return false;
00619   while(!mutex_param || !first_param_received);
00620   mutex_param = false;
00621   cam = _cam;
00622   mutex_param = true;
00623 
00624   return true;
00625 }
00626 
00627 
00628 void vpROSGrabber::imageCallback(const sensor_msgs::CompressedImage::ConstPtr& msg){
00629 
00630   cv::Mat data_t = cv::imdecode(msg->data,1);
00631   cv::Size data_size = data_t.size();
00632 
00633   while(!mutex_image);
00634   mutex_image = false;
00635   if(_rectify && p.initialized()){
00636     p.rectifyImage(data_t,data);
00637   }else{
00638     data_t.copyTo(data);
00639   }
00640   usWidth = data_size.width;
00641   usHeight = data_size.height;
00642   _sec = msg->header.stamp.sec;
00643   _nsec = msg->header.stamp.nsec;
00644   first_img_received = true;
00645   mutex_image = true;
00646 }
00647 
00648 
00649 void vpROSGrabber::imageCallbackRaw(const sensor_msgs::Image::ConstPtr& msg){
00650   while(!mutex_image);
00651   mutex_image = false;
00652   cv_bridge::CvImageConstPtr cv_ptr;
00653   try
00654   {
00655     cv_ptr = cv_bridge::toCvShare(msg, "bgr8");
00656   }
00657   catch (cv_bridge::Exception& e)
00658   {
00659     ROS_ERROR("cv_bridge exception: %s", e.what());
00660     return;
00661   }
00662   if(_rectify && p.initialized()){
00663     p.rectifyImage(cv_ptr->image,data);
00664   }else{
00665     cv_ptr->image.copyTo(data);
00666   }
00667   cv::Size data_size = data.size();
00668   usWidth = data_size.width;
00669   usHeight = data_size.height;
00670   _sec = msg->header.stamp.sec;
00671   _nsec = msg->header.stamp.nsec;
00672   first_img_received = true;
00673   mutex_image = true;
00674 }
00675 
00676 void vpROSGrabber::paramCallback(const sensor_msgs::CameraInfo::ConstPtr& msg){
00677   if (_rectify) {
00678     while(!mutex_param);
00679     mutex_param = false;
00680     _cam = visp_bridge::toVispCameraParameters(*msg);
00681     p.fromCameraInfo(msg);
00682     first_param_received = true;
00683     mutex_param = true;
00684   }
00685 }
00686 
00687 #endif
00688 


visp_ros
Author(s): Francois Pasteau, Fabien Spindler
autogenerated on Thu Mar 24 2016 03:32:49