00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00049 namespace enc = sensor_msgs::image_encodings;
00050 using namespace std;
00051 using namespace boost;
00052
00053 mesh_filter::DepthSelfFiltering::~DepthSelfFiltering ()
00054 {
00055 }
00056
00057 void mesh_filter::DepthSelfFiltering::onInit()
00058 {
00059 ros::NodeHandle& nh = getNodeHandle();
00060 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00061 input_depth_transport_.reset(new image_transport::ImageTransport(nh));
00062 filtered_depth_transport_.reset(new image_transport::ImageTransport(nh));
00063 filtered_label_transport_.reset(new image_transport::ImageTransport(nh));
00064 model_depth_transport_.reset(new image_transport::ImageTransport(nh));
00065 model_label_transport_.reset(new image_transport::ImageTransport(nh));
00066
00067
00068 private_nh.param("queue_size", queue_size_, 1);
00069 private_nh.param("near_clipping_plane_distance", near_clipping_plane_distance_, 0.4);
00070 private_nh.param("far_clipping_plane_distance", far_clipping_plane_distance_, 5.0);;
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_ = filtered_depth_transport_->advertiseCamera("/filtered/depth", queue_size_, itssc, itssc, rssc, rssc);
00083 pub_filtered_label_image_ = filtered_label_transport_->advertiseCamera("/filtered/labels", queue_size_, itssc, itssc, rssc, rssc);
00084 pub_model_depth_image_ = model_depth_transport_->advertiseCamera("/model/depth", queue_size_, itssc, itssc, rssc, rssc);
00085 pub_model_label_image_ = model_depth_transport_->advertiseCamera("/model/label", queue_size_, itssc, itssc, rssc, rssc);
00086
00087 filtered_depth_ptr_.reset (new cv_bridge::CvImage);
00088 filtered_label_ptr_.reset (new cv_bridge::CvImage);
00089 model_depth_ptr_.reset (new cv_bridge::CvImage);
00090 model_label_ptr_.reset (new cv_bridge::CvImage);
00091
00092 mesh_filter_.reset (new MeshFilter<StereoCameraModel>(bind(&TransformProvider::getTransform, &transform_provider_, _1, _2),
00093 mesh_filter::StereoCameraModel::RegisteredPSDKParams));
00094 mesh_filter_->parameters ().setDepthRange (near_clipping_plane_distance_, far_clipping_plane_distance_);
00095 mesh_filter_->setShadowThreshold(shadow_threshold_);
00096 mesh_filter_->setPaddingOffset (padding_offset_);
00097 mesh_filter_->setPaddingScale (padding_scale_);
00098
00099 addMeshes (*mesh_filter_);
00100 transform_provider_.start ();
00101 }
00102
00103
00104 void mesh_filter::DepthSelfFiltering::filter (const sensor_msgs::ImageConstPtr& depth_msg,
00105 const sensor_msgs::CameraInfoConstPtr& info_msg)
00106 {
00107 transform_provider_.setFrame (depth_msg->header.frame_id);
00108
00109 mesh_filter::StereoCameraModel::Parameters& params = mesh_filter_->parameters ();
00110 params.setCameraParameters (info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]);
00111 params.setImageSize (depth_msg->width, depth_msg->height);
00112
00113 const float* src = (const float*) &depth_msg->data[0];
00114
00115 static unsigned dataSize = 0;
00116 static unsigned short* data = 0;
00117 if (dataSize < depth_msg->width * depth_msg->height)
00118 data = new unsigned short [depth_msg->width * depth_msg->height];
00119 for (unsigned idx = 0; idx < depth_msg->width * depth_msg->height; ++idx)
00120 data [idx] = (unsigned short)(src [idx] * 1000.0);
00121
00122 mesh_filter_->filter (data, GL_UNSIGNED_SHORT);
00123
00124
00125
00126
00127 if (pub_filtered_depth_image_.getNumSubscribers() > 0)
00128 {
00129 filtered_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00130 filtered_depth_ptr_->header = depth_msg->header;
00131
00132 if (filtered_depth_ptr_->image.cols != depth_msg->width || filtered_depth_ptr_->image.rows != depth_msg->height)
00133 filtered_depth_ptr_->image = cv::Mat (depth_msg->height, depth_msg->width, CV_32FC1);
00134 mesh_filter_->getFilteredDepth ((float*)filtered_depth_ptr_->image.data);
00135 pub_filtered_depth_image_.publish (filtered_depth_ptr_->toImageMsg(), info_msg);
00136 }
00137
00138
00139 if (pub_model_depth_image_.getNumSubscribers() > 0)
00140 {
00141 model_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00142 model_depth_ptr_->header = depth_msg->header;
00143
00144 if (model_depth_ptr_->image.cols != depth_msg->width || model_depth_ptr_->image.rows != depth_msg->height)
00145 model_depth_ptr_->image = cv::Mat (depth_msg->height, depth_msg->width, CV_32FC1);
00146 mesh_filter_->getModelDepth ((float*)model_depth_ptr_->image.data);
00147 pub_model_depth_image_.publish (model_depth_ptr_->toImageMsg(), info_msg);
00148 }
00149
00150 if (pub_filtered_label_image_.getNumSubscribers() > 0)
00151 {
00152 filtered_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
00153 filtered_label_ptr_->header = depth_msg->header;
00154
00155 if (filtered_label_ptr_->image.cols != depth_msg->width || filtered_label_ptr_->image.rows != depth_msg->height)
00156 filtered_label_ptr_->image = cv::Mat (depth_msg->height, depth_msg->width, CV_8UC4);
00157 mesh_filter_->getFilteredLabels ((unsigned int*)filtered_label_ptr_->image.data);
00158 pub_filtered_label_image_.publish (filtered_label_ptr_->toImageMsg(), info_msg);
00159 }
00160
00161 if (pub_model_label_image_.getNumSubscribers() > 0)
00162 {
00163 model_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
00164 model_label_ptr_->header = depth_msg->header;
00165 if (model_label_ptr_->image.cols != depth_msg->width || model_label_ptr_->image.rows != depth_msg->height)
00166 model_label_ptr_->image = cv::Mat (depth_msg->height, depth_msg->width, CV_8UC4);
00167 mesh_filter_->getModelLabels ((unsigned int*)model_label_ptr_->image.data);
00168 pub_model_label_image_.publish (model_label_ptr_->toImageMsg(), info_msg);
00169 }
00170 }
00171
00172 void mesh_filter::DepthSelfFiltering::addMeshes (MeshFilter<StereoCameraModel>& mesh_filter)
00173 {
00174 robot_model_loader::RobotModelLoader robotModelLoader("robot_description");
00175 robot_model::RobotModelConstPtr robotModel = robotModelLoader.getModel();
00176 const vector<robot_model::LinkModel*> &links = robotModel->getLinkModelsWithCollisionGeometry();
00177 for (size_t i = 0 ; i < links.size() ; ++i)
00178 {
00179 shapes::ShapeConstPtr shape = links[i]->getShape();
00180 if (shape->type == shapes::MESH)
00181 {
00182 const shapes::Mesh &m = static_cast<const shapes::Mesh&>(*shape);
00183 MeshHandle mesh_handle = mesh_filter.addMesh (m);
00184 transform_provider_.addHandle (mesh_handle, links[i]->getName ());
00185 }
00186 }
00187 }
00188
00189
00190 void mesh_filter::DepthSelfFiltering::connectCb()
00191 {
00192 lock_guard<mutex> lock(connect_mutex_);
00193 if (pub_filtered_depth_image_.getNumSubscribers() == 0 &&
00194 pub_filtered_label_image_.getNumSubscribers() == 0 &&
00195 pub_model_depth_image_.getNumSubscribers() == 0 &&
00196 pub_model_label_image_.getNumSubscribers() == 0 )
00197 {
00198 sub_depth_image_.shutdown();
00199 }
00200 else if (!sub_depth_image_)
00201 {
00202 image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00203 sub_depth_image_ = input_depth_transport_->subscribeCamera("depth", queue_size_, &DepthSelfFiltering::depthCb, this, hints);
00204 }
00205 }
00206
00207 void mesh_filter::DepthSelfFiltering::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
00208 const sensor_msgs::CameraInfoConstPtr& info_msg)
00209 {
00210 filter (depth_msg, info_msg);
00211 }
00212
00213 #include <pluginlib/class_list_macros.h>
00214
00215 PLUGINLIB_EXPORT_CLASS (mesh_filter::DepthSelfFiltering, nodelet::Nodelet);