gazebo_ros_depth_camera.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 /*
00018    Desc: GazeboRosDepthCamera plugin for simulating cameras in Gazebo
00019    Author: John Hsu
00020    Date: 24 Sept 2008
00021  */
00022 
00023 #include <algorithm>
00024 #include <assert.h>
00025 #include <boost/thread/thread.hpp>
00026 #include <boost/bind.hpp>
00027 
00028 #include <gazebo_plugins/gazebo_ros_depth_camera.h>
00029 
00030 #include <gazebo/sensors/Sensor.hh>
00031 #include <sdf/sdf.hh>
00032 #include <gazebo/sensors/SensorTypes.hh>
00033 
00034 #include <sensor_msgs/point_cloud2_iterator.h>
00035 
00036 #include <tf/tf.h>
00037 
00038 namespace gazebo
00039 {
00040 // Register this plugin with the simulator
00041 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosDepthCamera)
00042 
00043 
00044 // Constructor
00045 GazeboRosDepthCamera::GazeboRosDepthCamera()
00046 {
00047   this->point_cloud_connect_count_ = 0;
00048   this->depth_info_connect_count_ = 0;
00049   this->last_depth_image_camera_info_update_time_ = common::Time(0);
00050 }
00051 
00053 // Destructor
00054 GazeboRosDepthCamera::~GazeboRosDepthCamera()
00055 {
00056 }
00057 
00059 // Load the controller
00060 void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00061 {
00062   DepthCameraPlugin::Load(_parent, _sdf);
00063 
00064   // Make sure the ROS node for Gazebo has already been initialized
00065   if (!ros::isInitialized())
00066   {
00067     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00068       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00069     return;
00070   }
00071 
00072   // copying from DepthCameraPlugin into GazeboRosCameraUtils
00073   this->parentSensor_ = this->parentSensor;
00074   this->width_ = this->width;
00075   this->height_ = this->height;
00076   this->depth_ = this->depth;
00077   this->format_ = this->format;
00078   this->camera_ = this->depthCamera;
00079 
00080   // using a different default
00081   if (!_sdf->HasElement("imageTopicName"))
00082     this->image_topic_name_ = "ir/image_raw";
00083   if (!_sdf->HasElement("cameraInfoTopicName"))
00084     this->camera_info_topic_name_ = "ir/camera_info";
00085 
00086   // point cloud stuff
00087   if (!_sdf->HasElement("pointCloudTopicName"))
00088     this->point_cloud_topic_name_ = "points";
00089   else
00090     this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get<std::string>();
00091 
00092   // depth image stuff
00093   if (!_sdf->HasElement("depthImageTopicName"))
00094     this->depth_image_topic_name_ = "depth/image_raw";
00095   else
00096     this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->Get<std::string>();
00097 
00098   if (!_sdf->HasElement("depthImageCameraInfoTopicName"))
00099     this->depth_image_camera_info_topic_name_ = "depth/camera_info";
00100   else
00101     this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->Get<std::string>();
00102 
00103   if (!_sdf->HasElement("pointCloudCutoff"))
00104     this->point_cloud_cutoff_ = 0.4;
00105   else
00106     this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<double>();
00107 
00108   load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosDepthCamera::Advertise, this));
00109   GazeboRosCameraUtils::Load(_parent, _sdf);
00110 }
00111 
00112 void GazeboRosDepthCamera::Advertise()
00113 {
00114   ros::AdvertiseOptions point_cloud_ao =
00115     ros::AdvertiseOptions::create<sensor_msgs::PointCloud2 >(
00116       this->point_cloud_topic_name_,1,
00117       boost::bind( &GazeboRosDepthCamera::PointCloudConnect,this),
00118       boost::bind( &GazeboRosDepthCamera::PointCloudDisconnect,this),
00119       ros::VoidPtr(), &this->camera_queue_);
00120   this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao);
00121 
00122   ros::AdvertiseOptions depth_image_ao =
00123     ros::AdvertiseOptions::create< sensor_msgs::Image >(
00124       this->depth_image_topic_name_,1,
00125       boost::bind( &GazeboRosDepthCamera::DepthImageConnect,this),
00126       boost::bind( &GazeboRosDepthCamera::DepthImageDisconnect,this),
00127       ros::VoidPtr(), &this->camera_queue_);
00128   this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao);
00129 
00130   ros::AdvertiseOptions depth_image_camera_info_ao =
00131     ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
00132         this->depth_image_camera_info_topic_name_,1,
00133         boost::bind( &GazeboRosDepthCamera::DepthInfoConnect,this),
00134         boost::bind( &GazeboRosDepthCamera::DepthInfoDisconnect,this),
00135         ros::VoidPtr(), &this->camera_queue_);
00136   this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao);
00137 }
00138 
00139 
00141 // Increment count
00142 void GazeboRosDepthCamera::PointCloudConnect()
00143 {
00144   this->point_cloud_connect_count_++;
00145   (*this->image_connect_count_)++;
00146   this->parentSensor->SetActive(true);
00147 }
00149 // Decrement count
00150 void GazeboRosDepthCamera::PointCloudDisconnect()
00151 {
00152   this->point_cloud_connect_count_--;
00153   (*this->image_connect_count_)--;
00154   if (this->point_cloud_connect_count_ <= 0)
00155     this->parentSensor->SetActive(false);
00156 }
00157 
00159 // Increment count
00160 void GazeboRosDepthCamera::DepthImageConnect()
00161 {
00162   this->depth_image_connect_count_++;
00163   this->parentSensor->SetActive(true);
00164 }
00166 // Decrement count
00167 void GazeboRosDepthCamera::DepthImageDisconnect()
00168 {
00169   this->depth_image_connect_count_--;
00170 }
00171 
00173 // Increment count
00174 void GazeboRosDepthCamera::DepthInfoConnect()
00175 {
00176   this->depth_info_connect_count_++;
00177 }
00179 // Decrement count
00180 void GazeboRosDepthCamera::DepthInfoDisconnect()
00181 {
00182   this->depth_info_connect_count_--;
00183 }
00184 
00186 // Update the controller
00187 void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image,
00188     unsigned int _width, unsigned int _height, unsigned int _depth,
00189     const std::string &_format)
00190 {
00191   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00192     return;
00193 
00194 # if GAZEBO_MAJOR_VERSION >= 7
00195   this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
00196 # else
00197   this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
00198 # endif
00199   if (this->parentSensor->IsActive())
00200   {
00201     if (this->point_cloud_connect_count_ <= 0 &&
00202         this->depth_image_connect_count_ <= 0 &&
00203         (*this->image_connect_count_) <= 0)
00204     {
00205       this->parentSensor->SetActive(false);
00206     }
00207     else
00208     {
00209       if (this->point_cloud_connect_count_ > 0)
00210         this->FillPointdCloud(_image);
00211 
00212       if (this->depth_image_connect_count_ > 0)
00213         this->FillDepthImage(_image);
00214     }
00215   }
00216   else
00217   {
00218     if (this->point_cloud_connect_count_ > 0 ||
00219         this->depth_image_connect_count_ <= 0)
00220       // do this first so there's chance for sensor to run 1 frame after activate
00221       this->parentSensor->SetActive(true);
00222   }
00223 }
00224 
00226 // Update the controller
00227 void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
00228     unsigned int _width, unsigned int _height, unsigned int _depth,
00229     const std::string &_format)
00230 {
00231   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00232     return;
00233 
00234 # if GAZEBO_MAJOR_VERSION >= 7
00235   this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
00236 # else
00237   this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
00238 # endif
00239   if (!this->parentSensor->IsActive())
00240   {
00241     if (this->point_cloud_connect_count_ > 0)
00242       // do this first so there's chance for sensor to run 1 frame after activate
00243       this->parentSensor->SetActive(true);
00244   }
00245   else
00246   {
00247     if (this->point_cloud_connect_count_ > 0)
00248     {
00249       this->lock_.lock();
00250       this->point_cloud_msg_.header.frame_id = this->frame_name_;
00251       this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00252       this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00253       this->point_cloud_msg_.width = this->width;
00254       this->point_cloud_msg_.height = this->height;
00255       this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width;
00256 
00257       sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg_);
00258       pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
00259       pcd_modifier.resize(_width*_height);
00260 
00261       point_cloud_msg_.is_dense = true;
00262 
00263       sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
00264       sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
00265       sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
00266       sensor_msgs::PointCloud2Iterator<float> iter_rgb(point_cloud_msg_, "rgb");
00267 
00268       for (unsigned int i = 0; i < _width; i++)
00269       {
00270         for (unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
00271         {
00272           unsigned int index = (j * _width) + i;
00273           *iter_x = _pcd[4 * index];
00274           *iter_y = _pcd[4 * index + 1];
00275           *iter_z = _pcd[4 * index + 2];
00276           *iter_rgb = _pcd[4 * index + 3];
00277           if (i == _width /2 && j == _height / 2)
00278           {
00279             uint32_t rgb = *reinterpret_cast<int*>(&(*iter_rgb));
00280             uint8_t r = (rgb >> 16) & 0x0000ff;
00281             uint8_t g = (rgb >> 8)  & 0x0000ff;
00282             uint8_t b = (rgb)       & 0x0000ff;
00283             std::cerr << (int)r << " " << (int)g << " " << (int)b << "\n";
00284           }
00285         }
00286       }
00287 
00288       this->point_cloud_pub_.publish(this->point_cloud_msg_);
00289       this->lock_.unlock();
00290     }
00291   }
00292 }
00293 
00295 // Update the controller
00296 void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
00297     unsigned int _width, unsigned int _height, unsigned int _depth,
00298     const std::string &_format)
00299 {
00300   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00301     return;
00302 
00303   //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
00304 # if GAZEBO_MAJOR_VERSION >= 7
00305   this->sensor_update_time_ = this->parentSensor->LastMeasurementTime();
00306 # else
00307   this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
00308 # endif
00309 
00310   if (!this->parentSensor->IsActive())
00311   {
00312     if ((*this->image_connect_count_) > 0)
00313       // do this first so there's chance for sensor to run 1 frame after activate
00314       this->parentSensor->SetActive(true);
00315   }
00316   else
00317   {
00318     if ((*this->image_connect_count_) > 0)
00319     {
00320       this->PutCameraData(_image);
00321       // TODO(lucasw) publish camera info with depth image
00322       // this->PublishCameraInfo(sensor_update_time);
00323     }
00324   }
00325 }
00326 
00328 // Put camera data to the interface
00329 void GazeboRosDepthCamera::FillPointdCloud(const float *_src)
00330 {
00331   this->lock_.lock();
00332 
00333   this->point_cloud_msg_.header.frame_id = this->frame_name_;
00334   this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00335   this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00336   this->point_cloud_msg_.width = this->width;
00337   this->point_cloud_msg_.height = this->height;
00338   this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width;
00339 
00341   FillPointCloudHelper(this->point_cloud_msg_,
00342                  this->height,
00343                  this->width,
00344                  this->skip_,
00345                  (void*)_src );
00346 
00347   this->point_cloud_pub_.publish(this->point_cloud_msg_);
00348 
00349   this->lock_.unlock();
00350 }
00351 
00353 // Put depth image data to the interface
00354 void GazeboRosDepthCamera::FillDepthImage(const float *_src)
00355 {
00356   this->lock_.lock();
00357   // copy data into image
00358   this->depth_image_msg_.header.frame_id = this->frame_name_;
00359   this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00360   this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00361 
00363   FillDepthImageHelper(this->depth_image_msg_,
00364                  this->height,
00365                  this->width,
00366                  this->skip_,
00367                  (void*)_src );
00368 
00369   this->depth_image_pub_.publish(this->depth_image_msg_);
00370 
00371   this->lock_.unlock();
00372 }
00373 
00374 
00375 // Fill depth information
00376 bool GazeboRosDepthCamera::FillPointCloudHelper(
00377     sensor_msgs::PointCloud2 &point_cloud_msg,
00378     uint32_t rows_arg, uint32_t cols_arg,
00379     uint32_t step_arg, void* data_arg)
00380 {
00381   sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg);
00382   pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
00383   pcd_modifier.resize(rows_arg*cols_arg);
00384 
00385   sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
00386   sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
00387   sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
00388   sensor_msgs::PointCloud2Iterator<uint8_t> iter_rgb(point_cloud_msg_, "rgb");
00389 
00390   point_cloud_msg.is_dense = true;
00391 
00392   float* toCopyFrom = (float*)data_arg;
00393   int index = 0;
00394 
00395 # if GAZEBO_MAJOR_VERSION >= 7
00396   double hfov = this->parentSensor->DepthCamera()->HFOV().Radian();
00397 # else
00398   double hfov = this->parentSensor->GetDepthCamera()->GetHFOV().Radian();
00399 # endif
00400   double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));
00401 
00402   // convert depth to point cloud
00403   for (uint32_t j=0; j<rows_arg; j++)
00404   {
00405     double pAngle;
00406     if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl);
00407     else            pAngle = 0.0;
00408 
00409     for (uint32_t i=0; i<cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
00410     {
00411       double yAngle;
00412       if (cols_arg>1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl);
00413       else            yAngle = 0.0;
00414 
00415       double depth = toCopyFrom[index++];
00416 
00417       // in optical frame
00418       // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
00419       // to urdf, where the *_optical_frame should have above relative
00420       // rotation from the physical camera *_frame
00421       *iter_x      = depth * tan(yAngle);
00422       *iter_y      = depth * tan(pAngle);
00423       if(depth > this->point_cloud_cutoff_)
00424       {
00425         *iter_z    = depth;
00426       }
00427       else //point in the unseeable range
00428       {
00429         *iter_x = *iter_y = *iter_z = std::numeric_limits<float>::quiet_NaN ();
00430         point_cloud_msg.is_dense = false;
00431       }
00432 
00433       // put image color data for each point
00434       uint8_t*  image_src = (uint8_t*)(&(this->image_msg_.data[0]));
00435       if (this->image_msg_.data.size() == rows_arg*cols_arg*3)
00436       {
00437         // color
00438         iter_rgb[0] = image_src[i*3+j*cols_arg*3+0];
00439         iter_rgb[1] = image_src[i*3+j*cols_arg*3+1];
00440         iter_rgb[2] = image_src[i*3+j*cols_arg*3+2];
00441       }
00442       else if (this->image_msg_.data.size() == rows_arg*cols_arg)
00443       {
00444         // mono (or bayer?  @todo; fix for bayer)
00445         iter_rgb[0] = image_src[i+j*cols_arg];
00446         iter_rgb[1] = image_src[i+j*cols_arg];
00447         iter_rgb[2] = image_src[i+j*cols_arg];
00448       }
00449       else
00450       {
00451         // no image
00452         iter_rgb[0] = 0;
00453         iter_rgb[1] = 0;
00454         iter_rgb[2] = 0;
00455       }
00456     }
00457   }
00458 
00459   return true;
00460 }
00461 
00462 // Fill depth information
00463 bool GazeboRosDepthCamera::FillDepthImageHelper(
00464     sensor_msgs::Image& image_msg,
00465     uint32_t rows_arg, uint32_t cols_arg,
00466     uint32_t step_arg, void* data_arg)
00467 {
00468   image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00469   image_msg.height = rows_arg;
00470   image_msg.width = cols_arg;
00471   image_msg.step = sizeof(float) * cols_arg;
00472   image_msg.data.resize(rows_arg * cols_arg * sizeof(float));
00473   image_msg.is_bigendian = 0;
00474 
00475   const float bad_point = std::numeric_limits<float>::quiet_NaN();
00476 
00477   float* dest = (float*)(&(image_msg.data[0]));
00478   float* toCopyFrom = (float*)data_arg;
00479   int index = 0;
00480 
00481   // convert depth to point cloud
00482   for (uint32_t j = 0; j < rows_arg; j++)
00483   {
00484     for (uint32_t i = 0; i < cols_arg; i++)
00485     {
00486       float depth = toCopyFrom[index++];
00487 
00488       if (depth > this->point_cloud_cutoff_)
00489       {
00490         dest[i + j * cols_arg] = depth;
00491       }
00492       else //point in the unseeable range
00493       {
00494         dest[i + j * cols_arg] = bad_point;
00495       }
00496     }
00497   }
00498   return true;
00499 }
00500 
00501 void GazeboRosDepthCamera::PublishCameraInfo()
00502 {
00503   ROS_DEBUG("publishing default camera info, then depth camera info");
00504   GazeboRosCameraUtils::PublishCameraInfo();
00505 
00506   if (this->depth_info_connect_count_ > 0)
00507   {
00508 # if GAZEBO_MAJOR_VERSION >= 7
00509     common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
00510 # else
00511     common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
00512 # endif
00513     this->sensor_update_time_ = sensor_update_time;
00514     if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
00515     {
00516       this->PublishCameraInfo(this->depth_image_camera_info_pub_);  // , sensor_update_time);
00517       this->last_depth_image_camera_info_update_time_ = sensor_update_time;
00518     }
00519   }
00520 }
00521 
00522 //@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp.
00523 /*
00524 #include <stereo_msgs/DisparityImage.h>
00525 pub_disparity_ = comm_nh.advertise<stereo_msgs::DisparityImage > ("depth/disparity", 5, subscriberChanged2, subscriberChanged2);
00526 
00527 void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time)
00528 {
00529   stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared<stereo_msgs::DisparityImage > ();
00530   disp_msg->header.stamp                  = time;
00531   disp_msg->header.frame_id               = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_;
00532   disp_msg->image.header                  = disp_msg->header;
00533   disp_msg->image.encoding                = sensor_msgs::image_encodings::TYPE_32FC1;
00534   disp_msg->image.height                  = depth_height_;
00535   disp_msg->image.width                   = depth_width_;
00536   disp_msg->image.step                    = disp_msg->image.width * sizeof (float);
00537   disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step);
00538   disp_msg->T = depth.getBaseline ();
00539   disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth ();
00540 
00542   disp_msg->min_disparity = 0.0;
00543   disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3;
00544   disp_msg->delta_d = 0.125;
00545 
00546   depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast<float*>(&disp_msg->image.data[0]), disp_msg->image.step);
00547 
00548   pub_disparity_.publish (disp_msg);
00549 }
00550 */
00551 
00552 
00553 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22