gazebo_noisydepth_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 Michael Pantic, ASL, ETH Zurich, Switzerland
00003  *
00004  * Forked from Openni/Kinect Depth Plugin, retaining original copyright header:
00005  * Copyright 2013 Open Source Robotics Foundation
00006  *
00007  * Licensed under the Apache License, Version 2.0 (the "License");
00008  * you may not use this file except in compliance with the License.
00009  * You may obtain a copy of the License at
00010  *
00011  *     http://www.apache.org/licenses/LICENSE-2.0
00012  *
00013  * Unless required by applicable law or agreed to in writing, software
00014  * distributed under the License is distributed on an "AS IS" BASIS,
00015  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00016  * See the License for the specific language governing permissions and
00017  * limitations under the License.
00018  *
00019  *
00020  * Desc: GazeboNoisyDepth plugin for simulating cameras in Gazebo
00021  * Author: Michael Pantic / John Hsu (original Depth Plugin)
00022  * Date: 03 Dez 18
00023  */
00024 #include <rotors_gazebo_plugins/gazebo_noisydepth_plugin.h>
00025 
00026 #include <algorithm>
00027 #include <assert.h>
00028 
00029 #include <boost/bind.hpp>
00030 #include <boost/thread/thread.hpp>
00031 #include <gazebo/sensors/Sensor.hh>
00032 #include <gazebo/sensors/SensorTypes.hh>
00033 #include <sdf/sdf.hh>
00034 #include <tf/tf.h>
00035 
00036 namespace gazebo {
00037 // Register this plugin with the simulator
00038 GZ_REGISTER_SENSOR_PLUGIN(GazeboNoisyDepth)
00039 
00040 GazeboNoisyDepth::GazeboNoisyDepth() {
00041   this->depth_info_connect_count_ = 0;
00042   this->depth_image_connect_count_ = 0;
00043   this->last_depth_image_camera_info_update_time_ = common::Time(0);
00044 }
00045 
00046 GazeboNoisyDepth::~GazeboNoisyDepth() {}
00047 
00048 void GazeboNoisyDepth::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) {
00049   DepthCameraPlugin::Load(_parent, _sdf);
00050 
00051   // copying from DepthCameraPlugin into GazeboRosCameraUtils
00052   this->parentSensor_ = this->parentSensor;
00053   this->width_ = this->width;
00054   this->height_ = this->height;
00055   this->depth_ = this->depth;
00056   this->format_ = this->format;
00057   this->camera_ = this->depthCamera;
00058 
00059   // using a different default
00060   if (!_sdf->HasElement("imageTopicName")) {
00061     this->image_topic_name_ = "ir/image_raw";
00062   }
00063 
00064   if (!_sdf->HasElement("cameraInfoTopicName")) {
00065     this->camera_info_topic_name_ = "ir/camera_info";
00066   }
00067 
00068   // depth image stuff
00069   if (!_sdf->HasElement("depthImageTopicName")) {
00070     this->depth_image_topic_name_ = "depth/image_raw";
00071   } else {
00072     this->depth_image_topic_name_ =
00073         _sdf->GetElement("depthImageTopicName")->Get<std::string>();
00074   }
00075 
00076   if (!_sdf->HasElement("depthImageCameraInfoTopicName")) {
00077     this->depth_image_camera_info_topic_name_ = "depth/camera_info";
00078   } else {
00079     this->depth_image_camera_info_topic_name_ =
00080         _sdf->GetElement("depthImageCameraInfoTopicName")->Get<std::string>();
00081   }
00082 
00083   // noise model stuff
00084   std::string noise_model;
00085   if (!_sdf->HasElement("depthNoiseModelName")) {
00086     noise_model = "Kinect";
00087     ROS_WARN_NAMED("NoisyDepth",
00088                    "depthNoiseModelName not defined, assuming 'Kinect'");
00089   } else {
00090     noise_model = _sdf->GetElement("depthNoiseModelName")->Get<std::string>();
00091   }
00092 
00093   if (boost::iequals(noise_model, "Kinect")) {
00094     this->noise_model.reset(new KinectDepthNoiseModel());
00095 
00096     /* no other properties for Kinect */
00097   } else if (boost::iequals(noise_model, "D435")) {
00098     D435DepthNoiseModel *model = new D435DepthNoiseModel();
00099     this->noise_model.reset(model);
00100 
00101     if (_sdf->HasElement("horizontal_fov")) {
00102       model->h_fov = _sdf->GetElement("horizontal_fov")->Get<float>();
00103     }
00104 
00105     if (_sdf->HasElement("baseline")) {
00106       model->baseline = _sdf->GetElement("baseline")->Get<float>();
00107     }
00108 
00109     /* Load D435 specific noise params if available*/
00110     if (_sdf->HasElement("D435NoiseSubpixelErr")) {
00111       model->subpixel_err =
00112           _sdf->GetElement("D435NoiseSubpixelErr")->Get<float>();
00113     }
00114 
00115     if (_sdf->HasElement("D435MaxStdev")) {
00116       model->max_stdev = _sdf->GetElement("D435MaxStdev")->Get<float>();
00117     }
00118 
00119     ROS_INFO_STREAM_NAMED(
00120         "NoisyDepth", "D435 Depth noise configuration: "
00121                           << "\tHorizontal FoV: " << model->h_fov << std::endl
00122                           << "\tBaseline: " << model->baseline << std::endl
00123                           << "\tSubpixel Err: " << model->subpixel_err
00124                           << std::endl
00125                           << "\tNoise StDev cutoff: " << model->max_stdev);
00126   } else {
00127     ROS_WARN_NAMED("NoisyDepth",
00128                    "Invalid depthNoiseModelName (%s), assuming 'Kinect'",
00129                    noise_model.c_str());
00130     this->noise_model.reset(new KinectDepthNoiseModel());
00131   }
00132 
00133   /* Load properties that apply for all noise model */
00134   /* Note that these min/max values may not be the same as near/far clip range
00135    * of the camera */
00136   if (_sdf->HasElement("depthNoiseMinDist")) {
00137     this->noise_model->min_depth =
00138         _sdf->GetElement("depthNoiseMinDist")->Get<float>();
00139   }
00140 
00141   if (_sdf->HasElement("depthNoiseMaxDist")) {
00142     this->noise_model->max_depth =
00143         _sdf->GetElement("depthNoiseMaxDist")->Get<float>();
00144   }
00145 
00146   load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboNoisyDepth::Advertise, this));
00147 
00148   GazeboRosCameraUtils::Load(_parent, _sdf);
00149 }
00150 
00151 void GazeboNoisyDepth::Advertise() {
00152   ros::AdvertiseOptions depth_image_ao =
00153       ros::AdvertiseOptions::create<sensor_msgs::Image>(
00154           this->depth_image_topic_name_, 1,
00155           boost::bind(&GazeboNoisyDepth::DepthImageConnect, this),
00156           boost::bind(&GazeboNoisyDepth::DepthImageDisconnect, this),
00157           ros::VoidPtr(), &this->camera_queue_);
00158 
00159   this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao);
00160 
00161   ros::AdvertiseOptions depth_image_camera_info_ao =
00162       ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
00163           this->depth_image_camera_info_topic_name_, 1,
00164           boost::bind(&GazeboNoisyDepth::DepthInfoConnect, this),
00165           boost::bind(&GazeboNoisyDepth::DepthInfoDisconnect, this),
00166           ros::VoidPtr(), &this->camera_queue_);
00167 
00168   this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao);
00169 }
00170 
00171 void GazeboNoisyDepth::DepthImageConnect() {
00172   ++this->depth_image_connect_count_;
00173   this->parentSensor->SetActive(true);
00174 }
00175 
00176 void GazeboNoisyDepth::DepthImageDisconnect() {
00177   --this->depth_image_connect_count_;
00178 }
00179 
00180 void GazeboNoisyDepth::DepthInfoConnect() {
00181   ++this->depth_info_connect_count_;
00182 }
00183 
00184 void GazeboNoisyDepth::DepthInfoDisconnect() {
00185   --this->depth_info_connect_count_;
00186 }
00187 
00188 void GazeboNoisyDepth::OnNewDepthFrame(const float *_image, unsigned int _width,
00189                                        unsigned int _height,
00190                                        unsigned int _depth,
00191                                        const std::string &_format) {
00192   if (!this->initialized_ || this->height_ <= 0 || this->width_ <= 0) return;
00193 
00194   this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
00195 
00196   // check if there are subscribers, if not disable parent, else process images..
00197   if (this->parentSensor->IsActive()) {
00198     if (this->depth_image_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) {
00199       this->parentSensor->SetActive(false);
00200     } else {
00201       if (this->depth_image_connect_count_ > 0) this->FillDepthImage(_image);
00202     }
00203   }
00204   else {
00205     // if parent is disabled, but has subscribers, enable it.
00206     if ((*this->image_connect_count_) > 0){
00207       this->parentSensor->SetActive(true);
00208     }
00209   }
00210   PublishCameraInfo();
00211 }
00212 
00213 void GazeboNoisyDepth::OnNewImageFrame(const unsigned char *_image,
00214                                        unsigned int _width,
00215                                        unsigned int _height,
00216                                        unsigned int _depth,
00217                                        const std::string &_format) {
00218   if (!this->initialized_ || this->height_ <= 0 || this->width_ <= 0) return;
00219 
00220   this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
00221 
00222   // check if there are subscribers, if not disable parent, else process images..
00223   if (this->parentSensor->IsActive()) {
00224     if (this->depth_image_connect_count_ <= 0 &&
00225         (*this->image_connect_count_) <= 0) {
00226       this->parentSensor->SetActive(false);
00227     } else {
00228       if ((*this->image_connect_count_) > 0) this->PutCameraData(_image);
00229     }
00230   } else {
00231     if ((*this->image_connect_count_) > 0) {
00232       // if parent is disabled, but has subscribers, enable it.
00233       this->parentSensor->SetActive(true);
00234     }
00235   }
00236 }
00237 
00238 void GazeboNoisyDepth::FillDepthImage(const float *_src) {
00239   this->lock_.lock();
00240   // copy data into image
00241   this->depth_image_msg_.header.frame_id = this->frame_name_;
00242   this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
00243   this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
00244 
00245   // copy from depth to depth image message
00246   if(FillDepthImageHelper(this->height, this->width,this->skip_, _src, &this->depth_image_msg_)){
00247     this->depth_image_pub_.publish(this->depth_image_msg_);
00248   }
00249 
00250   this->lock_.unlock();
00251 }
00252 
00253 bool GazeboNoisyDepth::FillDepthImageHelper(const uint32_t rows_arg,
00254                                             const uint32_t cols_arg,
00255                                             const uint32_t step_arg,
00256                                             const float *data_arg,
00257                                             sensor_msgs::Image *image_msg) {
00258   if(data_arg == nullptr){
00259     ROS_WARN_NAMED("NoisyDepth", "Invalid data array received - nullptr.");
00260     return false;
00261   }
00262 
00263   image_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00264   image_msg->height = rows_arg;
00265   image_msg->width = cols_arg;
00266   image_msg->step = sizeof(float) * cols_arg;
00267   image_msg->data.resize(rows_arg * cols_arg * sizeof(float));
00268   image_msg->is_bigendian = 0;
00269 
00270   float *dest = (float *)(&(image_msg->data[0]));
00271   memcpy(dest, data_arg, sizeof(float) * width * height);
00272 
00273   noise_model->ApplyNoise(width, height, dest);
00274 
00275   return true;
00276 }
00277 
00278 void GazeboNoisyDepth::PublishCameraInfo() {
00279 
00280   // first publish parent camera info (ir camera)
00281   GazeboRosCameraUtils::PublishCameraInfo();
00282 
00283   if (this->depth_info_connect_count_ > 0) {
00284     this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
00285 #if GAZEBO_MAJOR_VERSION >= 8
00286     common::Time cur_time = this->world_->SimTime();
00287 #else
00288     common::Time cur_time = this->world_->GetSimTime();
00289 #endif
00290 
00291     if (this->sensor_update_time_ -
00292             this->last_depth_image_camera_info_update_time_ >= this->update_period_) {
00293       this->GazeboRosCameraUtils::PublishCameraInfo(this->depth_image_camera_info_pub_);
00294       this->last_depth_image_camera_info_update_time_ = this->sensor_update_time_;
00295     }
00296   }
00297 }
00298 }


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43