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 #include "jsk_pcl_ros/color_filter.h"
00036
00037 #include <pluginlib/class_list_macros.h>
00038
00039 namespace jsk_pcl_ros
00040 {
00041
00042
00043
00044 void RGBColorFilter::onInit()
00045 {
00046 r_max_ = 255;
00047 r_min_ = 0;
00048 g_max_ = 255;
00049 g_min_ = 0;
00050 b_max_ = 255;
00051 b_min_ = 0;
00052
00053 ColorFilter::onInit();
00054 }
00055
00056 void RGBColorFilter::updateCondition()
00057 {
00058 ConditionPtr condp (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00059
00060 int r_max, r_min, g_max, g_min, b_max, b_min;
00061 if ( r_max_ >= r_min_ ) {
00062 r_max = r_max_;
00063 r_min = r_min_;
00064 }
00065 else {
00066 r_max = r_min_;
00067 r_min = r_max_;
00068 }
00069 if ( g_max_ >= g_min_ ) {
00070 g_max = g_max_;
00071 g_min = g_min_;
00072 }
00073 else {
00074 g_max = g_min_;
00075 g_min = g_max_;
00076 }
00077 if ( b_max_ >= b_min_ ) {
00078 b_max = b_max_;
00079 b_min = b_min_;
00080 }
00081 else {
00082 b_max = b_min_;
00083 b_min = b_max_;
00084 }
00085
00086 {
00087 ConditionPtr cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00088 ComparisonPtr le (new Comparison ("r", pcl::ComparisonOps::LE, r_max));
00089 ComparisonPtr ge (new Comparison ("r", pcl::ComparisonOps::GE, r_min));
00090 cond->addComparison (le);
00091 cond->addComparison (ge);
00092 condp->addCondition(cond);
00093 }
00094 {
00095 ConditionPtr cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00096 ComparisonPtr le (new Comparison ("g", pcl::ComparisonOps::LE, g_max));
00097 ComparisonPtr ge (new Comparison ("g", pcl::ComparisonOps::GE, g_min));
00098 cond->addComparison (le);
00099 cond->addComparison (ge);
00100 condp->addCondition(cond);
00101 }
00102 {
00103 ConditionPtr cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00104 ComparisonPtr le (new Comparison ("b", pcl::ComparisonOps::LE, b_max));
00105 ComparisonPtr ge (new Comparison ("b", pcl::ComparisonOps::GE, b_min));
00106 cond->addComparison (le);
00107 cond->addComparison (ge);
00108 condp->addCondition(cond);
00109 }
00110
00111 filter_instance_.setCondition (condp);
00112 }
00113
00114 void RGBColorFilter::configCallback(jsk_pcl_ros::RGBColorFilterConfig &config, uint32_t level)
00115 {
00116 boost::mutex::scoped_lock lock (mutex_);
00117 r_max_ = config.r_limit_max;
00118 r_min_ = config.r_limit_min;
00119 g_max_ = config.g_limit_max;
00120 g_min_ = config.g_limit_min;
00121 b_max_ = config.b_limit_max;
00122 b_min_ = config.b_limit_min;
00123 updateCondition();
00124 }
00125
00126
00127 void HSIColorFilter::onInit()
00128 {
00129 h_max_ = 127;
00130 h_min_ = -128;
00131 s_max_ = 255;
00132 s_min_ = 0;
00133 i_max_ = 255;
00134 i_min_ = 0;
00135
00136 ColorFilter::onInit();
00137 }
00138
00139 void HSIColorFilter::updateCondition()
00140 {
00141 ConditionPtr condp (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00142
00143 int h_max, h_min, s_max, s_min, i_max, i_min;
00144 if ( h_max_ >= h_min_ ) {
00145 h_max = h_max_;
00146 h_min = h_min_;
00147 }
00148 else {
00149 h_max = h_min_;
00150 h_min = h_max_;
00151 }
00152 if ( s_max_ >= s_min_ ) {
00153 s_max = s_max_;
00154 s_min = s_min_;
00155 }
00156 else {
00157 s_max = s_min_;
00158 s_min = s_max_;
00159 }
00160 if ( i_max_ >= i_min_ ) {
00161 i_max = i_max_;
00162 i_min = i_min_;
00163 }
00164 else {
00165 i_max = i_min_;
00166 i_min = i_max_;
00167 }
00168
00169 {
00170 ConditionPtr cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00171 ComparisonPtr le (new Comparison ("h", pcl::ComparisonOps::LE, h_max));
00172 ComparisonPtr ge (new Comparison ("h", pcl::ComparisonOps::GE, h_min));
00173 cond->addComparison (le);
00174 cond->addComparison (ge);
00175 condp->addCondition(cond);
00176 }
00177 {
00178 ConditionPtr cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00179 ComparisonPtr le (new Comparison ("s", pcl::ComparisonOps::LE, s_max));
00180 ComparisonPtr ge (new Comparison ("s", pcl::ComparisonOps::GE, s_min));
00181 cond->addComparison (le);
00182 cond->addComparison (ge);
00183 condp->addCondition(cond);
00184 }
00185 {
00186 ConditionPtr cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
00187 ComparisonPtr le (new Comparison ("i", pcl::ComparisonOps::LE, i_max));
00188 ComparisonPtr ge (new Comparison ("i", pcl::ComparisonOps::GE, i_min));
00189 cond->addComparison (le);
00190 cond->addComparison (ge);
00191 condp->addCondition(cond);
00192 }
00193
00194 filter_instance_.setCondition (condp);
00195 }
00196
00197 void HSIColorFilter::configCallback(jsk_pcl_ros::HSIColorFilterConfig &config, uint32_t level)
00198 {
00199 boost::mutex::scoped_lock lock (mutex_);
00200 h_max_ = config.h_limit_max;
00201 h_min_ = config.h_limit_min;
00202 s_max_ = config.s_limit_max;
00203 s_min_ = config.s_limit_min;
00204 i_max_ = config.i_limit_max;
00205 i_min_ = config.i_limit_min;
00206 updateCondition();
00207 }
00208
00209
00210 template <class PackedComparison, typename Config>
00211 void ColorFilter<PackedComparison, Config>::filter(const sensor_msgs::PointCloud2ConstPtr &input,
00212 const PCLIndicesMsg::ConstPtr& indices)
00213 {
00214
00215 boost::mutex::scoped_lock lock (mutex_);
00216 pcl::PointCloud<pcl::PointXYZRGB> tmp_in, tmp_out;
00217 sensor_msgs::PointCloud2 out;
00218 fromROSMsg(*input, tmp_in);
00219
00220 filter_instance_.setInputCloud (tmp_in.makeShared());
00221 if (indices) {
00222 pcl::IndicesPtr vindices;
00223 vindices.reset(new std::vector<int> (indices->indices));
00224 filter_instance_.setIndices(vindices);
00225 }
00226 filter_instance_.filter(tmp_out);
00227 if (tmp_out.points.size() > 0) {
00228 toROSMsg(tmp_out, out);
00229 pub_.publish(out);
00230 }
00231 }
00232
00233 template <class PackedComparison, typename Config>
00234 void ColorFilter<PackedComparison, Config>::filter(const sensor_msgs::PointCloud2ConstPtr &input)
00235 {
00236 filter(input, PCLIndicesMsg::ConstPtr());
00237 }
00238
00239 template <class PackedComparison, typename Config>
00240 void ColorFilter<PackedComparison, Config>::onInit()
00241 {
00242 ConnectionBasedNodelet::onInit();
00243
00244 filter_instance_ = pcl::ConditionalRemoval<pcl::PointXYZRGB>(true);
00245 updateCondition();
00246 pnh_->param("use_indices", use_indices_, false);
00247 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00248
00249 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00250 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00251 boost::bind (&ColorFilter::configCallback, this, _1, _2);
00252 srv_->setCallback (f);
00253
00254 }
00255
00256 template <class PackedComparison, typename Config>
00257 void ColorFilter<PackedComparison, Config>::subscribe()
00258 {
00259 sub_input_.subscribe(*pnh_, "input", 1);
00260 if (use_indices_) {
00261 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(10);
00262 sub_indices_.subscribe(*pnh_, "indices", 1);
00263 sync_->connectInput(sub_input_, sub_indices_);
00264 sync_->registerCallback(boost::bind(&ColorFilter::filter, this, _1, _2));
00265
00266 }
00267 else {
00268 sub_input_.registerCallback(&ColorFilter::filter, this);
00269 }
00270 }
00271
00272 template <class PackedComparison, typename Config>
00273 void ColorFilter<PackedComparison, Config>::unsubscribe()
00274 {
00275 sub_input_.unsubscribe();
00276 if (use_indices_) {
00277 sub_indices_.unsubscribe();
00278 }
00279 }
00280
00281 }
00282
00283 typedef jsk_pcl_ros::RGBColorFilter RGBColorFilter;
00284 typedef jsk_pcl_ros::HSIColorFilter HSIColorFilter;
00285 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::RGBColorFilter, nodelet::Nodelet);
00286 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::HSIColorFilter, nodelet::Nodelet);