gazebo_ros_camera_utils.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #include <string>
19 #include <algorithm>
20 #include <assert.h>
21 #include <boost/thread/thread.hpp>
22 #include <boost/bind.hpp>
23 
24 #include <tf/tf.h>
25 #include <tf/transform_listener.h>
26 #include <sensor_msgs/Image.h>
27 #include <sensor_msgs/fill_image.h>
29 #include <geometry_msgs/Point32.h>
30 #include <sensor_msgs/ChannelFloat32.h>
32 
33 #include <sdf/sdf.hh>
34 
35 #include <gazebo/physics/World.hh>
36 #include <gazebo/physics/HingeJoint.hh>
37 #include <gazebo/sensors/Sensor.hh>
38 #include <gazebo/common/Exception.hh>
39 #include <gazebo/sensors/CameraSensor.hh>
40 #include <gazebo/sensors/SensorTypes.hh>
41 #include <gazebo/rendering/Camera.hh>
42 #include <gazebo/rendering/Distortion.hh>
43 
45 
46 namespace gazebo
47 {
49 // Constructor
51 {
52  this->last_update_time_ = common::Time(0);
53  this->last_info_update_time_ = common::Time(0);
54  this->height_ = 0;
55  this->width_ = 0;
56  this->skip_ = 0;
57  this->format_ = "";
58  this->initialized_ = false;
59 }
60 
62  gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
63 {
64  if (this->initialized_)
65  {
66  ROS_INFO_NAMED("camera_utils", "Reconfigure request for the gazebo ros camera_: %s. New rate: %.2f",
67  this->camera_name_.c_str(), config.imager_rate);
68  this->parentSensor_->SetUpdateRate(config.imager_rate);
69  }
70 }
71 
73 // Destructor
75 {
76  this->parentSensor_->SetActive(false);
77  this->rosnode_->shutdown();
78  this->camera_queue_.clear();
79  this->camera_queue_.disable();
80  this->callback_queue_thread_.join();
81  delete this->rosnode_;
82 }
83 
85 // Load the controller
86 void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
87  sdf::ElementPtr _sdf,
88  const std::string &_camera_name_suffix,
89  double _hack_baseline)
90 {
91  // default Load:
92  // provide _camera_name_suffix to prevent LoadThread() creating the ros::NodeHandle with
93  //an incomplete this->camera_name_ namespace. There was a race condition when the _camera_name_suffix
94  //was appended in this function.
95  this->Load(_parent, _sdf, _camera_name_suffix);
96 
97  // overwrite hack baseline if specified at load
98  // example usage in gazebo_ros_multicamera
99  this->hack_baseline_ = _hack_baseline;
100 }
101 
103 // Load the controller
104 void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
105  sdf::ElementPtr _sdf,
106  const std::string &_camera_name_suffix)
107 {
108  // Get the world name.
109  std::string world_name = _parent->WorldName();
110 
111  // Get the world_
112  this->world_ = physics::get_world(world_name);
113 
114  // save pointers
115  this->sdf = _sdf;
116 
117  // maintain for one more release for backwards compatibility with
118  // pr2_gazebo_plugins
119  this->world = this->world_;
120 
121  std::stringstream ss;
122  this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Camera");
123 
124  this->image_topic_name_ = "image_raw";
125  if (this->sdf->HasElement("imageTopicName"))
126  this->image_topic_name_ = this->sdf->Get<std::string>("imageTopicName");
127 
128  this->trigger_topic_name_ = "image_trigger";
129  if (this->sdf->HasElement("triggerTopicName"))
130  this->trigger_topic_name_ = this->sdf->Get<std::string>("triggerTopicName");
131 
132  this->camera_info_topic_name_ = "camera_info";
133  if (this->sdf->HasElement("cameraInfoTopicName"))
135  this->sdf->Get<std::string>("cameraInfoTopicName");
136 
137  if (!this->sdf->HasElement("cameraName"))
138  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <cameraName>, default to empty");
139  else
140  this->camera_name_ = this->sdf->Get<std::string>("cameraName");
141 
142  // overwrite camera suffix
143  // example usage in gazebo_ros_multicamera
144  this->camera_name_ += _camera_name_suffix;
145 
146  if (!this->sdf->HasElement("frameName"))
147  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <frameName>, defaults to /world");
148  else
149  this->frame_name_ = this->sdf->Get<std::string>("frameName");
150 
151  if (!this->sdf->HasElement("updateRate"))
152  {
153  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <updateRate>, defaults to unlimited (0).");
154  this->update_rate_ = 0;
155  }
156  else
157  this->update_rate_ = this->sdf->Get<double>("updateRate");
158 
159  if (!this->sdf->HasElement("CxPrime"))
160  {
161  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <CxPrime>, defaults to 0");
162  this->cx_prime_ = 0;
163  }
164  else
165  this->cx_prime_ = this->sdf->Get<double>("CxPrime");
166 
167  if (!this->sdf->HasElement("Cx"))
168  {
169  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <Cx>, defaults to 0");
170  this->cx_= 0;
171  }
172  else
173  this->cx_ = this->sdf->Get<double>("Cx");
174 
175  if (!this->sdf->HasElement("Cy"))
176  {
177  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <Cy>, defaults to 0");
178  this->cy_= 0;
179  }
180  else
181  this->cy_ = this->sdf->Get<double>("Cy");
182 
183  if (!this->sdf->HasElement("focalLength"))
184  {
185  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <focalLength>, defaults to 0");
186  this->focal_length_= 0;
187  }
188  else
189  this->focal_length_ = this->sdf->Get<double>("focalLength");
190 
191  if (!this->sdf->HasElement("hackBaseline"))
192  {
193  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <hackBaseline>, defaults to 0");
194  this->hack_baseline_= 0;
195  }
196  else
197  this->hack_baseline_ = this->sdf->Get<double>("hackBaseline");
198 
199  if (!this->sdf->HasElement("distortionK1"))
200  {
201  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <distortionK1>, defaults to 0");
202  this->distortion_k1_= 0;
203  }
204  else
205  this->distortion_k1_ = this->sdf->Get<double>("distortionK1");
206 
207  if (!this->sdf->HasElement("distortionK2"))
208  {
209  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <distortionK2>, defaults to 0");
210  this->distortion_k2_= 0;
211  }
212  else
213  this->distortion_k2_ = this->sdf->Get<double>("distortionK2");
214 
215  if (!this->sdf->HasElement("distortionK3"))
216  {
217  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <distortionK3>, defaults to 0");
218  this->distortion_k3_= 0;
219  }
220  else
221  this->distortion_k3_ = this->sdf->Get<double>("distortionK3");
222 
223  if (!this->sdf->HasElement("distortionT1"))
224  {
225  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <distortionT1>, defaults to 0");
226  this->distortion_t1_= 0;
227  }
228  else
229  this->distortion_t1_ = this->sdf->Get<double>("distortionT1");
230 
231  if (!this->sdf->HasElement("distortionT2"))
232  {
233  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <distortionT2>, defaults to 0");
234  this->distortion_t2_= 0;
235  }
236  else
237  this->distortion_t2_ = this->sdf->Get<double>("distortionT2");
238 
239  if (!this->sdf->HasElement("borderCrop"))
240  {
241  ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing <borderCrop>, defaults to true");
242  this->border_crop_ = true;
243  }
244  else
245  this->border_crop_ = this->sdf->Get<bool>("borderCrop");
246 
247  // initialize shared_ptr members
248  if (!this->image_connect_count_) this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
250  if (!this->was_active_) this->was_active_ = boost::shared_ptr<bool>(new bool(false));
251 
252  // ros callback queue for processing subscription
253  this->deferred_load_thread_ = boost::thread(
254  boost::bind(&GazeboRosCameraUtils::LoadThread, this));
255 }
256 
257 event::ConnectionPtr GazeboRosCameraUtils::OnLoad(const boost::function<void()>& load_function)
258 {
259  return load_event_.Connect(load_function);
260 }
261 
263 // Load the controller
265 {
266  // Exit if no ROS
267  if (!ros::isInitialized())
268  {
269  gzerr << "Not loading plugin since ROS hasn't been "
270  << "properly initialized. Try starting gazebo with ros plugin:\n"
271  << " gazebo -s libgazebo_ros_api_plugin.so\n";
272  return;
273  }
274 
275  // Sensor generation off by default. Must do this before advertising the
276  // associated ROS topics.
277  this->parentSensor_->SetActive(false);
278 
279  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_ + "/" + this->camera_name_);
280 
281  // initialize camera_info_manager
283  *this->rosnode_, this->camera_name_));
284 
286 
287  // resolve tf prefix
288  this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
289  if(this->tf_prefix_.empty()) {
290  this->tf_prefix_ = this->robot_namespace_;
291  boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
292  }
293  this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
294 
295  ROS_INFO_NAMED("camera_utils", "Camera Plugin (ns = %s) <tf_prefix_>, set to \"%s\"",
296  this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
297 
298 
299  if (!this->camera_name_.empty())
300  {
301  dyn_srv_ =
302  new dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
303  (*this->rosnode_);
304  dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
305  ::CallbackType f =
306  boost::bind(&GazeboRosCameraUtils::configCallback, this, _1, _2);
307  dyn_srv_->setCallback(f);
308  }
309  else
310  {
311  ROS_WARN_NAMED("camera_utils", "dynamic reconfigure is not enabled for this image topic [%s]"
312  " becuase <cameraName> is not specified",
313  this->image_topic_name_.c_str());
314  }
315 
316  this->image_pub_ = this->itnode_->advertise(
317  this->image_topic_name_, 2,
318  boost::bind(&GazeboRosCameraUtils::ImageConnect, this),
319  boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this),
320  ros::VoidPtr(), true);
321 
322  // camera info publish rate will be synchronized to image sensor
323  // publish rates.
324  // If someone connects to camera_info, sensor will be activated
325  // and camera_info will be published alongside image_raw with the
326  // same timestamps. This incurrs additional computational cost when
327  // there are subscribers to camera_info, but better mimics behavior
328  // of image_pipeline.
330  ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
331  this->camera_info_topic_name_, 2,
332  boost::bind(&GazeboRosCameraUtils::ImageConnect, this),
333  boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this),
334  ros::VoidPtr(), &this->camera_queue_);
335  this->camera_info_pub_ = this->rosnode_->advertise(cio);
336 
337  /* disabling fov and rate setting for each camera
338  ros::SubscribeOptions zoom_so =
339  ros::SubscribeOptions::create<std_msgs::Float64>(
340  "set_hfov", 1,
341  boost::bind(&GazeboRosCameraUtils::SetHFOV, this, _1),
342  ros::VoidPtr(), &this->camera_queue_);
343  this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so);
344 
345  ros::SubscribeOptions rate_so =
346  ros::SubscribeOptions::create<std_msgs::Float64>(
347  "set_update_rate", 1,
348  boost::bind(&GazeboRosCameraUtils::SetUpdateRate, this, _1),
349  ros::VoidPtr(), &this->camera_queue_);
350  this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so);
351  */
352 
353  if (this->CanTriggerCamera())
354  {
355  ros::SubscribeOptions trigger_so =
356  ros::SubscribeOptions::create<std_msgs::Empty>(
357  this->trigger_topic_name_, 1,
358  boost::bind(&GazeboRosCameraUtils::TriggerCameraInternal, this, _1),
359  ros::VoidPtr(), &this->camera_queue_);
360  this->trigger_subscriber_ = this->rosnode_->subscribe(trigger_so);
361  }
362 
363  this->Init();
364 }
365 
367 {
368 }
369 
371 {
372  return false;
373 }
374 
376  const std_msgs::Empty::ConstPtr &dummy)
377 {
378  TriggerCamera();
379 }
380 
382 // Set Horizontal Field of View
383 void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov)
384 {
385 #if GAZEBO_MAJOR_VERSION >= 7
386  this->camera_->SetHFOV(ignition::math::Angle(hfov->data));
387 #else
388  this->camera_->SetHFOV(gazebo::math::Angle(hfov->data));
389 #endif
390 }
391 
393 // Set Update Rate
395  const std_msgs::Float64::ConstPtr& update_rate)
396 {
397  this->parentSensor_->SetUpdateRate(update_rate->data);
398 }
399 
401 // Increment count
403 {
404  boost::mutex::scoped_lock lock(*this->image_connect_count_lock_);
405 
406  // upon first connection, remember if camera was active.
407  if ((*this->image_connect_count_) == 0)
408  *this->was_active_ = this->parentSensor_->IsActive();
409 
410  (*this->image_connect_count_)++;
411 
412  this->parentSensor_->SetActive(true);
413 }
415 // Decrement count
417 {
418  boost::mutex::scoped_lock lock(*this->image_connect_count_lock_);
419 
420  (*this->image_connect_count_)--;
421 
422  // if there are no more subscribers, but camera was active to begin with,
423  // leave it active. Use case: this could be a multicamera, where
424  // each camera shares the same parentSensor_.
425  if ((*this->image_connect_count_) <= 0 && !*this->was_active_)
426  this->parentSensor_->SetActive(false);
427 }
428 
430 // Initialize the controller
432 {
433  // prepare to throttle this plugin at the same rate
434  // ideally, we should invoke a plugin update when the sensor updates,
435  // have to think about how to do that properly later
436  if (this->update_rate_ > 0.0)
437  this->update_period_ = 1.0/this->update_rate_;
438  else
439  this->update_period_ = 0.0;
440 
441  // set buffer size
442  if (this->format_ == "L8" || this->format_ == "L_INT8")
443  {
445  this->skip_ = 1;
446  }
447  else if (this->format_ == "L16" || this->format_ == "L_INT16")
448  {
450  this->skip_ = 2;
451  }
452  else if (this->format_ == "R8G8B8" || this->format_ == "RGB_INT8")
453  {
455  this->skip_ = 3;
456  }
457  else if (this->format_ == "B8G8R8" || this->format_ == "BGR_INT8")
458  {
460  this->skip_ = 3;
461  }
462  else if (this->format_ == "R16G16B16" || this->format_ == "RGB_INT16")
463  {
465  this->skip_ = 6;
466  }
467  else if (this->format_ == "BAYER_RGGB8")
468  {
469  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
471  this->skip_ = 1;
472  }
473  else if (this->format_ == "BAYER_BGGR8")
474  {
475  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
477  this->skip_ = 1;
478  }
479  else if (this->format_ == "BAYER_GBRG8")
480  {
481  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
483  this->skip_ = 1;
484  }
485  else if (this->format_ == "BAYER_GRBG8")
486  {
487  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
489  this->skip_ = 1;
490  }
491  else
492  {
493  ROS_ERROR_NAMED("camera_utils", "Unsupported Gazebo ImageFormat\n");
495  this->skip_ = 3;
496  }
497 
499  if (this->cx_prime_ == 0)
500  this->cx_prime_ = (static_cast<double>(this->width_) + 1.0) /2.0;
501  if (this->cx_ == 0)
502  this->cx_ = (static_cast<double>(this->width_) + 1.0) /2.0;
503  if (this->cy_ == 0)
504  this->cy_ = (static_cast<double>(this->height_) + 1.0) /2.0;
505 
506 
507  double hfov = this->camera_->HFOV().Radian();
508  double computed_focal_length =
509  (static_cast<double>(this->width_)) /
510  (2.0 * tan(hfov / 2.0));
511 
512  if (this->focal_length_ == 0)
513  {
514  this->focal_length_ = computed_focal_length;
515  }
516  else
517  {
518  // check against float precision
519  if (!ignition::math::equal(this->focal_length_, computed_focal_length))
520  {
521  ROS_WARN_NAMED("camera_utils", "The <focal_length>[%f] you have provided for camera_ [%s]"
522  " is inconsistent with specified image_width [%d] and"
523  " HFOV [%f]. Please double check to see that"
524  " focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
525  " the explected focal_lengtth value is [%f],"
526  " please update your camera_ model description accordingly.",
527  this->focal_length_, this->parentSensor_->Name().c_str(),
528  this->width_, hfov,
529  computed_focal_length);
530  }
531  }
532 
533  // fill CameraInfo
534  sensor_msgs::CameraInfo camera_info_msg;
535 
536  camera_info_msg.header.frame_id = this->frame_name_;
537 
538  camera_info_msg.height = this->height_;
539  camera_info_msg.width = this->width_;
540  // distortion
541 #if ROS_VERSION_MINIMUM(1, 3, 0)
542  camera_info_msg.distortion_model = "plumb_bob";
543  camera_info_msg.D.resize(5);
544 #endif
545  // Allow the user to disable automatic cropping (used to remove barrel
546  // distortion black border. The crop can be useful, but also skewes
547  // the lens distortion, making the supplied k and t values incorrect.
548  if(this->camera_->LensDistortion())
549  {
550  this->camera_->LensDistortion()->SetCrop(this->border_crop_);
551  }
552 
553  // D = {k1, k2, t1, t2, k3}, as specified in:
554  // - sensor_msgs/CameraInfo: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
555  // - OpenCV: http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
556  camera_info_msg.D[0] = this->distortion_k1_;
557  camera_info_msg.D[1] = this->distortion_k2_;
558  camera_info_msg.D[2] = this->distortion_t1_;
559  camera_info_msg.D[3] = this->distortion_t2_;
560  camera_info_msg.D[4] = this->distortion_k3_;
561  // original camera_ matrix
562  camera_info_msg.K[0] = this->focal_length_;
563  camera_info_msg.K[1] = 0.0;
564  camera_info_msg.K[2] = this->cx_;
565  camera_info_msg.K[3] = 0.0;
566  camera_info_msg.K[4] = this->focal_length_;
567  camera_info_msg.K[5] = this->cy_;
568  camera_info_msg.K[6] = 0.0;
569  camera_info_msg.K[7] = 0.0;
570  camera_info_msg.K[8] = 1.0;
571  // rectification
572  camera_info_msg.R[0] = 1.0;
573  camera_info_msg.R[1] = 0.0;
574  camera_info_msg.R[2] = 0.0;
575  camera_info_msg.R[3] = 0.0;
576  camera_info_msg.R[4] = 1.0;
577  camera_info_msg.R[5] = 0.0;
578  camera_info_msg.R[6] = 0.0;
579  camera_info_msg.R[7] = 0.0;
580  camera_info_msg.R[8] = 1.0;
581  // camera_ projection matrix (same as camera_ matrix due
582  // to lack of distortion/rectification) (is this generated?)
583  camera_info_msg.P[0] = this->focal_length_;
584  camera_info_msg.P[1] = 0.0;
585  camera_info_msg.P[2] = this->cx_;
586  camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_;
587  camera_info_msg.P[4] = 0.0;
588  camera_info_msg.P[5] = this->focal_length_;
589  camera_info_msg.P[6] = this->cy_;
590  camera_info_msg.P[7] = 0.0;
591  camera_info_msg.P[8] = 0.0;
592  camera_info_msg.P[9] = 0.0;
593  camera_info_msg.P[10] = 1.0;
594  camera_info_msg.P[11] = 0.0;
595 
596  this->camera_info_manager_->setCameraInfo(camera_info_msg);
597 
598  // start custom queue for camera_
599  this->callback_queue_thread_ = boost::thread(
600  boost::bind(&GazeboRosCameraUtils::CameraQueueThread, this));
601 
602  load_event_();
603  this->initialized_ = true;
604 }
605 
607 // Put camera_ data to the interface
608 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src,
609  common::Time &last_update_time)
610 {
611  this->sensor_update_time_ = last_update_time;
612  this->PutCameraData(_src);
613 }
614 
615 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src)
616 {
617  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
618  return;
619 
621  if ((*this->image_connect_count_) > 0)
622  {
623  boost::mutex::scoped_lock lock(this->lock_);
624 
625  // copy data into image
626  this->image_msg_.header.frame_id = this->frame_name_;
627  this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
628  this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
629 
630  // copy from src to image_msg_
631  fillImage(this->image_msg_, this->type_, this->height_, this->width_,
632  this->skip_*this->width_, reinterpret_cast<const void*>(_src));
633 
634  // publish to ros
635  this->image_pub_.publish(this->image_msg_);
636  }
637 }
638 
640 // Put camera_ data to the interface
641 void GazeboRosCameraUtils::PublishCameraInfo(common::Time &last_update_time)
642 {
643  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
644  return;
645 
646  this->sensor_update_time_ = last_update_time;
647  this->PublishCameraInfo();
648 }
649 
651 {
652  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
653  return;
654 
655  if (this->camera_info_pub_.getNumSubscribers() > 0)
656  {
657  this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
658  if (this->sensor_update_time_ - this->last_info_update_time_ >= this->update_period_)
659  {
660  this->PublishCameraInfo(this->camera_info_pub_);
662  }
663  }
664 }
665 
667  ros::Publisher camera_info_publisher)
668 {
669  sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo();
670 
671  camera_info_msg.header.stamp.sec = this->sensor_update_time_.sec;
672  camera_info_msg.header.stamp.nsec = this->sensor_update_time_.nsec;
673 
674  camera_info_publisher.publish(camera_info_msg);
675 }
676 
677 
679 // Put camera_ data to the interface
681 {
682  static const double timeout = 0.001;
683 
684  while (this->rosnode_->ok())
685  {
688  }
689 }
690 }
const std::string BAYER_GRBG8
std::string image_topic_name_
ROS image topic name.
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
std::string getPrefixParam(ros::NodeHandle &nh)
#define ROS_WARN_NAMED(name,...)
f
dynamic_reconfigure::Server< gazebo_plugins::GazeboRosCameraConfig > * dyn_srv_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
boost::shared_ptr< camera_info_manager::CameraInfoManager > camera_info_manager_
std::string robot_namespace_
for setting ROS name space
ROSCPP_DECL bool isInitialized()
boost::shared_ptr< bool > was_active_
Keep track when we activate this camera through ros subscription, was it already active? resume state when unsubscribed.
image_transport::ImageTransport * itnode_
image_transport::Publisher image_pub_
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
std::string camera_name_
ROS camera name.
void SetHFOV(const std_msgs::Float64::ConstPtr &hfov)
: Camera modification functions
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
const std::string BAYER_GBRG8
void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy)
std::string resolve(const std::string &prefix, const std::string &frame_name)
sensor_msgs::Image image_msg_
ROS image message.
#define ROS_DEBUG_NAMED(name,...)
ros::Publisher camera_info_pub_
camera info
event::ConnectionPtr OnLoad(const boost::function< void()> &)
void publish(const sensor_msgs::Image &message) const
bool initialized_
True if camera util is initialized.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double update_rate_
update rate of this sensor
const std::string MONO16
std::string frame_name_
ROS frame transform name to use in the image message header. This should typically match the link nam...
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
const std::string BAYER_BGGR8
uint32_t getNumSubscribers() const
void SetUpdateRate(const std_msgs::Float64::ConstPtr &update_rate)
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
std::string trigger_topic_name_
ROS trigger topic name.
#define ROS_ERROR_NAMED(name,...)
void configCallback(gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
const std::string BAYER_RGGB8
bool ok() const
std::string GetRobotNamespace(const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo=NULL)
Reads the name space tag of a sensor plugin.
std::string type_
size of image buffer
boost::shared_ptr< void > VoidPtr
boost::shared_ptr< boost::mutex > image_connect_count_lock_
A mutex to lock access to image_connect_count_.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27