depth_self_filter_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Suat Gedikli */
00036 
00037 #include <moveit/mesh_filter/depth_self_filter_nodelet.h>
00038 #include <moveit/mesh_filter/stereo_camera_model.h>
00039 #include <moveit/mesh_filter/mesh_filter.h>
00040 #include <ros/ros.h>
00041 #include <image_transport/subscriber_filter.h>
00042 #include <sensor_msgs/image_encodings.h>
00043 #include <moveit/robot_model_loader/robot_model_loader.h>
00044 #include <moveit/robot_model/robot_model.h>
00045 #include <eigen3/Eigen/Eigen>
00046 #include <cv_bridge/cv_bridge.h>
00047 
00048 namespace enc = sensor_msgs::image_encodings;
00049 using namespace std;
00050 using namespace boost;
00051 
00052 mesh_filter::DepthSelfFiltering::~DepthSelfFiltering()
00053 {
00054 }
00055 
00056 void mesh_filter::DepthSelfFiltering::onInit()
00057 {
00058   ros::NodeHandle& nh = getNodeHandle();
00059   ros::NodeHandle& private_nh = getPrivateNodeHandle();
00060   input_depth_transport_.reset(new image_transport::ImageTransport(nh));
00061   filtered_depth_transport_.reset(new image_transport::ImageTransport(nh));
00062   filtered_label_transport_.reset(new image_transport::ImageTransport(nh));
00063   model_depth_transport_.reset(new image_transport::ImageTransport(nh));
00064   model_label_transport_.reset(new image_transport::ImageTransport(nh));
00065 
00066   // Read parameters
00067   private_nh.param("queue_size", queue_size_, 1);
00068   private_nh.param("near_clipping_plane_distance", near_clipping_plane_distance_, 0.4);
00069   private_nh.param("far_clipping_plane_distance", far_clipping_plane_distance_, 5.0);
00070   ;
00071   private_nh.param("shadow_threshold", shadow_threshold_, 0.3);
00072   private_nh.param("padding_scale", padding_scale_, 1.0);
00073   private_nh.param("padding_offset", padding_offset_, 0.005);
00074   double tf_update_rate = 30;
00075   private_nh.param("tf_update_rate", tf_update_rate, 30.0);
00076   transform_provider_.setUpdateInterval(long(1000000.0 / tf_update_rate));
00077 
00078   image_transport::SubscriberStatusCallback itssc = bind(&DepthSelfFiltering::connectCb, this);
00079   ros::SubscriberStatusCallback rssc = bind(&DepthSelfFiltering::connectCb, this);
00080 
00081   lock_guard<mutex> lock(connect_mutex_);
00082   pub_filtered_depth_image_ =
00083       filtered_depth_transport_->advertiseCamera("/filtered/depth", queue_size_, itssc, itssc, rssc, rssc);
00084   pub_filtered_label_image_ =
00085       filtered_label_transport_->advertiseCamera("/filtered/labels", queue_size_, itssc, itssc, rssc, rssc);
00086   pub_model_depth_image_ =
00087       model_depth_transport_->advertiseCamera("/model/depth", queue_size_, itssc, itssc, rssc, rssc);
00088   pub_model_label_image_ =
00089       model_depth_transport_->advertiseCamera("/model/label", queue_size_, itssc, itssc, rssc, rssc);
00090 
00091   filtered_depth_ptr_.reset(new cv_bridge::CvImage);
00092   filtered_label_ptr_.reset(new cv_bridge::CvImage);
00093   model_depth_ptr_.reset(new cv_bridge::CvImage);
00094   model_label_ptr_.reset(new cv_bridge::CvImage);
00095 
00096   mesh_filter_.reset(
00097       new MeshFilter<StereoCameraModel>(bind(&TransformProvider::getTransform, &transform_provider_, _1, _2),
00098                                         mesh_filter::StereoCameraModel::RegisteredPSDKParams));
00099   mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
00100   mesh_filter_->setShadowThreshold(shadow_threshold_);
00101   mesh_filter_->setPaddingOffset(padding_offset_);
00102   mesh_filter_->setPaddingScale(padding_scale_);
00103   // add meshesfla
00104   addMeshes(*mesh_filter_);
00105   transform_provider_.start();
00106 }
00107 
00108 void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& depth_msg,
00109                                              const sensor_msgs::CameraInfoConstPtr& info_msg)
00110 {
00111   transform_provider_.setFrame(depth_msg->header.frame_id);
00112   // do filtering here
00113   mesh_filter::StereoCameraModel::Parameters& params = mesh_filter_->parameters();
00114   params.setCameraParameters(info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]);
00115   params.setImageSize(depth_msg->width, depth_msg->height);
00116 
00117   const float* src = (const float*)&depth_msg->data[0];
00118   //*
00119   static unsigned dataSize = 0;
00120   static unsigned short* data = 0;
00121   if (dataSize < depth_msg->width * depth_msg->height)
00122     data = new unsigned short[depth_msg->width * depth_msg->height];
00123   for (unsigned idx = 0; idx < depth_msg->width * depth_msg->height; ++idx)
00124     data[idx] = (unsigned short)(src[idx] * 1000.0);
00125 
00126   mesh_filter_->filter(data, GL_UNSIGNED_SHORT);
00127   // delete[] data;
00128   /*/
00129   mesh_filter_->filter ((void*) &depth_msg->data[0], GL_FLOAT);
00130   //*/
00131   if (pub_filtered_depth_image_.getNumSubscribers() > 0)
00132   {
00133     filtered_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00134     filtered_depth_ptr_->header = depth_msg->header;
00135 
00136     if (filtered_depth_ptr_->image.cols != depth_msg->width || filtered_depth_ptr_->image.rows != depth_msg->height)
00137       filtered_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
00138     mesh_filter_->getFilteredDepth((float*)filtered_depth_ptr_->image.data);
00139     pub_filtered_depth_image_.publish(filtered_depth_ptr_->toImageMsg(), info_msg);
00140   }
00141 
00142   // this is from rendering of the model
00143   if (pub_model_depth_image_.getNumSubscribers() > 0)
00144   {
00145     model_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00146     model_depth_ptr_->header = depth_msg->header;
00147 
00148     if (model_depth_ptr_->image.cols != depth_msg->width || model_depth_ptr_->image.rows != depth_msg->height)
00149       model_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
00150     mesh_filter_->getModelDepth((float*)model_depth_ptr_->image.data);
00151     pub_model_depth_image_.publish(model_depth_ptr_->toImageMsg(), info_msg);
00152   }
00153 
00154   if (pub_filtered_label_image_.getNumSubscribers() > 0)
00155   {
00156     filtered_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
00157     filtered_label_ptr_->header = depth_msg->header;
00158 
00159     if (filtered_label_ptr_->image.cols != depth_msg->width || filtered_label_ptr_->image.rows != depth_msg->height)
00160       filtered_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
00161     mesh_filter_->getFilteredLabels((unsigned int*)filtered_label_ptr_->image.data);
00162     pub_filtered_label_image_.publish(filtered_label_ptr_->toImageMsg(), info_msg);
00163   }
00164 
00165   if (pub_model_label_image_.getNumSubscribers() > 0)
00166   {
00167     model_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
00168     model_label_ptr_->header = depth_msg->header;
00169     if (model_label_ptr_->image.cols != depth_msg->width || model_label_ptr_->image.rows != depth_msg->height)
00170       model_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
00171     mesh_filter_->getModelLabels((unsigned int*)model_label_ptr_->image.data);
00172     pub_model_label_image_.publish(model_label_ptr_->toImageMsg(), info_msg);
00173   }
00174 }
00175 
00176 void mesh_filter::DepthSelfFiltering::addMeshes(MeshFilter<StereoCameraModel>& mesh_filter)
00177 {
00178   robot_model_loader::RobotModelLoader robotModelLoader("robot_description");
00179   robot_model::RobotModelConstPtr robotModel = robotModelLoader.getModel();
00180   const vector<robot_model::LinkModel*>& links = robotModel->getLinkModelsWithCollisionGeometry();
00181   for (size_t i = 0; i < links.size(); ++i)
00182   {
00183     shapes::ShapeConstPtr shape = links[i]->getShape();
00184     if (shape->type == shapes::MESH)
00185     {
00186       const shapes::Mesh& m = static_cast<const shapes::Mesh&>(*shape);
00187       MeshHandle mesh_handle = mesh_filter.addMesh(m);
00188       transform_provider_.addHandle(mesh_handle, links[i]->getName());
00189     }
00190   }
00191 }
00192 
00193 // Handles (un)subscribing when clients (un)subscribe
00194 void mesh_filter::DepthSelfFiltering::connectCb()
00195 {
00196   lock_guard<mutex> lock(connect_mutex_);
00197   if (pub_filtered_depth_image_.getNumSubscribers() == 0 && pub_filtered_label_image_.getNumSubscribers() == 0 &&
00198       pub_model_depth_image_.getNumSubscribers() == 0 && pub_model_label_image_.getNumSubscribers() == 0)
00199   {
00200     sub_depth_image_.shutdown();
00201   }
00202   else if (!sub_depth_image_)
00203   {
00204     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00205     sub_depth_image_ =
00206         input_depth_transport_->subscribeCamera("depth", queue_size_, &DepthSelfFiltering::depthCb, this, hints);
00207   }
00208 }
00209 
00210 void mesh_filter::DepthSelfFiltering::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
00211                                               const sensor_msgs::CameraInfoConstPtr& info_msg)
00212 {
00213   filter(depth_msg, info_msg);
00214 }
00215 
00216 #include <pluginlib/class_list_macros.h>
00217 PLUGINLIB_EXPORT_CLASS(mesh_filter::DepthSelfFiltering, nodelet::Nodelet)


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon Apr 23 2018 10:25:10