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(); }