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
00042
00043 #include <pal_vision_segmentation/DepthSegmentConfig.h>
00044
00045
00046 #include <ros/ros.h>
00047 #include <ros/callback_queue.h>
00048 #include <image_transport/image_transport.h>
00049 #include <cv_bridge/cv_bridge.h>
00050 #include <sensor_msgs/image_encodings.h>
00051 #include <dynamic_reconfigure/server.h>
00052 #include <message_filters/subscriber.h>
00053 #include <message_filters/synchronizer.h>
00054 #include <message_filters/sync_policies/approximate_time.h>
00055
00056
00057 #include <opencv2/core/core.hpp>
00058 #include <opencv2/imgproc/imgproc.hpp>
00059 #include <opencv2/highgui/highgui.hpp>
00060
00061
00062 #include <boost/scoped_ptr.hpp>
00063
00064
00065
00066 double threshold;
00067 int dilate_iterations;
00068 int dilate_size;
00069 int erode_iterations;
00070 int erode_size;
00071
00072 image_transport::Publisher mask_pub, img_masked_pub;
00073 cv::Mat imgDepth;
00074 cv::Mat mask;
00075
00076 boost::scoped_ptr< message_filters::Subscriber<sensor_msgs::Image> > imSub;
00077 boost::scoped_ptr< message_filters::Subscriber<sensor_msgs::Image> > imDepthSub;
00078
00079 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
00080 sensor_msgs::Image> ApproxSync;
00081
00082 boost::scoped_ptr< message_filters::Synchronizer<ApproxSync> > synchronizer;
00083
00084 bool enabled;
00085
00086 void getImage(const sensor_msgs::ImageConstPtr& msg,
00087 const std::string& encoding,
00088 cv::Mat& img)
00089 {
00090 cv_bridge::CvImagePtr cv_ptr;
00091 try
00092 {
00093 cv_ptr = cv_bridge::toCvCopy(msg, encoding);
00094 img = cv_ptr->image;
00095 }
00096 catch (cv_bridge::Exception& e)
00097 {
00098 ROS_ERROR("cv_bridge exception: %s", e.what());
00099 return;
00100 }
00101 }
00102
00103 void publishResult(const sensor_msgs::ImageConstPtr& imgMsg)
00104 {
00105 if ( mask_pub.getNumSubscribers() > 0 )
00106 {
00107 cv_bridge::CvImage mask_msg;
00108 mask_msg.header = imgMsg->header;
00109 mask_msg.encoding = sensor_msgs::image_encodings::MONO8;
00110 mask_msg.image = mask;
00111 mask_pub.publish(mask_msg.toImageMsg());
00112 }
00113
00114 if ( img_masked_pub.getNumSubscribers() > 0 )
00115 {
00116 cv::Mat img;
00117
00118 getImage(imgMsg,
00119 sensor_msgs::image_encodings::BGR8,
00120 img);
00121
00122 cv::Mat img_masked;
00123 img.copyTo(img_masked, mask);
00124
00125 cv_bridge::CvImage img_masked_msg;
00126 img_masked_msg.header = imgMsg->header;
00127 img_masked_msg.encoding = sensor_msgs::image_encodings::BGR8;
00128 img_masked_msg.image = img_masked;
00129 img_masked_pub.publish(img_masked_msg.toImageMsg());
00130 }
00131 }
00132
00133 void callback(const sensor_msgs::ImageConstPtr& imMsg,
00134 const sensor_msgs::ImageConstPtr& imDepthMsg)
00135 {
00136 getImage(imDepthMsg,
00137 sensor_msgs::image_encodings::TYPE_32FC1,
00138 imgDepth);
00139
00140
00141 mask = cv::Mat::zeros(imgDepth.size(), CV_8UC1);
00142 for (int r = 0; r < imgDepth.size().height; ++r)
00143 for (int c = 0; c < imgDepth.size().width; ++c)
00144 {
00145 float depth = imgDepth.at<float>(r, c);
00146 if ( !std::isnan(depth) && depth < threshold )
00147 mask.at<uchar>(r, c) = 255;
00148 }
00149
00150
00151 cv::Mat tmp;
00152 cv::erode(mask, tmp,
00153 cv::Mat::ones(erode_size, erode_size, CV_8UC1),
00154 cv::Point(-1, -1), erode_iterations);
00155
00156 cv::dilate(tmp, mask,
00157 cv::Mat::ones(dilate_size, dilate_size, CV_8UC1),
00158 cv::Point(-1, -1), dilate_iterations);
00159
00160 publishResult(imMsg);
00161 }
00162
00163 void reconf_callback(pal_vision_segmentation::DepthSegmentConfig &config, uint32_t level)
00164 {
00165 threshold = config.threshold;
00166 dilate_iterations = config.dilate_iterations;
00167 dilate_size = config.dilate_size;
00168 erode_iterations = config.erode_iterations;
00169 erode_size = config.erode_size;
00170 }
00171
00172 void getParameters(ros::NodeHandle& nh)
00173 {
00174 nh.param<double>("threshold", threshold, 1.6);
00175 nh.param<int>("dilate_iterations", dilate_iterations, 9);
00176 nh.param<int>("dilate_size", dilate_size, 7);
00177 nh.param<int>("erode_iterations", erode_iterations, 1);
00178 nh.param<int>("erode_size", erode_size, 3);
00179 }
00180
00184 void start(ros::NodeHandle& nh)
00185 {
00186 ROS_WARN("Enabling node because there are subscribers");
00187
00188 imSub.reset( new
00189 message_filters::Subscriber<sensor_msgs::Image>(nh,
00190 "image",
00191 5) );
00192
00193
00194 imDepthSub.reset( new
00195 message_filters::Subscriber<sensor_msgs::Image>(nh,
00196 "depth_image",
00197 5) );
00198
00199
00200 synchronizer.reset( new message_filters::Synchronizer<ApproxSync>(10,
00201 *imSub,
00202 *imDepthSub) );
00203
00204 synchronizer->registerCallback(boost::bind(&callback, _1, _2));
00205
00206 enabled = true;
00207 }
00208
00212 void stop()
00213 {
00214 ROS_WARN("Disabling node because there are no subscribers");
00215 synchronizer.reset();
00216 imSub.reset();
00217 imDepthSub.reset();
00218 enabled = false;
00219 }
00220
00221
00222 int main(int argc, char *argv[] )
00223 {
00224 ros::init(argc, argv, "depth_segmentation");
00225 ros::NodeHandle nh;
00226 ros::NodeHandle pnh("~");
00227
00228 ros::CallbackQueue cbQueue;
00229 nh.setCallbackQueue(&cbQueue);
00230 pnh.setCallbackQueue(&cbQueue);
00231
00232 enabled = false;
00233
00234 getParameters(nh);
00235
00236 dynamic_reconfigure::Server<pal_vision_segmentation::DepthSegmentConfig> server(pnh);
00237 dynamic_reconfigure::Server<pal_vision_segmentation::DepthSegmentConfig>::CallbackType f;
00238 f = boost::bind(&reconf_callback, _1, _2);
00239 server.setCallback(f);
00240
00241
00242 image_transport::ImageTransport it(pnh);
00243 mask_pub = it.advertise("mask", 1);
00244 img_masked_pub = it.advertise("image_masked", 1);
00245
00246 int rate = 30;
00247 ros::Rate loopRate(rate);
00248 double halfPeriod = 0.5*1.0/rate;
00249
00250 while ( ros::ok() )
00251 {
00252 bool anySubscriber = img_masked_pub.getNumSubscribers() > 0 ||
00253 mask_pub.getNumSubscribers() > 0;
00254
00255 if ( !enabled && anySubscriber )
00256 start(nh);
00257 else if ( enabled && !anySubscriber )
00258 stop();
00259
00260
00261 cbQueue.callAvailable(ros::WallDuration(halfPeriod));
00262
00263 loopRate.sleep();
00264 }
00265
00266 }
00267