vpROSGrabber.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * $Id: vpROSGrabber.cpp 3530 2012-01-03 10:52:12Z fpasteau $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Camera video capture for ROS image_transort_compressed.
36  *
37  * Authors:
38  * Francois Pasteau
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
48 #include <visp_ros/vpROSGrabber.h>
49 
50 #if defined(VISP_HAVE_OPENCV)
51 
52 #include <visp/vpImageConvert.h>
53 #include <visp/vpFrameGrabberException.h>
54 #include <sensor_msgs/CompressedImage.h>
55 #include <cv_bridge/cv_bridge.h>
56 
57 #include <iostream>
58 #include <math.h>
59 
64  isInitialized(false),
65  mutex_image(true),
66  mutex_param(true),
67  first_img_received(false),
68  first_param_received(false),
69  _rectify(true),
70  flip(false),
71  _topic_image("image"),
72  _topic_info("camera_info"),
73  _master_uri("http://localhost:11311"),
74  _nodespace(""),
75  _image_transport("raw"),
76  _sec(0),
77  _nsec(0)
78 {
79 
80 }
81 
82 
89 {
90  close();
91 }
92 
93 
94 
106 void vpROSGrabber::open(int argc, char **argv){
107 
108  if(!isInitialized){
109  std::string str;
110  if(!ros::isInitialized()) ros::init(argc, argv, "visp_node", ros::init_options::AnonymousName);
111  n = new ros::NodeHandle;
112  if(_image_transport == "raw"){
113  if (ros::param::get("~image_transport", str)){
114  _image_transport = str;
115  }else{
116  _image_transport = "raw";
117  ros::param::set("~image_transport", "raw");
118  }
119  }
120  if(_image_transport == "raw") {
121  std::cout << "Subscribe to raw image on " << _nodespace + _topic_image << " topic" << std::endl;
123  }
124  else {
125  std::cout << "Subscribe to image on " << _nodespace + _topic_image << " topic" << std::endl;
127  }
128 
129  std::cout << "Subscribe to camera_info on " << _nodespace + _topic_info << " topic" << std::endl;
131 
132  spinner = new ros::AsyncSpinner(1);
133  spinner->start();
134  usWidth = -1;
135  usHeight = -1;
136  isInitialized = true;
137  }
138 }
139 
140 
152  close();
153  throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
154  "ROS grabber already initialised with a different master_URI (" + ros::master::getURI() +" != " + _master_uri + ")") );
155  }
156  if(!isInitialized){
157  int argc = 2;
158  char *argv[2];
159  argv[0] = new char [255];
160  argv[1] = new char [255];
161 
162  std::string exe = "ros.exe", arg1 = "__master:=";
163  strcpy(argv[0], exe.c_str());
164  arg1.append(_master_uri);
165  strcpy(argv[1], arg1.c_str());
166  open(argc, argv);
167 
168  // Wait for a first image
169  while( !first_img_received) vpTime::wait(40);
170 
171  delete [] argv[0];
172  delete [] argv[1];
173  }
174 }
175 
176 
186 void vpROSGrabber::open(vpImage<unsigned char> &I)
187 {
188  open();
189  acquire(I);
190 }
191 
192 
202 void vpROSGrabber::open(vpImage<vpRGBa> &I)
203 {
204  open();
205  acquire(I);
206 }
207 
208 
209 
210 
222 void vpROSGrabber::acquire(vpImage<unsigned char> &I, struct timespec &timestamp)
223 {
224  if (isInitialized==false) {
225  close();
226  throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
227  }
228  while(!mutex_image || !first_img_received);
229  mutex_image = false;
230  timestamp . tv_sec = _sec;
231  timestamp . tv_nsec = _nsec;
232  vpImageConvert::convert(data, I, flip);
233  first_img_received = false;
234  mutex_image = true;
235 }
236 
237 
238 
253 bool vpROSGrabber::acquireNoWait(vpImage<unsigned char> &I, struct timespec &timestamp)
254 {
255  bool new_image = false;
256  if (isInitialized==false) {
257  close();
258  throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
259  }
260  while(!mutex_image);
261  mutex_image = false;
262  timestamp . tv_sec = _sec;
263  timestamp . tv_nsec = _nsec;
264  vpImageConvert::convert(data, I, flip);
265  new_image = first_img_received;
266  first_img_received = false;
267  mutex_image = true;
268  return new_image;
269 }
270 
271 
283 void vpROSGrabber::acquire(vpImage<vpRGBa> &I, struct timespec &timestamp)
284 {
285  if (isInitialized==false) {
286  close();
287  throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
288  }
289  while(!mutex_image || !first_img_received);
290  mutex_image = false;
291  timestamp . tv_sec = _sec;
292  timestamp . tv_nsec = _nsec;
293  vpImageConvert::convert(data, I, flip);
294  first_img_received = false;
295  mutex_image = true;
296 }
297 
298 
299 
314 bool vpROSGrabber::acquireNoWait(vpImage<vpRGBa> &I, struct timespec &timestamp)
315 {
316  bool new_image = false;
317  if (isInitialized==false) {
318  close();
319  throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
320  }
321  while(!mutex_image);
322  mutex_image = false;
323  timestamp . tv_sec = _sec;
324  timestamp . tv_nsec = _nsec;
325  vpImageConvert::convert(data, I, flip);
326  new_image = first_img_received;
327  first_img_received = false;
328  mutex_image = true;
329  return new_image;
330 }
331 
332 
333 
344 cv::Mat vpROSGrabber::acquire(struct timespec &timestamp)
345 {
346  cv::Mat retour;
347  if (isInitialized==false) {
348  close();
349  throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
350  }
351  while(!mutex_image || !first_img_received);
352  mutex_image = false;
353  timestamp . tv_sec = _sec;
354  timestamp . tv_nsec = _nsec;
355  retour = data.clone();
356  first_img_received = false;
357  mutex_image = true;
358  return retour;
359 }
360 
361 
362 
372 void vpROSGrabber::acquire(vpImage<unsigned char> &I)
373 {
374  struct timespec timestamp;
375  acquire(I, timestamp);
376 }
377 
378 
379 
391 bool vpROSGrabber::acquireNoWait(vpImage<unsigned char> &I)
392 {
393  struct timespec timestamp;
394  return acquireNoWait(I, timestamp);
395 }
396 
397 
407 void vpROSGrabber::acquire(vpImage<vpRGBa> &I)
408 {
409  struct timespec timestamp;
410  acquire(I, timestamp);
411 }
412 
413 
414 
426 bool vpROSGrabber::acquireNoWait(vpImage<vpRGBa> &I)
427 {
428  struct timespec timestamp;
429  return acquireNoWait(I, timestamp);
430 }
431 
432 
433 
443 {
444  struct timespec timestamp;
445  return acquire(timestamp);
446 }
447 
448 
450  if(isInitialized){
451  isInitialized = false;
452  spinner->stop();
453  delete spinner;
454  delete n;
455  }
456 }
457 
458 
468 void vpROSGrabber::setFlip(bool flipType)
469 {
470  flip = flipType;
471 }
472 
473 
481 void vpROSGrabber::setRectify(bool rectify)
482 {
483  _rectify = rectify;
484 }
485 
486 
493 void vpROSGrabber::getWidth(unsigned short &width) const
494 {
495  width = getWidth();
496 }
497 
498 
506 void vpROSGrabber::getHeight(unsigned short &height)const
507 {
508  height = getHeight();
509 }
510 
511 
518 unsigned short vpROSGrabber::getWidth()const
519 {
520  return usWidth;
521 }
522 
529 unsigned short vpROSGrabber::getHeight()const
530 {
531  return usHeight;
532 }
533 
534 
542 void vpROSGrabber::setCameraInfoTopic(std::string topic_name)
543 {
544  _topic_info = topic_name;
545 }
546 
547 
555 void vpROSGrabber::setImageTopic(std::string topic_name)
556 {
557  _topic_image = topic_name;
558 }
559 
560 
568 void vpROSGrabber::setMasterURI(std::string master_uri)
569 {
570  _master_uri = master_uri;
571 }
572 
580 void vpROSGrabber::setNodespace(std::string nodespace)
581 {
582  _nodespace = nodespace;
583 }
584 
585 
586 void setImageTransport(std::string image_transport);
587 
597 void vpROSGrabber::setImageTransport(std::string image_transport)
598 {
599  _image_transport = image_transport;
600 }
601 
610 bool vpROSGrabber::getCameraInfo(vpCameraParameters &cam){
611  if (! isInitialized) {
612  close();
613  throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") );
614  }
615 
616  // Test: if we get an image (first_img_received=true) we should have the camera parameters (first_param_received=true) if they are available
618  return false;
619  while(!mutex_param || !first_param_received);
620  mutex_param = false;
621  cam = _cam;
622  mutex_param = true;
623 
624  return true;
625 }
626 
627 
628 void vpROSGrabber::imageCallback(const sensor_msgs::CompressedImage::ConstPtr& msg){
629 
630  cv::Mat data_t = cv::imdecode(msg->data,1);
631  cv::Size data_size = data_t.size();
632 
633  while(!mutex_image);
634  mutex_image = false;
635  if(_rectify && p.initialized()){
636  p.rectifyImage(data_t,data);
637  }else{
638  data_t.copyTo(data);
639  }
640  usWidth = data_size.width;
641  usHeight = data_size.height;
642  _sec = msg->header.stamp.sec;
643  _nsec = msg->header.stamp.nsec;
644  first_img_received = true;
645  mutex_image = true;
646 }
647 
648 
649 void vpROSGrabber::imageCallbackRaw(const sensor_msgs::Image::ConstPtr& msg){
650  while(!mutex_image);
651  mutex_image = false;
653  try
654  {
655  cv_ptr = cv_bridge::toCvShare(msg, "bgr8");
656  }
657  catch (cv_bridge::Exception& e)
658  {
659  ROS_ERROR("cv_bridge exception: %s", e.what());
660  return;
661  }
662  if(_rectify && p.initialized()){
663  p.rectifyImage(cv_ptr->image,data);
664  }else{
665  cv_ptr->image.copyTo(data);
666  }
667  cv::Size data_size = data.size();
668  usWidth = data_size.width;
669  usHeight = data_size.height;
670  _sec = msg->header.stamp.sec;
671  _nsec = msg->header.stamp.nsec;
672  first_img_received = true;
673  mutex_image = true;
674 }
675 
676 void vpROSGrabber::paramCallback(const sensor_msgs::CameraInfo::ConstPtr& msg){
677  if (_rectify) {
678  while(!mutex_param);
679  mutex_param = false;
681  p.fromCameraInfo(msg);
682  first_param_received = true;
683  mutex_param = true;
684  }
685 }
686 
687 #endif
688 
volatile unsigned short usWidth
Definition: vpROSGrabber.h:111
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::string _nodespace
Definition: vpROSGrabber.h:126
void imageCallbackRaw(const sensor_msgs::Image::ConstPtr &msg)
volatile uint32_t _sec
Definition: vpROSGrabber.h:122
ROSCPP_DECL const std::string & getURI()
volatile bool first_img_received
Definition: vpROSGrabber.h:121
vpCameraParameters _cam
Definition: vpROSGrabber.h:128
void setRectify(bool rectify)
void setFlip(bool flipType)
vpCameraParameters toVispCameraParameters(const sensor_msgs::CameraInfo &cam_info)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
volatile bool mutex_param
Definition: vpROSGrabber.h:117
ROSCPP_DECL bool isInitialized()
void setCameraInfoTopic(std::string topic_name)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool acquireNoWait(vpImage< unsigned char > &I)
virtual ~vpROSGrabber()
bool getCameraInfo(vpCameraParameters &cam)
void paramCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
volatile unsigned short usHeight
Definition: vpROSGrabber.h:112
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
volatile bool isInitialized
Definition: vpROSGrabber.h:110
volatile uint32_t _nsec
Definition: vpROSGrabber.h:122
void rectifyImage(const cv::Mat &raw, cv::Mat &rectified, int interpolation=cv::INTER_LINEAR) const
cv::Mat acquire()
ROSCPP_DECL bool get(const std::string &key, std::string &s)
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
std::string _image_transport
Definition: vpROSGrabber.h:127
ros::Subscriber image_data
Definition: vpROSGrabber.h:107
unsigned short getHeight() const
volatile bool _rectify
Definition: vpROSGrabber.h:116
void imageCallback(const sensor_msgs::CompressedImage::ConstPtr &msg)
ros::AsyncSpinner * spinner
Definition: vpROSGrabber.h:109
void setImageTransport(std::string image_transport)
class for Camera video capture for ROS middleware.
ros::Subscriber image_info
Definition: vpROSGrabber.h:108
volatile bool mutex_image
Definition: vpROSGrabber.h:117
std::string _topic_info
Definition: vpROSGrabber.h:125
void setNodespace(std::string nodespace)
void setMasterURI(std::string master_uri)
std::string _master_uri
Definition: vpROSGrabber.h:123
volatile bool first_param_received
Definition: vpROSGrabber.h:121
cv::Mat data
Definition: vpROSGrabber.h:114
ros::NodeHandle * n
Definition: vpROSGrabber.h:106
void setImageTopic(std::string topic_name)
#define ROS_ERROR(...)
image_geometry::PinholeCameraModel p
Definition: vpROSGrabber.h:113
unsigned short getWidth() const
std::string _topic_image
Definition: vpROSGrabber.h:124


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais
autogenerated on Tue Nov 24 2020 03:41:24