gazebo_ros_openni_kinect.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  @mainpage
00023    Desc: GazeboRosOpenniKinect plugin for simulating cameras in Gazebo
00024    Author: John Hsu
00025    Date: 24 Sept 2008
00026    SVN info: $Id$
00027  @htmlinclude manifest.html
00028  @b GazeboRosOpenniKinect plugin broadcasts ROS Image messages
00029  */
00030 
00031 #include <algorithm>
00032 #include <assert.h>
00033 #include <boost/thread/thread.hpp>
00034 #include <boost/bind.hpp>
00035 
00036 #include <gazebo_plugins/gazebo_ros_openni_kinect.h>
00037 
00038 #include "sensors/Sensor.hh"
00039 #include "sdf/interface/SDF.hh"
00040 #include "sensors/SensorTypes.hh"
00041 
00042 // for creating PointCloud2 from pcl point cloud
00043 #include "pcl/ros/conversions.h"
00044 
00045 #include "tf/tf.h"
00046 
00047 namespace gazebo
00048 {
00049 
00051 // Constructor
00052 GazeboRosOpenniKinect::GazeboRosOpenniKinect()
00053 {
00054   this->point_cloud_connect_count_ = 0;
00055   this->depth_info_connect_count_ = 0;
00056   this->last_depth_image_camera_info_update_time_ = common::Time(0);
00057 }
00058 
00060 // Destructor
00061 GazeboRosOpenniKinect::~GazeboRosOpenniKinect()
00062 {
00063 }
00064 
00066 // Load the controller
00067 void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00068 {
00069   DepthCameraPlugin::Load(_parent, _sdf);
00070 
00071   // copying from DepthCameraPlugin into GazeboRosCameraUtils
00072   this->parentSensor_ = this->parentSensor;
00073   this->width_ = this->width;
00074   this->height_ = this->height;
00075   this->depth_ = this->depth;
00076   this->format_ = this->format;
00077   this->camera_ = this->depthCamera;
00078 
00079   GazeboRosCameraUtils::Load(_parent, _sdf);
00080 
00081   // using a different default
00082   if (!_sdf->GetElement("imageTopicName"))
00083     this->image_topic_name_ = "ir/image_raw";
00084   if (!_sdf->HasElement("cameraInfoTopicName"))
00085     this->camera_info_topic_name_ = "ir/camera_info";
00086 
00087   // point cloud stuff
00088   if (!_sdf->GetElement("pointCloudTopicName"))
00089     this->point_cloud_topic_name_ = "points";
00090   else
00091     this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->GetValueString();
00092 
00093   // depth image stuff
00094   if (!_sdf->GetElement("depthImageTopicName"))
00095     this->depth_image_topic_name_ = "depth/image_raw";
00096   else
00097     this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->GetValueString();
00098 
00099   if (!_sdf->GetElement("depthImageCameraInfoTopicName"))
00100     this->depth_image_camera_info_topic_name_ = "depth/camera_info";
00101   else
00102     this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->GetValueString();
00103 
00104   if (!_sdf->GetElement("pointCloudCutoff"))
00105     this->point_cloud_cutoff_ = 0.4;
00106   else
00107     this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->GetValueDouble();
00108 
00109   ros::AdvertiseOptions point_cloud_ao = 
00110     ros::AdvertiseOptions::create<sensor_msgs::PointCloud2 >(
00111       this->point_cloud_topic_name_,1,
00112       boost::bind( &GazeboRosOpenniKinect::PointCloudConnect,this),
00113       boost::bind( &GazeboRosOpenniKinect::PointCloudDisconnect,this), 
00114       ros::VoidPtr(), &this->camera_queue_);
00115   this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao);
00116 
00117   ros::AdvertiseOptions depth_image_ao = 
00118     ros::AdvertiseOptions::create< sensor_msgs::Image >(
00119       this->depth_image_topic_name_,1,
00120       boost::bind( &GazeboRosOpenniKinect::DepthImageConnect,this),
00121       boost::bind( &GazeboRosOpenniKinect::DepthImageDisconnect,this), 
00122       ros::VoidPtr(), &this->camera_queue_);
00123   this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao);
00124 
00125   ros::AdvertiseOptions depth_image_camera_info_ao = 
00126     ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
00127         this->depth_image_camera_info_topic_name_,1,
00128         boost::bind( &GazeboRosOpenniKinect::DepthInfoConnect,this),
00129         boost::bind( &GazeboRosOpenniKinect::DepthInfoDisconnect,this), 
00130         ros::VoidPtr(), &this->camera_queue_);
00131   this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao);
00132 }
00133 
00135 // Increment count
00136 void GazeboRosOpenniKinect::PointCloudConnect()
00137 {
00138   this->point_cloud_connect_count_++;
00139   this->image_connect_count_++;
00140   this->parentSensor->SetActive(true);
00141 }
00143 // Decrement count
00144 void GazeboRosOpenniKinect::PointCloudDisconnect()
00145 {
00146   this->point_cloud_connect_count_--;
00147   this->image_connect_count_--;
00148   if (this->point_cloud_connect_count_ <= 0)
00149     this->parentSensor->SetActive(false);
00150 }
00151 
00153 // Increment count
00154 void GazeboRosOpenniKinect::DepthImageConnect()
00155 {
00156   this->depth_image_connect_count_++;
00157   this->parentSensor->SetActive(true);
00158 }
00160 // Decrement count
00161 void GazeboRosOpenniKinect::DepthImageDisconnect()
00162 {
00163   this->depth_image_connect_count_--;
00164 }
00165 
00167 // Increment count
00168 void GazeboRosOpenniKinect::DepthInfoConnect()
00169 {
00170   this->depth_info_connect_count_++;
00171 }
00173 // Decrement count
00174 void GazeboRosOpenniKinect::DepthInfoDisconnect()
00175 {
00176   this->depth_info_connect_count_--;
00177 }
00178 
00180 // Update the controller
00181 void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image, 
00182     unsigned int _width, unsigned int _height, unsigned int _depth, 
00183     const std::string &_format)
00184 {
00185   this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
00186   if (this->parentSensor->IsActive())
00187   {
00188     if (this->point_cloud_connect_count_ <= 0 &&
00189         this->depth_image_connect_count_ <= 0 &&
00190         this->image_connect_count_ <= 0)
00191     {
00192       this->parentSensor->SetActive(false);
00193     }
00194     else
00195     {
00196       if (this->point_cloud_connect_count_ > 0)
00197         this->FillPointdCloud(_image);
00198 
00199       if (this->depth_image_connect_count_ > 0)
00200         this->FillDepthImage(_image);
00201     }
00202   }
00203   else
00204   {
00205     if (this->point_cloud_connect_count_ > 0 ||
00206         this->depth_image_connect_count_ <= 0)
00207       // do this first so there's chance for sensor to run 1 frame after activate
00208       this->parentSensor->SetActive(true);
00209   }
00210 }
00211 
00213 // Update the controller
00214 void GazeboRosOpenniKinect::OnNewImageFrame(const unsigned char *_image, 
00215     unsigned int _width, unsigned int _height, unsigned int _depth, 
00216     const std::string &_format)
00217 {
00218   //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
00219   this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00220 
00221   if (this->parentSensor->IsActive())
00222   {
00223     if (this->point_cloud_connect_count_ <= 0 &&
00224         this->depth_image_connect_count_ <= 0 &&
00225         this->image_connect_count_ <= 0)
00226     {
00227       this->parentSensor->SetActive(false);
00228     }
00229     else
00230     {
00231       if (this->image_connect_count_ > 0)
00232         this->PutCameraData(_image);
00233     }
00234   }
00235   else
00236   {
00237     if (this->image_connect_count_ > 0)
00238       // do this first so there's chance for sensor to run 1 frame after activate
00239       this->parentSensor->SetActive(true);
00240   }
00241 }
00242 
00244 // Put point cloud data to the interface
00245 void GazeboRosOpenniKinect::FillPointdCloud(const float *_src)
00246 {
00247   this->lock_.lock();
00248 
00249   this->point_cloud_msg_.header.frame_id = this->frame_name_;
00250   this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00251   this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00252   this->point_cloud_msg_.width = this->width;
00253   this->point_cloud_msg_.height = this->height;
00254   this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width;
00255 
00257   FillPointCloudHelper(this->point_cloud_msg_,
00258                  this->height,
00259                  this->width,
00260                  this->skip_,
00261                  (void*)_src );
00262 
00263   this->point_cloud_pub_.publish(this->point_cloud_msg_);
00264 
00265   this->lock_.unlock();
00266 }
00267 
00269 // Put depth image data to the interface
00270 void GazeboRosOpenniKinect::FillDepthImage(const float *_src)
00271 {
00272   this->lock_.lock();
00273   // copy data into image
00274   this->depth_image_msg_.header.frame_id = this->frame_name_;
00275   this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00276   this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00277 
00279   FillDepthImageHelper(this->depth_image_msg_,
00280                  this->height,
00281                  this->width,
00282                  this->skip_,
00283                  (void*)_src );
00284 
00285   this->depth_image_pub_.publish(this->depth_image_msg_);
00286 
00287   this->lock_.unlock();
00288 }
00289 
00290 
00291 // Fill depth information
00292 bool GazeboRosOpenniKinect::FillPointCloudHelper(
00293     sensor_msgs::PointCloud2 &point_cloud_msg,
00294     uint32_t rows_arg, uint32_t cols_arg,
00295     uint32_t step_arg, void* data_arg)
00296 {
00297   pcl::PointCloud<pcl::PointXYZRGB> point_cloud;
00298 
00299   point_cloud.points.resize(0);
00300   point_cloud.is_dense = true;
00301 
00302   float* toCopyFrom = (float*)data_arg;
00303   int index = 0;
00304 
00305   double hfov = this->parentSensor->GetDepthCamera()->GetHFOV().Radian();
00306   double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));
00307 
00308   // convert depth to point cloud
00309   for (uint32_t j=0; j<rows_arg; j++)
00310   {
00311     double pAngle;
00312     if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl);
00313     else            pAngle = 0.0;
00314 
00315     for (uint32_t i=0; i<cols_arg; i++)
00316     {
00317       double yAngle;
00318       if (cols_arg>1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl);
00319       else            yAngle = 0.0;
00320 
00321       double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip();
00322 
00323       // in optical frame
00324       // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
00325       // to urdf, where the *_optical_frame should have above relative
00326       // rotation from the physical camera *_frame
00327       pcl::PointXYZRGB point;
00328       point.x      = depth * tan(yAngle);
00329       point.y      = depth * tan(pAngle);
00330       if(depth > this->point_cloud_cutoff_)
00331       {
00332         point.z    = depth;
00333       }
00334       else //point in the unseeable range
00335       {
00336         point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
00337         point_cloud.is_dense = false;
00338       }
00339 
00340       // put image color data for each point
00341       uint8_t*  image_src = (uint8_t*)(&(this->image_msg_.data[0]));
00342       if (this->image_msg_.data.size() == rows_arg*cols_arg*3)
00343       {
00344         // color
00345         point.r = image_src[i*3+j*cols_arg*3+0];
00346         point.g = image_src[i*3+j*cols_arg*3+1];
00347         point.b = image_src[i*3+j*cols_arg*3+2];
00348       }
00349       else if (this->image_msg_.data.size() == rows_arg*cols_arg)
00350       {
00351         // mono (or bayer?  @todo; fix for bayer)
00352         point.r = image_src[i+j*cols_arg];
00353         point.g = image_src[i+j*cols_arg];
00354         point.b = image_src[i+j*cols_arg];
00355       }
00356       else
00357       {
00358         // no image
00359         point.r = 0;
00360         point.g = 0;
00361         point.b = 0;
00362       }
00363 
00364       point_cloud.points.push_back(point);
00365     }
00366   }
00367 
00368   point_cloud.header = point_cloud_msg.header;
00369   pcl::toROSMsg(point_cloud, point_cloud_msg);
00370   return true;
00371 }
00372 
00373 // Fill depth information
00374 bool GazeboRosOpenniKinect::FillDepthImageHelper(
00375     sensor_msgs::Image& image_msg,
00376     uint32_t rows_arg, uint32_t cols_arg,
00377     uint32_t step_arg, void* data_arg)
00378 {
00379   image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00380   image_msg.height = rows_arg;
00381   image_msg.width = cols_arg;
00382   image_msg.step = sizeof(float) * cols_arg;
00383   image_msg.data.resize(rows_arg * cols_arg * sizeof(float));
00384   image_msg.is_bigendian = 0;
00385 
00386   const float bad_point = std::numeric_limits<float>::quiet_NaN();
00387 
00388   float* dest = (float*)(&(image_msg.data[0]));
00389   float* toCopyFrom = (float*)data_arg;
00390   int index = 0;
00391 
00392   // convert depth to point cloud
00393   for (uint32_t j = 0; j < rows_arg; j++)
00394   {
00395     for (uint32_t i = 0; i < cols_arg; i++)
00396     {
00397       float depth = toCopyFrom[index++];
00398 
00399       if (depth > this->point_cloud_cutoff_)
00400       {
00401         dest[i + j * cols_arg] = depth;
00402       }
00403       else //point in the unseeable range
00404       {
00405         dest[i + j * cols_arg] = bad_point;
00406       }
00407     }
00408   }
00409   return true;
00410 }
00411 
00412 void GazeboRosOpenniKinect::PublishCameraInfo()
00413 {
00414   ROS_DEBUG("publishing default camera info, then openni kinect camera info");
00415   GazeboRosCameraUtils::PublishCameraInfo();
00416 
00417   if (this->depth_info_connect_count_ > 0)
00418   {
00419     this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00420     common::Time cur_time = this->world_->GetSimTime();
00421     if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
00422     {
00423       this->PublishCameraInfo(this->depth_image_camera_info_pub_);
00424       this->last_depth_image_camera_info_update_time_ = cur_time;
00425     }
00426   }
00427 }
00428 
00429 //@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp.
00430 /*
00431 #include <stereo_msgs/DisparityImage.h>
00432 pub_disparity_ = comm_nh.advertise<stereo_msgs::DisparityImage > ("depth/disparity", 5, subscriberChanged2, subscriberChanged2);
00433 
00434 void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time)
00435 {
00436   stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared<stereo_msgs::DisparityImage > ();
00437   disp_msg->header.stamp                  = time;
00438   disp_msg->header.frame_id               = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_;
00439   disp_msg->image.header                  = disp_msg->header;
00440   disp_msg->image.encoding                = sensor_msgs::image_encodings::TYPE_32FC1;
00441   disp_msg->image.height                  = depth_height_;
00442   disp_msg->image.width                   = depth_width_;
00443   disp_msg->image.step                    = disp_msg->image.width * sizeof (float);
00444   disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step);
00445   disp_msg->T = depth.getBaseline ();
00446   disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth ();
00447 
00449   disp_msg->min_disparity = 0.0;
00450   disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3;
00451   disp_msg->delta_d = 0.125;
00452 
00453   depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast<float*>(&disp_msg->image.data[0]), disp_msg->image.step);
00454 
00455   pub_disparity_.publish (disp_msg);
00456 }
00457 */
00458 
00459 // Register this plugin with the simulator
00460 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosOpenniKinect)
00461 
00462 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Mon Oct 6 2014 12:15:44