00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
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
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
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
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
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
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
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
00134
00135
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
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
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
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
00233 this->parentSensor->SetActive(true);
00234 }
00235 }
00236 }
00237
00238 void GazeboNoisyDepth::FillDepthImage(const float *_src) {
00239 this->lock_.lock();
00240
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
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
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 }