gazebo_ros_openni_kinect.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 /*
19  Desc: GazeboRosOpenniKinect plugin for simulating cameras in Gazebo
20  Author: John Hsu
21  Date: 24 Sept 2008
22  */
23 
24 #include <algorithm>
25 #include <assert.h>
26 #include <boost/thread/thread.hpp>
27 #include <boost/bind.hpp>
28 
30 
31 #include <gazebo/sensors/Sensor.hh>
32 #include <sdf/sdf.hh>
33 #include <gazebo/sensors/SensorTypes.hh>
34 
35 #ifdef ENABLE_PROFILER
36 #include <ignition/common/Profiler.hh>
37 #endif
38 
40 
41 #include <tf/tf.h>
42 
43 namespace gazebo
44 {
45 // Register this plugin with the simulator
46 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosOpenniKinect)
47 
48 // Constructor
51 {
52  this->point_cloud_connect_count_ = 0;
53  this->depth_info_connect_count_ = 0;
54  this->depth_image_connect_count_ = 0;
55  this->last_depth_image_camera_info_update_time_ = common::Time(0);
56 }
57 
59 // Destructor
61 {
62 }
63 
65 // Load the controller
66 void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
67 {
68  DepthCameraPlugin::Load(_parent, _sdf);
69 
70  // copying from DepthCameraPlugin into GazeboRosCameraUtils
71  this->parentSensor_ = this->parentSensor;
72  this->width_ = this->width;
73  this->height_ = this->height;
74  this->depth_ = this->depth;
75  this->format_ = this->format;
76  this->camera_ = this->depthCamera;
77 
78  // using a different default
79  if (!_sdf->HasElement("imageTopicName"))
80  this->image_topic_name_ = "ir/image_raw";
81  if (!_sdf->HasElement("cameraInfoTopicName"))
82  this->camera_info_topic_name_ = "ir/camera_info";
83 
84  // point cloud stuff
85  if (!_sdf->HasElement("pointCloudTopicName"))
86  this->point_cloud_topic_name_ = "points";
87  else
88  this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get<std::string>();
89 
90  // depth image stuff
91  if (!_sdf->HasElement("depthImageTopicName"))
92  this->depth_image_topic_name_ = "depth/image_raw";
93  else
94  this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->Get<std::string>();
95 
96  if (!_sdf->HasElement("depthImageCameraInfoTopicName"))
97  this->depth_image_camera_info_topic_name_ = "depth/camera_info";
98  else
99  this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->Get<std::string>();
100 
101  if (!_sdf->HasElement("pointCloudCutoff"))
102  this->point_cloud_cutoff_ = 0.4;
103  else
104  this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<double>();
105  if (!_sdf->HasElement("pointCloudCutoffMax"))
106  this->point_cloud_cutoff_max_ = 5.0;
107  else
108  this->point_cloud_cutoff_max_ = _sdf->GetElement("pointCloudCutoffMax")->Get<double>();
109 
110  // allow optional publication of depth images in 16UC1 instead of 32FC1
111  if (!_sdf->HasElement("useDepth16UC1Format"))
112  this->use_depth_image_16UC1_format_ = false;
113  else
114  this->use_depth_image_16UC1_format_ = _sdf->GetElement("useDepth16UC1Format")->Get<bool>();
115 
117  GazeboRosCameraUtils::Load(_parent, _sdf);
118 }
119 
121 {
122  ros::AdvertiseOptions point_cloud_ao =
123  ros::AdvertiseOptions::create<sensor_msgs::PointCloud2 >(
124  this->point_cloud_topic_name_,1,
125  boost::bind( &GazeboRosOpenniKinect::PointCloudConnect,this),
127  ros::VoidPtr(), &this->camera_queue_);
128  this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao);
129 
130  ros::AdvertiseOptions depth_image_ao =
131  ros::AdvertiseOptions::create< sensor_msgs::Image >(
132  this->depth_image_topic_name_,1,
133  boost::bind( &GazeboRosOpenniKinect::DepthImageConnect,this),
135  ros::VoidPtr(), &this->camera_queue_);
136  this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao);
137 
138  ros::AdvertiseOptions depth_image_camera_info_ao =
139  ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
141  boost::bind( &GazeboRosOpenniKinect::DepthInfoConnect,this),
142  boost::bind( &GazeboRosOpenniKinect::DepthInfoDisconnect,this),
143  ros::VoidPtr(), &this->camera_queue_);
144  this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao);
145 }
146 
148 // Increment count
150 {
152  (*this->image_connect_count_)++;
153  this->parentSensor->SetActive(true);
154 }
156 // Decrement count
158 {
160  (*this->image_connect_count_)--;
161  if (this->point_cloud_connect_count_ <= 0)
162  this->parentSensor->SetActive(false);
163 }
164 
166 // Increment count
168 {
170  this->parentSensor->SetActive(true);
171 }
173 // Decrement count
175 {
177 }
178 
180 // Increment count
182 {
184 }
186 // Decrement count
188 {
190 }
191 
193 // Update the controller
194 void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image,
195  unsigned int _width, unsigned int _height, unsigned int _depth,
196  const std::string &_format)
197 {
198 #ifdef ENABLE_PROFILER
199  IGN_PROFILE("GazeboRosOpenniKinect::OnNewDepthFrame");
200 #endif
201  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
202  return;
203 #ifdef ENABLE_PROFILER
204  IGN_PROFILE_BEGIN("fill ROS message");
205 #endif
206  this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
207  if (this->parentSensor->IsActive())
208  {
209  if (this->point_cloud_connect_count_ <= 0 &&
210  this->depth_image_connect_count_ <= 0 &&
211  (*this->image_connect_count_) <= 0)
212  {
213  this->parentSensor->SetActive(false);
214  }
215  else
216  {
217  if (this->point_cloud_connect_count_ > 0)
218  this->FillPointdCloud(_image);
219 
220  if (this->depth_image_connect_count_ > 0)
221  this->FillDepthImage(_image);
222  }
223  }
224  else
225  {
226  if (this->point_cloud_connect_count_ > 0 ||
227  this->depth_image_connect_count_ <= 0)
228  // do this first so there's chance for sensor to run 1 frame after activate
229  this->parentSensor->SetActive(true);
230  }
231 #ifdef ENABLE_PROFILER
232  IGN_PROFILE_END();
233  IGN_PROFILE_BEGIN("PublishCameraInfo");
234 #endif
236 #ifdef ENABLE_PROFILER
237  IGN_PROFILE_END();
238 #endif
239 }
240 
242 // Update the controller
243 void GazeboRosOpenniKinect::OnNewImageFrame(const unsigned char *_image,
244  unsigned int _width, unsigned int _height, unsigned int _depth,
245  const std::string &_format)
246 {
247  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
248  return;
249 
250  //ROS_ERROR_NAMED("openni_kinect", "camera_ new frame %s %s",this->parentSensor_->Name().c_str(),this->frame_name_.c_str());
251  this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
252 
253  if (this->parentSensor->IsActive())
254  {
255  if (this->point_cloud_connect_count_ <= 0 &&
256  this->depth_image_connect_count_ <= 0 &&
257  (*this->image_connect_count_) <= 0)
258  {
259  this->parentSensor->SetActive(false);
260  }
261  else
262  {
263  if ((*this->image_connect_count_) > 0)
264  this->PutCameraData(_image);
265  }
266  }
267  else
268  {
269  if ((*this->image_connect_count_) > 0)
270  // do this first so there's chance for sensor to run 1 frame after activate
271  this->parentSensor->SetActive(true);
272  }
273 }
274 
276 // Put point cloud data to the interface
278 {
279  this->lock_.lock();
280 
281  this->point_cloud_msg_.header.frame_id = this->frame_name_;
282  this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
283  this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
284  this->point_cloud_msg_.width = this->width;
285  this->point_cloud_msg_.height = this->height;
286  this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width;
287 
290  this->height,
291  this->width,
292  this->skip_,
293  (void*)_src );
294 
296 
297  this->lock_.unlock();
298 }
299 
301 // Put depth image data to the interface
303 {
304  this->lock_.lock();
305  // copy data into image
306  this->depth_image_msg_.header.frame_id = this->frame_name_;
307  this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
308  this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
309 
312  this->height,
313  this->width,
314  this->skip_,
315  (void*)_src );
316 
318 
319  this->lock_.unlock();
320 }
321 
322 
323 // Fill depth information
325  sensor_msgs::PointCloud2 &point_cloud_msg,
326  uint32_t rows_arg, uint32_t cols_arg,
327  uint32_t step_arg, void* data_arg)
328 {
329  sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg);
330  pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
331  // convert to flat array shape, we need to reconvert later
332  pcd_modifier.resize(rows_arg*cols_arg);
333  point_cloud_msg.is_dense = true;
334 
339 
340  float* toCopyFrom = (float*)data_arg;
341  int index = 0;
342 
343  double hfov = this->parentSensor->DepthCamera()->HFOV().Radian();
344  double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));
345 
346  // convert depth to point cloud
347  for (uint32_t j=0; j<rows_arg; j++)
348  {
349  double pAngle;
350  if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl);
351  else pAngle = 0.0;
352 
353  for (uint32_t i=0; i<cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
354  {
355  double yAngle;
356  if (cols_arg>1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl);
357  else yAngle = 0.0;
358 
359  double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip();
360 
361  if(depth > this->point_cloud_cutoff_ &&
362  depth < this->point_cloud_cutoff_max_)
363  {
364  // in optical frame
365  // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
366  // to urdf, where the *_optical_frame should have above relative
367  // rotation from the physical camera *_frame
368  *iter_x = depth * tan(yAngle);
369  *iter_y = depth * tan(pAngle);
370  *iter_z = depth;
371  }
372  else //point in the unseeable range
373  {
374  *iter_x = *iter_y = *iter_z = std::numeric_limits<float>::quiet_NaN ();
375  point_cloud_msg.is_dense = false;
376  }
377 
378  // put image color data for each point
379  uint8_t* image_src = (uint8_t*)(&(this->image_msg_.data[0]));
380  if (this->image_msg_.data.size() == rows_arg*cols_arg*3)
381  {
382  // color
383  iter_rgb[0] = image_src[i*3+j*cols_arg*3+0];
384  iter_rgb[1] = image_src[i*3+j*cols_arg*3+1];
385  iter_rgb[2] = image_src[i*3+j*cols_arg*3+2];
386  }
387  else if (this->image_msg_.data.size() == rows_arg*cols_arg)
388  {
389  // mono (or bayer? @todo; fix for bayer)
390  iter_rgb[0] = image_src[i+j*cols_arg];
391  iter_rgb[1] = image_src[i+j*cols_arg];
392  iter_rgb[2] = image_src[i+j*cols_arg];
393  }
394  else
395  {
396  // no image
397  iter_rgb[0] = 0;
398  iter_rgb[1] = 0;
399  iter_rgb[2] = 0;
400  }
401  }
402  }
403 
404  // reconvert to original height and width after the flat reshape
405  point_cloud_msg.height = rows_arg;
406  point_cloud_msg.width = cols_arg;
407  point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width;
408 
409  return true;
410 }
411 
412 // Fill depth information
414  sensor_msgs::Image& image_msg,
415  uint32_t rows_arg, uint32_t cols_arg,
416  uint32_t step_arg, void* data_arg)
417 {
418  image_msg.height = rows_arg;
419  image_msg.width = cols_arg;
420  image_msg.is_bigendian = 0;
421  // deal with the differences in between 32FC1 & 16UC1
422  // http://www.ros.org/reps/rep-0118.html#id4
423  union uint16_or_float
424  {
425  uint16_t* dest_uint16;
426  float* dest_float;
427  };
428  uint16_or_float dest;
430  {
431  image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
432  image_msg.step = sizeof(float) * cols_arg;
433  image_msg.data.resize(rows_arg * cols_arg * sizeof(float));
434  dest.dest_float = (float*)(&(image_msg.data[0]));
435  }
436  else
437  {
438  image_msg.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
439  image_msg.step = sizeof(uint16_t) * cols_arg;
440  image_msg.data.resize(rows_arg * cols_arg * sizeof(uint16_t));
441  dest.dest_uint16 = (uint16_t*)(&(image_msg.data[0]));
442  }
443 
444  float* toCopyFrom = (float*)data_arg;
445  int index = 0;
446 
447  // convert depth to point cloud
448  for (uint32_t j = 0; j < rows_arg; j++)
449  {
450  for (uint32_t i = 0; i < cols_arg; i++)
451  {
452  float depth = toCopyFrom[index++];
453 
454  if (depth > this->point_cloud_cutoff_ &&
455  depth < this->point_cloud_cutoff_max_)
456  {
458  dest.dest_float[i + j * cols_arg] = depth;
459  else
460  dest.dest_uint16[i + j * cols_arg] = depth * 1000.0;
461  }
462  else //point in the unseeable range
463  {
465  dest.dest_float[i + j * cols_arg] = std::numeric_limits<float>::quiet_NaN();
466  else
467  dest.dest_uint16[i + j * cols_arg] = 0;
468  }
469  }
470  }
471  return true;
472 }
473 
475 {
476  ROS_DEBUG_NAMED("openni_kinect", "publishing default camera info, then openni kinect camera info");
478 
479  if (this->depth_info_connect_count_ > 0)
480  {
481  this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
482 #if GAZEBO_MAJOR_VERSION >= 8
483  common::Time cur_time = this->world_->SimTime();
484 #else
485  common::Time cur_time = this->world_->GetSimTime();
486 #endif
488  {
491  }
492  }
493 }
494 
495 //@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp.
496 /*
497 #include <stereo_msgs/DisparityImage.h>
498 pub_disparity_ = comm_nh.advertise<stereo_msgs::DisparityImage > ("depth/disparity", 5, subscriberChanged2, subscriberChanged2);
499 
500 void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time)
501 {
502  stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared<stereo_msgs::DisparityImage > ();
503  disp_msg->header.stamp = time;
504  disp_msg->header.frame_id = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_;
505  disp_msg->image.header = disp_msg->header;
506  disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
507  disp_msg->image.height = depth_height_;
508  disp_msg->image.width = depth_width_;
509  disp_msg->image.step = disp_msg->image.width * sizeof (float);
510  disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step);
511  disp_msg->T = depth.getBaseline ();
512  disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth ();
513 
515  disp_msg->min_disparity = 0.0;
516  disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3;
517  disp_msg->delta_d = 0.125;
518 
519  depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast<float*>(&disp_msg->image.data[0]), disp_msg->image.step);
520 
521  pub_disparity_.publish (disp_msg);
522 }
523 */
524 
525 }
gazebo::GazeboRosCameraUtils::initialized_
bool initialized_
True if camera util is initialized.
Definition: gazebo_ros_camera_utils.h:224
gazebo::GazeboRosOpenniKinect::depth_info_connect_count_
int depth_info_connect_count_
Definition: gazebo_ros_openni_kinect.h:133
point_cloud2_iterator.h
gazebo::GazeboRosOpenniKinect::DepthInfoDisconnect
void DepthInfoDisconnect()
Definition: gazebo_ros_openni_kinect.cpp:187
gazebo::GazeboRosOpenniKinect::point_cloud_topic_name_
std::string point_cloud_topic_name_
ROS image topic name.
Definition: gazebo_ros_openni_kinect.h:124
gazebo::GazeboRosOpenniKinect::point_cloud_cutoff_max_
double point_cloud_cutoff_max_
Maximum range of the point cloud.
Definition: gazebo_ros_openni_kinect.h:121
gazebo::GazeboRosOpenniKinect::point_cloud_pub_
ros::Publisher point_cloud_pub_
A pointer to the ROS node. A node will be instantiated if it does not exist.
Definition: gazebo_ros_openni_kinect.h:111
boost::shared_ptr< void >
gazebo::GazeboRosCameraUtils::parentSensor_
sensors::SensorPtr parentSensor_
Definition: gazebo_ros_camera_utils.h:193
gazebo
gazebo::GazeboRosOpenniKinect::DepthImageDisconnect
void DepthImageDisconnect()
Definition: gazebo_ros_openni_kinect.cpp:174
gazebo::GazeboRosOpenniKinect::point_cloud_cutoff_
double point_cloud_cutoff_
Minimum range of the point cloud.
Definition: gazebo_ros_openni_kinect.h:119
gazebo::GazeboRosOpenniKinect::PointCloudDisconnect
void PointCloudDisconnect()
Definition: gazebo_ros_openni_kinect.cpp:157
gazebo::GazeboRosCameraUtils::PutCameraData
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
Definition: gazebo_ros_camera_utils.cpp:639
gazebo::GazeboRosOpenniKinect::last_depth_image_camera_info_update_time_
common::Time last_depth_image_camera_info_update_time_
Definition: gazebo_ros_openni_kinect.h:136
gazebo::GazeboRosCameraUtils::image_topic_name_
std::string image_topic_name_
ROS image topic name.
Definition: gazebo_ros_camera_utils.h:126
gazebo::GazeboRosCameraUtils::width_
unsigned int width_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosOpenniKinect::FillPointCloudHelper
bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)
Definition: gazebo_ros_openni_kinect.cpp:324
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
gazebo::GazeboRosCameraUtils::rosnode_
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
Definition: gazebo_ros_camera_utils.h:109
ros::AdvertiseOptions
sensor_msgs::image_encodings::TYPE_16UC1
const std::string TYPE_16UC1
gazebo::GazeboRosOpenniKinect::DepthInfoConnect
void DepthInfoConnect()
Definition: gazebo_ros_openni_kinect.cpp:181
gazebo::GazeboRosCameraUtils::height_
unsigned int height_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosCameraUtils::PublishCameraInfo
void PublishCameraInfo()
Definition: gazebo_ros_camera_utils.cpp:674
gazebo::GazeboRosOpenniKinect::PointCloudConnect
void PointCloudConnect()
Definition: gazebo_ros_openni_kinect.cpp:149
sensor_msgs::PointCloud2Iterator
gazebo::GazeboRosOpenniKinect::depth_image_camera_info_pub_
ros::Publisher depth_image_camera_info_pub_
Definition: gazebo_ros_openni_kinect.h:138
gazebo::GazeboRosOpenniKinect::Advertise
virtual void Advertise()
Advertise point cloud and depth image.
Definition: gazebo_ros_openni_kinect.cpp:120
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
gazebo::GazeboRosCameraUtils::frame_name_
std::string frame_name_
ROS frame transform name to use in the image message header. This should typically match the link nam...
Definition: gazebo_ros_camera_utils.h:142
gazebo::GazeboRosOpenniKinect::depth_image_msg_
sensor_msgs::Image depth_image_msg_
Definition: gazebo_ros_openni_kinect.h:116
gazebo::GazeboRosOpenniKinect::point_cloud_connect_count_
int point_cloud_connect_count_
Keep track of number of connctions for point clouds.
Definition: gazebo_ros_openni_kinect.h:93
gazebo::GazeboRosOpenniKinect::depth_sensor_update_time_
common::Time depth_sensor_update_time_
Definition: gazebo_ros_openni_kinect.h:129
gazebo::GazeboRosCameraUtils::image_connect_count_
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
Definition: gazebo_ros_camera_utils.h:92
gazebo::GazeboRosCameraUtils::update_period_
double update_period_
Definition: gazebo_ros_camera_utils.h:145
gazebo::GazeboRosCameraUtils::sensor_update_time_
common::Time sensor_update_time_
Definition: gazebo_ros_camera_utils.h:201
gazebo::GazeboRosOpenniKinect::load_connection_
event::ConnectionPtr load_connection_
Definition: gazebo_ros_openni_kinect.h:143
gazebo::GazeboRosOpenniKinect::FillPointdCloud
void FillPointdCloud(const float *_src)
push point cloud data into ros topic
Definition: gazebo_ros_openni_kinect.cpp:277
gazebo::GazeboRosCameraUtils::depth_
unsigned int depth_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosOpenniKinect::FillDepthImage
void FillDepthImage(const float *_src)
push depth image data into ros topic
Definition: gazebo_ros_openni_kinect.cpp:302
gazebo::GazeboRosCameraUtils::lock_
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
Definition: gazebo_ros_camera_utils.h:167
gazebo::GazeboRosCameraUtils::Load
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
Definition: gazebo_ros_camera_utils.cpp:104
gazebo::GazeboRosOpenniKinect::~GazeboRosOpenniKinect
~GazeboRosOpenniKinect()
Destructor.
Definition: gazebo_ros_openni_kinect.cpp:60
gazebo::GazeboRosOpenniKinect::FillDepthImageHelper
bool FillDepthImageHelper(sensor_msgs::Image &image_msg, uint32_t height, uint32_t width, uint32_t step, void *data_arg)
Definition: gazebo_ros_openni_kinect.cpp:413
tf.h
gazebo::GazeboRosOpenniKinect::Load
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
Definition: gazebo_ros_openni_kinect.cpp:66
sensor_msgs::PointCloud2Modifier::resize
void resize(size_t size)
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
gazebo::GazeboRosOpenniKinect::use_depth_image_16UC1_format_
bool use_depth_image_16UC1_format_
Definition: gazebo_ros_openni_kinect.h:137
sensor_msgs::PointCloud2Modifier
gazebo::GazeboRosCameraUtils::skip_
int skip_
Definition: gazebo_ros_camera_utils.h:171
gazebo::GazeboRosOpenniKinect::depth_image_connect_count_
int depth_image_connect_count_
Keep track of number of connctions for point clouds.
Definition: gazebo_ros_openni_kinect.h:98
gazebo::GazeboRosOpenniKinect::depth_image_camera_info_topic_name_
std::string depth_image_camera_info_topic_name_
Definition: gazebo_ros_openni_kinect.h:132
gazebo::GazeboRosCameraUtils::format_
std::string format_
Definition: gazebo_ros_camera_utils.h:191
gazebo::GazeboRosCameraUtils::image_msg_
sensor_msgs::Image image_msg_
ROS image message.
Definition: gazebo_ros_camera_utils.h:114
gazebo::GazeboRosCameraUtils::world_
physics::WorldPtr world_
Definition: gazebo_ros_camera_utils.h:197
gazebo::GazeboRosOpenniKinect
Definition: gazebo_ros_openni_kinect.h:60
assert.h
gazebo::GazeboRosCameraUtils::camera_info_topic_name_
std::string camera_info_topic_name_
Definition: gazebo_ros_camera_utils.h:137
gazebo::GazeboRosOpenniKinect::depth_image_topic_name_
std::string depth_image_topic_name_
image where each pixel contains the depth data
Definition: gazebo_ros_openni_kinect.h:128
gazebo::GazeboRosOpenniKinect::OnNewImageFrame
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
Definition: gazebo_ros_openni_kinect.cpp:243
gazebo::GazeboRosCameraUtils::camera_queue_
ros::CallbackQueue camera_queue_
Definition: gazebo_ros_camera_utils.h:184
gazebo::GazeboRosCameraUtils::camera_
rendering::CameraPtr camera_
Definition: gazebo_ros_camera_utils.h:194
gazebo::GazeboRosOpenniKinect::PublishCameraInfo
virtual void PublishCameraInfo()
Definition: gazebo_ros_openni_kinect.cpp:474
sensor_msgs::PointCloud2Modifier::setPointCloud2FieldsByString
void setPointCloud2FieldsByString(int n_fields,...)
gazebo::GazeboRosOpenniKinect::point_cloud_msg_
sensor_msgs::PointCloud2 point_cloud_msg_
PointCloud2 point cloud message.
Definition: gazebo_ros_openni_kinect.h:115
gazebo::GazeboRosOpenniKinect::DepthImageConnect
void DepthImageConnect()
Definition: gazebo_ros_openni_kinect.cpp:167
gazebo::GazeboRosCameraUtils::OnLoad
event::ConnectionPtr OnLoad(const boost::function< void()> &)
Definition: gazebo_ros_camera_utils.cpp:266
gazebo::GazeboRosOpenniKinect::OnNewDepthFrame
virtual void OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
Definition: gazebo_ros_openni_kinect.cpp:194
gazebo::GazeboRosOpenniKinect::depth_image_pub_
ros::Publisher depth_image_pub_
Definition: gazebo_ros_openni_kinect.h:112
gazebo_ros_openni_kinect.h


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Mar 2 2022 00:24:25