gazebo_ros_openni_kinect.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 /*
00019    Desc: GazeboRosOpenniKinect plugin for simulating cameras in Gazebo
00020    Author: John Hsu
00021    Date: 24 Sept 2008
00022  */
00023 
00024 #include <algorithm>
00025 #include <assert.h>
00026 #include <boost/thread/thread.hpp>
00027 #include <boost/bind.hpp>
00028 
00029 #include <gazebo_plugins/gazebo_ros_openni_kinect.h>
00030 
00031 #include <gazebo/sensors/Sensor.hh>
00032 #include <sdf/sdf.hh>
00033 #include <gazebo/sensors/SensorTypes.hh>
00034 
00035 #include <sensor_msgs/point_cloud2_iterator.h>
00036 
00037 #include <tf/tf.h>
00038 
00039 namespace gazebo
00040 {
00041 // Register this plugin with the simulator
00042 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosOpenniKinect)
00043 
00044 
00045 // Constructor
00046 GazeboRosOpenniKinect::GazeboRosOpenniKinect()
00047 {
00048   this->point_cloud_connect_count_ = 0;
00049   this->depth_info_connect_count_ = 0;
00050   this->depth_image_connect_count_ = 0;
00051   this->last_depth_image_camera_info_update_time_ = common::Time(0);
00052 }
00053 
00055 // Destructor
00056 GazeboRosOpenniKinect::~GazeboRosOpenniKinect()
00057 {
00058 }
00059 
00061 // Load the controller
00062 void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00063 {
00064   DepthCameraPlugin::Load(_parent, _sdf);
00065 
00066   // copying from DepthCameraPlugin into GazeboRosCameraUtils
00067   this->parentSensor_ = this->parentSensor;
00068   this->width_ = this->width;
00069   this->height_ = this->height;
00070   this->depth_ = this->depth;
00071   this->format_ = this->format;
00072   this->camera_ = this->depthCamera;
00073 
00074   // using a different default
00075   if (!_sdf->HasElement("imageTopicName"))
00076     this->image_topic_name_ = "ir/image_raw";
00077   if (!_sdf->HasElement("cameraInfoTopicName"))
00078     this->camera_info_topic_name_ = "ir/camera_info";
00079 
00080   // point cloud stuff
00081   if (!_sdf->HasElement("pointCloudTopicName"))
00082     this->point_cloud_topic_name_ = "points";
00083   else
00084     this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get<std::string>();
00085 
00086   // depth image stuff
00087   if (!_sdf->HasElement("depthImageTopicName"))
00088     this->depth_image_topic_name_ = "depth/image_raw";
00089   else
00090     this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->Get<std::string>();
00091 
00092   if (!_sdf->HasElement("depthImageCameraInfoTopicName"))
00093     this->depth_image_camera_info_topic_name_ = "depth/camera_info";
00094   else
00095     this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->Get<std::string>();
00096 
00097   if (!_sdf->HasElement("pointCloudCutoff"))
00098     this->point_cloud_cutoff_ = 0.4;
00099   else
00100     this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<double>();
00101   if (!_sdf->HasElement("pointCloudCutoffMax"))
00102     this->point_cloud_cutoff_max_ = 5.0;
00103   else
00104     this->point_cloud_cutoff_max_ = _sdf->GetElement("pointCloudCutoffMax")->Get<double>();
00105 
00106   load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosOpenniKinect::Advertise, this));
00107   GazeboRosCameraUtils::Load(_parent, _sdf);
00108 }
00109 
00110 void GazeboRosOpenniKinect::Advertise()
00111 {
00112   ros::AdvertiseOptions point_cloud_ao =
00113     ros::AdvertiseOptions::create<sensor_msgs::PointCloud2 >(
00114       this->point_cloud_topic_name_,1,
00115       boost::bind( &GazeboRosOpenniKinect::PointCloudConnect,this),
00116       boost::bind( &GazeboRosOpenniKinect::PointCloudDisconnect,this),
00117       ros::VoidPtr(), &this->camera_queue_);
00118   this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao);
00119 
00120   ros::AdvertiseOptions depth_image_ao =
00121     ros::AdvertiseOptions::create< sensor_msgs::Image >(
00122       this->depth_image_topic_name_,1,
00123       boost::bind( &GazeboRosOpenniKinect::DepthImageConnect,this),
00124       boost::bind( &GazeboRosOpenniKinect::DepthImageDisconnect,this),
00125       ros::VoidPtr(), &this->camera_queue_);
00126   this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao);
00127 
00128   ros::AdvertiseOptions depth_image_camera_info_ao =
00129     ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
00130         this->depth_image_camera_info_topic_name_,1,
00131         boost::bind( &GazeboRosOpenniKinect::DepthInfoConnect,this),
00132         boost::bind( &GazeboRosOpenniKinect::DepthInfoDisconnect,this),
00133         ros::VoidPtr(), &this->camera_queue_);
00134   this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao);
00135 }
00136 
00138 // Increment count
00139 void GazeboRosOpenniKinect::PointCloudConnect()
00140 {
00141   this->point_cloud_connect_count_++;
00142   (*this->image_connect_count_)++;
00143   this->parentSensor->SetActive(true);
00144 }
00146 // Decrement count
00147 void GazeboRosOpenniKinect::PointCloudDisconnect()
00148 {
00149   this->point_cloud_connect_count_--;
00150   (*this->image_connect_count_)--;
00151   if (this->point_cloud_connect_count_ <= 0)
00152     this->parentSensor->SetActive(false);
00153 }
00154 
00156 // Increment count
00157 void GazeboRosOpenniKinect::DepthImageConnect()
00158 {
00159   this->depth_image_connect_count_++;
00160   this->parentSensor->SetActive(true);
00161 }
00163 // Decrement count
00164 void GazeboRosOpenniKinect::DepthImageDisconnect()
00165 {
00166   this->depth_image_connect_count_--;
00167 }
00168 
00170 // Increment count
00171 void GazeboRosOpenniKinect::DepthInfoConnect()
00172 {
00173   this->depth_info_connect_count_++;
00174 }
00176 // Decrement count
00177 void GazeboRosOpenniKinect::DepthInfoDisconnect()
00178 {
00179   this->depth_info_connect_count_--;
00180 }
00181 
00183 // Update the controller
00184 void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image,
00185     unsigned int _width, unsigned int _height, unsigned int _depth,
00186     const std::string &_format)
00187 {
00188   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00189     return;
00190 
00191 # if GAZEBO_MAJOR_VERSION >= 7
00192   this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
00193 # else
00194   this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
00195 # endif
00196   if (this->parentSensor->IsActive())
00197   {
00198     if (this->point_cloud_connect_count_ <= 0 &&
00199         this->depth_image_connect_count_ <= 0 &&
00200         (*this->image_connect_count_) <= 0)
00201     {
00202       this->parentSensor->SetActive(false);
00203     }
00204     else
00205     {
00206       if (this->point_cloud_connect_count_ > 0)
00207         this->FillPointdCloud(_image);
00208 
00209       if (this->depth_image_connect_count_ > 0)
00210         this->FillDepthImage(_image);
00211     }
00212   }
00213   else
00214   {
00215     if (this->point_cloud_connect_count_ > 0 ||
00216         this->depth_image_connect_count_ <= 0)
00217       // do this first so there's chance for sensor to run 1 frame after activate
00218       this->parentSensor->SetActive(true);
00219   }
00220   PublishCameraInfo();
00221 }
00222 
00224 // Update the controller
00225 void GazeboRosOpenniKinect::OnNewImageFrame(const unsigned char *_image,
00226     unsigned int _width, unsigned int _height, unsigned int _depth,
00227     const std::string &_format)
00228 {
00229   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00230     return;
00231 
00232   //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
00233 # if GAZEBO_MAJOR_VERSION >= 7
00234   this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
00235 # else
00236   this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime();
00237 # endif
00238 
00239   if (this->parentSensor->IsActive())
00240   {
00241     if (this->point_cloud_connect_count_ <= 0 &&
00242         this->depth_image_connect_count_ <= 0 &&
00243         (*this->image_connect_count_) <= 0)
00244     {
00245       this->parentSensor->SetActive(false);
00246     }
00247     else
00248     {
00249       if ((*this->image_connect_count_) > 0)
00250         this->PutCameraData(_image);
00251     }
00252   }
00253   else
00254   {
00255     if ((*this->image_connect_count_) > 0)
00256       // do this first so there's chance for sensor to run 1 frame after activate
00257       this->parentSensor->SetActive(true);
00258   }
00259 }
00260 
00262 // Put point cloud data to the interface
00263 void GazeboRosOpenniKinect::FillPointdCloud(const float *_src)
00264 {
00265   this->lock_.lock();
00266 
00267   this->point_cloud_msg_.header.frame_id = this->frame_name_;
00268   this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00269   this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00270   this->point_cloud_msg_.width = this->width;
00271   this->point_cloud_msg_.height = this->height;
00272   this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width;
00273 
00275   FillPointCloudHelper(this->point_cloud_msg_,
00276                  this->height,
00277                  this->width,
00278                  this->skip_,
00279                  (void*)_src );
00280 
00281   this->point_cloud_pub_.publish(this->point_cloud_msg_);
00282 
00283   this->lock_.unlock();
00284 }
00285 
00287 // Put depth image data to the interface
00288 void GazeboRosOpenniKinect::FillDepthImage(const float *_src)
00289 {
00290   this->lock_.lock();
00291   // copy data into image
00292   this->depth_image_msg_.header.frame_id = this->frame_name_;
00293   this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00294   this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00295 
00297   FillDepthImageHelper(this->depth_image_msg_,
00298                  this->height,
00299                  this->width,
00300                  this->skip_,
00301                  (void*)_src );
00302 
00303   this->depth_image_pub_.publish(this->depth_image_msg_);
00304 
00305   this->lock_.unlock();
00306 }
00307 
00308 
00309 // Fill depth information
00310 bool GazeboRosOpenniKinect::FillPointCloudHelper(
00311     sensor_msgs::PointCloud2 &point_cloud_msg,
00312     uint32_t rows_arg, uint32_t cols_arg,
00313     uint32_t step_arg, void* data_arg)
00314 {
00315   sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg);
00316   pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
00317   // convert to flat array shape, we need to reconvert later
00318   pcd_modifier.resize(rows_arg*cols_arg);
00319   point_cloud_msg.is_dense = true;
00320 
00321   sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
00322   sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
00323   sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
00324   sensor_msgs::PointCloud2Iterator<uint8_t> iter_rgb(point_cloud_msg_, "rgb");
00325 
00326   float* toCopyFrom = (float*)data_arg;
00327   int index = 0;
00328 
00329 # if GAZEBO_MAJOR_VERSION >= 7
00330   double hfov = this->parentSensor->DepthCamera()->HFOV().Radian();
00331 # else
00332   double hfov = this->parentSensor->GetDepthCamera()->GetHFOV().Radian();
00333 # endif
00334   double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));
00335 
00336   // convert depth to point cloud
00337   for (uint32_t j=0; j<rows_arg; j++)
00338   {
00339     double pAngle;
00340     if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl);
00341     else            pAngle = 0.0;
00342 
00343     for (uint32_t i=0; i<cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
00344     {
00345       double yAngle;
00346       if (cols_arg>1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl);
00347       else            yAngle = 0.0;
00348 
00349       double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip();
00350 
00351       if(depth > this->point_cloud_cutoff_ &&
00352          depth < this->point_cloud_cutoff_max_)
00353       {
00354         // in optical frame
00355         // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
00356         // to urdf, where the *_optical_frame should have above relative
00357         // rotation from the physical camera *_frame
00358         *iter_x = depth * tan(yAngle);
00359         *iter_y = depth * tan(pAngle);
00360         *iter_z = depth;
00361       }
00362       else //point in the unseeable range
00363       {
00364         *iter_x = *iter_y = *iter_z = std::numeric_limits<float>::quiet_NaN ();
00365         point_cloud_msg.is_dense = false;
00366       }
00367 
00368       // put image color data for each point
00369       uint8_t*  image_src = (uint8_t*)(&(this->image_msg_.data[0]));
00370       if (this->image_msg_.data.size() == rows_arg*cols_arg*3)
00371       {
00372         // color
00373         iter_rgb[0] = image_src[i*3+j*cols_arg*3+0];
00374         iter_rgb[1] = image_src[i*3+j*cols_arg*3+1];
00375         iter_rgb[2] = image_src[i*3+j*cols_arg*3+2];
00376       }
00377       else if (this->image_msg_.data.size() == rows_arg*cols_arg)
00378       {
00379         // mono (or bayer?  @todo; fix for bayer)
00380         iter_rgb[0] = image_src[i+j*cols_arg];
00381         iter_rgb[1] = image_src[i+j*cols_arg];
00382         iter_rgb[2] = image_src[i+j*cols_arg];
00383       }
00384       else
00385       {
00386         // no image
00387         iter_rgb[0] = 0;
00388         iter_rgb[1] = 0;
00389         iter_rgb[2] = 0;
00390       }
00391     }
00392   }
00393 
00394   // reconvert to original height and width after the flat reshape
00395   point_cloud_msg.height = rows_arg;
00396   point_cloud_msg.width = cols_arg;
00397   point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width;
00398 
00399   return true;
00400 }
00401 
00402 // Fill depth information
00403 bool GazeboRosOpenniKinect::FillDepthImageHelper(
00404     sensor_msgs::Image& image_msg,
00405     uint32_t rows_arg, uint32_t cols_arg,
00406     uint32_t step_arg, void* data_arg)
00407 {
00408   image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00409   image_msg.height = rows_arg;
00410   image_msg.width = cols_arg;
00411   image_msg.step = sizeof(float) * cols_arg;
00412   image_msg.data.resize(rows_arg * cols_arg * sizeof(float));
00413   image_msg.is_bigendian = 0;
00414 
00415   const float bad_point = std::numeric_limits<float>::quiet_NaN();
00416 
00417   float* dest = (float*)(&(image_msg.data[0]));
00418   float* toCopyFrom = (float*)data_arg;
00419   int index = 0;
00420 
00421   // convert depth to point cloud
00422   for (uint32_t j = 0; j < rows_arg; j++)
00423   {
00424     for (uint32_t i = 0; i < cols_arg; i++)
00425     {
00426       float depth = toCopyFrom[index++];
00427 
00428       if (depth > this->point_cloud_cutoff_ &&
00429           depth < this->point_cloud_cutoff_max_)
00430       {
00431         dest[i + j * cols_arg] = depth;
00432       }
00433       else //point in the unseeable range
00434       {
00435         dest[i + j * cols_arg] = bad_point;
00436       }
00437     }
00438   }
00439   return true;
00440 }
00441 
00442 void GazeboRosOpenniKinect::PublishCameraInfo()
00443 {
00444   ROS_DEBUG("publishing default camera info, then openni kinect camera info");
00445   GazeboRosCameraUtils::PublishCameraInfo();
00446 
00447   if (this->depth_info_connect_count_ > 0)
00448   {
00449 # if GAZEBO_MAJOR_VERSION >= 7
00450     this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
00451 # else
00452     this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime();
00453 # endif
00454     if (this->sensor_update_time_ - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
00455     {
00456       this->PublishCameraInfo(this->depth_image_camera_info_pub_);
00457       this->last_depth_image_camera_info_update_time_ = this->sensor_update_time_;
00458     }
00459   }
00460 }
00461 
00462 //@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp.
00463 /*
00464 #include <stereo_msgs/DisparityImage.h>
00465 pub_disparity_ = comm_nh.advertise<stereo_msgs::DisparityImage > ("depth/disparity", 5, subscriberChanged2, subscriberChanged2);
00466 
00467 void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time)
00468 {
00469   stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared<stereo_msgs::DisparityImage > ();
00470   disp_msg->header.stamp                  = time;
00471   disp_msg->header.frame_id               = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_;
00472   disp_msg->image.header                  = disp_msg->header;
00473   disp_msg->image.encoding                = sensor_msgs::image_encodings::TYPE_32FC1;
00474   disp_msg->image.height                  = depth_height_;
00475   disp_msg->image.width                   = depth_width_;
00476   disp_msg->image.step                    = disp_msg->image.width * sizeof (float);
00477   disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step);
00478   disp_msg->T = depth.getBaseline ();
00479   disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth ();
00480 
00482   disp_msg->min_disparity = 0.0;
00483   disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3;
00484   disp_msg->delta_d = 0.125;
00485 
00486   depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast<float*>(&disp_msg->image.data[0]), disp_msg->image.step);
00487 
00488   pub_disparity_.publish (disp_msg);
00489 }
00490 */
00491 
00492 }


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