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  " because <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 incurs 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  this->camera_->SetHFOV(ignition::math::Angle(hfov->data));
386 }
387 
389 // Set Update Rate
391  const std_msgs::Float64::ConstPtr& update_rate)
392 {
393  this->parentSensor_->SetUpdateRate(update_rate->data);
394 }
395 
397 // Increment count
399 {
400  boost::mutex::scoped_lock lock(*this->image_connect_count_lock_);
401 
402  // upon first connection, remember if camera was active.
403  if ((*this->image_connect_count_) == 0)
404  *this->was_active_ = this->parentSensor_->IsActive();
405 
406  (*this->image_connect_count_)++;
407 
408  this->parentSensor_->SetActive(true);
409 }
411 // Decrement count
413 {
414  boost::mutex::scoped_lock lock(*this->image_connect_count_lock_);
415 
416  (*this->image_connect_count_)--;
417 
418  // if there are no more subscribers, but camera was active to begin with,
419  // leave it active. Use case: this could be a multicamera, where
420  // each camera shares the same parentSensor_.
421  if ((*this->image_connect_count_) <= 0 && !*this->was_active_)
422  this->parentSensor_->SetActive(false);
423 }
424 
426 // Initialize the controller
428 {
429  // prepare to throttle this plugin at the same rate
430  // ideally, we should invoke a plugin update when the sensor updates,
431  // have to think about how to do that properly later
432  if (this->update_rate_ > 0.0)
433  this->update_period_ = 1.0/this->update_rate_;
434  else
435  this->update_period_ = 0.0;
436 
437  // set buffer size
438  if (this->format_ == "L8" || this->format_ == "L_INT8")
439  {
441  this->skip_ = 1;
442  }
443  else if (this->format_ == "L16" || this->format_ == "L_INT16")
444  {
446  this->skip_ = 2;
447  }
448  else if (this->format_ == "R8G8B8" || this->format_ == "RGB_INT8")
449  {
451  this->skip_ = 3;
452  }
453  else if (this->format_ == "B8G8R8" || this->format_ == "BGR_INT8")
454  {
456  this->skip_ = 3;
457  }
458  else if (this->format_ == "R16G16B16" || this->format_ == "RGB_INT16")
459  {
461  this->skip_ = 6;
462  }
463  else if (this->format_ == "BAYER_RGGB8")
464  {
465  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
467  this->skip_ = 1;
468  }
469  else if (this->format_ == "BAYER_BGGR8")
470  {
471  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
473  this->skip_ = 1;
474  }
475  else if (this->format_ == "BAYER_GBRG8")
476  {
477  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
479  this->skip_ = 1;
480  }
481  else if (this->format_ == "BAYER_GRBG8")
482  {
483  ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive.");
485  this->skip_ = 1;
486  }
487  else
488  {
489  ROS_ERROR_NAMED("camera_utils", "Unsupported Gazebo ImageFormat\n");
491  this->skip_ = 3;
492  }
493 
495  if (this->cx_prime_ == 0)
496  this->cx_prime_ = (static_cast<double>(this->width_) + 1.0) /2.0;
497  if (this->cx_ == 0)
498  this->cx_ = (static_cast<double>(this->width_) + 1.0) /2.0;
499  if (this->cy_ == 0)
500  this->cy_ = (static_cast<double>(this->height_) + 1.0) /2.0;
501 
502 
503  double hfov = this->camera_->HFOV().Radian();
504  double computed_focal_length =
505  (static_cast<double>(this->width_)) /
506  (2.0 * tan(hfov / 2.0));
507 
508  if (this->focal_length_ == 0)
509  {
510  this->focal_length_ = computed_focal_length;
511  }
512  else
513  {
514  // check against float precision
515  if (!ignition::math::equal(this->focal_length_, computed_focal_length))
516  {
517  ROS_WARN_NAMED("camera_utils", "The <focal_length>[%f] you have provided for camera_ [%s]"
518  " is inconsistent with specified image_width [%d] and"
519  " HFOV [%f]. Please double check to see that"
520  " focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
521  " the expected focal_length value is [%f],"
522  " please update your camera_ model description accordingly.",
523  this->focal_length_, this->parentSensor_->Name().c_str(),
524  this->width_, hfov,
525  computed_focal_length);
526  }
527  }
528 
529  // fill CameraInfo
530  sensor_msgs::CameraInfo camera_info_msg;
531 
532  camera_info_msg.header.frame_id = this->frame_name_;
533 
534  camera_info_msg.height = this->height_;
535  camera_info_msg.width = this->width_;
536  // distortion
537 #if ROS_VERSION_MINIMUM(1, 3, 0)
538  camera_info_msg.distortion_model = "plumb_bob";
539  camera_info_msg.D.resize(5);
540 #endif
541  // Allow the user to disable automatic cropping (used to remove barrel
542  // distortion black border. The crop can be useful, but also skews
543  // the lens distortion, making the supplied k and t values incorrect.
544  if(this->camera_->LensDistortion())
545  {
546  this->camera_->LensDistortion()->SetCrop(this->border_crop_);
547  }
548 
549  // D = {k1, k2, t1, t2, k3}, as specified in:
550  // - sensor_msgs/CameraInfo: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
551  // - OpenCV: http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
552  camera_info_msg.D[0] = this->distortion_k1_;
553  camera_info_msg.D[1] = this->distortion_k2_;
554  camera_info_msg.D[2] = this->distortion_t1_;
555  camera_info_msg.D[3] = this->distortion_t2_;
556  camera_info_msg.D[4] = this->distortion_k3_;
557  // original camera_ matrix
558  camera_info_msg.K[0] = this->focal_length_;
559  camera_info_msg.K[1] = 0.0;
560  camera_info_msg.K[2] = this->cx_;
561  camera_info_msg.K[3] = 0.0;
562  camera_info_msg.K[4] = this->focal_length_;
563  camera_info_msg.K[5] = this->cy_;
564  camera_info_msg.K[6] = 0.0;
565  camera_info_msg.K[7] = 0.0;
566  camera_info_msg.K[8] = 1.0;
567  // rectification
568  camera_info_msg.R[0] = 1.0;
569  camera_info_msg.R[1] = 0.0;
570  camera_info_msg.R[2] = 0.0;
571  camera_info_msg.R[3] = 0.0;
572  camera_info_msg.R[4] = 1.0;
573  camera_info_msg.R[5] = 0.0;
574  camera_info_msg.R[6] = 0.0;
575  camera_info_msg.R[7] = 0.0;
576  camera_info_msg.R[8] = 1.0;
577  // camera_ projection matrix (same as camera_ matrix due
578  // to lack of distortion/rectification) (is this generated?)
579  camera_info_msg.P[0] = this->focal_length_;
580  camera_info_msg.P[1] = 0.0;
581  camera_info_msg.P[2] = this->cx_;
582  camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_;
583  camera_info_msg.P[4] = 0.0;
584  camera_info_msg.P[5] = this->focal_length_;
585  camera_info_msg.P[6] = this->cy_;
586  camera_info_msg.P[7] = 0.0;
587  camera_info_msg.P[8] = 0.0;
588  camera_info_msg.P[9] = 0.0;
589  camera_info_msg.P[10] = 1.0;
590  camera_info_msg.P[11] = 0.0;
591 
592  this->camera_info_manager_->setCameraInfo(camera_info_msg);
593 
594  // start custom queue for camera_
595  this->callback_queue_thread_ = boost::thread(
596  boost::bind(&GazeboRosCameraUtils::CameraQueueThread, this));
597 
598  load_event_();
599  this->initialized_ = true;
600 }
601 
603 // Put camera_ data to the interface
604 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src,
605  common::Time &last_update_time)
606 {
607  this->sensor_update_time_ = last_update_time;
608  this->PutCameraData(_src);
609 }
610 
611 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src)
612 {
613  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
614  return;
615 
617  if ((*this->image_connect_count_) > 0)
618  {
619  boost::mutex::scoped_lock lock(this->lock_);
620 
621  // copy data into image
622  this->image_msg_.header.frame_id = this->frame_name_;
623  this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
624  this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
625 
626  // copy from src to image_msg_
627  fillImage(this->image_msg_, this->type_, this->height_, this->width_,
628  this->skip_*this->width_, reinterpret_cast<const void*>(_src));
629 
630  // publish to ros
631  this->image_pub_.publish(this->image_msg_);
632  }
633 }
634 
636 // Put camera_ data to the interface
637 void GazeboRosCameraUtils::PublishCameraInfo(common::Time &last_update_time)
638 {
639  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
640  return;
641 
642  this->sensor_update_time_ = last_update_time;
643  this->PublishCameraInfo();
644 }
645 
647 {
648  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
649  return;
650 
651  if (this->camera_info_pub_.getNumSubscribers() > 0)
652  {
653  this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
654  if (this->sensor_update_time_ - this->last_info_update_time_ >= this->update_period_)
655  {
656  this->PublishCameraInfo(this->camera_info_pub_);
658  }
659  }
660 }
661 
663  ros::Publisher camera_info_publisher)
664 {
665  sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo();
666 
667  camera_info_msg.header.stamp.sec = this->sensor_update_time_.sec;
668  camera_info_msg.header.stamp.nsec = this->sensor_update_time_.nsec;
669 
670  camera_info_publisher.publish(camera_info_msg);
671 }
672 
673 
675 // Put camera_ data to the interface
677 {
678  static const double timeout = 0.001;
679 
680  while (this->rosnode_->ok())
681  {
684  }
685 }
686 }
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 Apr 6 2021 02:19:39