void GazeboNoisyDepth::FillDepthImage(const float *_src) {
  this->lock_.lock();
  // copy data into image
  this->depth_image_msg_.header.frame_id = this->frame_name_;
  this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
  this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;

  // copy from depth to depth image message
  if(FillDepthImageHelper(this->height, this->width,this->skip_, _src, &this->depth_image_msg_)){
    this->depth_image_pub_.publish(this->depth_image_msg_);
  }

  this->lock_.unlock();
}
////////////////////////////////////////////////////////////////////////////////
// Put depth image data to the interface
void GazeboRosOpenniKinect::FillDepthImage(const float *_src) {
	this->lock_.lock();
	// copy data into image
	this->depth_image_msg_.header.frame_id = this->frame_name_;
	this->depth_image_msg_.header.stamp.sec =
			this->depth_sensor_update_time_.sec;
	this->depth_image_msg_.header.stamp.nsec =
			this->depth_sensor_update_time_.nsec;

	///copy from depth to depth image message
	FillDepthImageHelper(this->depth_image_msg_, this->height, this->width,
			this->skip_, (void*) _src);

	this->depth_image_pub_.publish(this->depth_image_msg_);

	this->lock_.unlock();
}