Go to the documentation of this file.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 #include "jsk_pcl_ros/bounding_box_filter.h"
00037
00038 namespace jsk_pcl_ros
00039 {
00040 void BoundingBoxFilter::onInit()
00041 {
00042 ConnectionBasedNodelet::onInit();
00043
00045
00047 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00048 dynamic_reconfigure::Server<Config>::CallbackType f =
00049 boost::bind (
00050 &BoundingBoxFilter::configCallback, this, _1, _2);
00051 srv_->setCallback (f);
00052
00054
00056 pnh_->param("with_indices", with_indices_, true);
00057 filtered_box_pub_
00058 = advertise<jsk_recognition_msgs::BoundingBoxArray>(*pnh_, "output_box", 1);
00059 if (with_indices_) {
00060 filtered_indices_pub_
00061 = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output_indices", 1);
00062 }
00063
00064 onInitPostProcess();
00065 }
00066
00067 void BoundingBoxFilter::subscribe()
00068 {
00069 sub_box_.subscribe(*pnh_, "input_box", 1);
00070 if (with_indices_) {
00071 sub_indices_.subscribe(*pnh_, "input_indices", 1);
00072 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00073 sync_->connectInput(sub_box_, sub_indices_);
00074 sync_->registerCallback(boost::bind(&BoundingBoxFilter::filterWithIndices, this, _1, _2));
00075 } else {
00076 sub_box_.registerCallback(boost::bind(&BoundingBoxFilter::filter, this, _1));
00077 }
00078 }
00079
00080 void BoundingBoxFilter::unsubscribe()
00081 {
00082 sub_box_.unsubscribe();
00083 if (with_indices_) {
00084 sub_indices_.unsubscribe();
00085 }
00086 }
00087
00088 void BoundingBoxFilter::filterBoundingBoxes(
00089 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& box_array_msg,
00090 std::vector<size_t>& keep)
00091 {
00092 for (size_t i = 0; i < box_array_msg->boxes.size(); i++) {
00093 jsk_recognition_msgs::BoundingBox box = box_array_msg->boxes[i];
00094 if (!filter_limit_negative_) {
00095 if (use_x_dimension_) {
00096 if (box.dimensions.x < x_dimension_min_ ||
00097 box.dimensions.x > x_dimension_max_) {
00098 continue;
00099 }
00100 }
00101 if (use_y_dimension_) {
00102 if (box.dimensions.y < y_dimension_min_ ||
00103 box.dimensions.y > y_dimension_max_) {
00104 continue;
00105 }
00106 }
00107 if (use_z_dimension_) {
00108 if (box.dimensions.z < z_dimension_min_ ||
00109 box.dimensions.z > z_dimension_max_) {
00110 continue;
00111 }
00112 }
00113 }
00114 else {
00115 if (use_x_dimension_) {
00116 if (box.dimensions.x > x_dimension_min_ &&
00117 box.dimensions.x < x_dimension_max_) {
00118 continue;
00119 }
00120 }
00121 if (use_y_dimension_) {
00122 if (box.dimensions.y > y_dimension_min_ &&
00123 box.dimensions.y < y_dimension_max_) {
00124 continue;
00125 }
00126 }
00127 if (use_z_dimension_) {
00128 if (box.dimensions.z > z_dimension_min_ &&
00129 box.dimensions.z < z_dimension_max_) {
00130 continue;
00131 }
00132 }
00133 }
00134 keep.push_back(i);
00135 }
00136 }
00137
00138 void BoundingBoxFilter::filter(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& box_array_msg)
00139 {
00140 boost::mutex::scoped_lock lock(mutex_);
00141
00142 jsk_recognition_msgs::BoundingBoxArray filtered_box_array;
00143 filtered_box_array.header = box_array_msg->header;
00144
00145 std::vector<size_t> keep;
00146 filterBoundingBoxes(box_array_msg, keep);
00147 for (size_t j = 0; j < keep.size(); j++)
00148 {
00149 size_t i = keep[j];
00150 filtered_box_array.boxes.push_back(box_array_msg->boxes[i]);
00151 }
00152
00153
00154 filtered_box_pub_.publish(filtered_box_array);
00155
00156
00157 size_t pass_count = keep.size();
00158 size_t remove_count = box_array_msg->boxes.size() - pass_count;
00159 remove_counter_.add(remove_count);
00160 pass_counter_.add(pass_count);
00161 }
00162
00163 void BoundingBoxFilter::filterWithIndices(
00164 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& box_array_msg,
00165 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg)
00166 {
00167 boost::mutex::scoped_lock lock(mutex_);
00168
00169 if (box_array_msg->boxes.size() != indices_msg->cluster_indices.size()) {
00170 NODELET_ERROR(
00171 "the size of message ~input_box and ~input_indices are different");
00172 return;
00173 }
00174 vital_checker_->poke();
00175
00176 jsk_recognition_msgs::BoundingBoxArray filtered_box_array;
00177 jsk_recognition_msgs::ClusterPointIndices filtered_indices;
00178 filtered_box_array.header = box_array_msg->header;
00179 filtered_indices.header = indices_msg->header;
00180
00181 std::vector<size_t> keep;
00182 filterBoundingBoxes(box_array_msg, keep);
00183 for (size_t j = 0; j < keep.size(); j++)
00184 {
00185 size_t i = keep[j];
00186 filtered_box_array.boxes.push_back(box_array_msg->boxes[i]);
00187 filtered_indices.cluster_indices.push_back(indices_msg->cluster_indices[i]);
00188 }
00189
00190
00191 filtered_box_pub_.publish(filtered_box_array);
00192 filtered_indices_pub_.publish(filtered_indices);
00193
00194
00195 size_t pass_count = keep.size();
00196 size_t remove_count = box_array_msg->boxes.size() - pass_count;
00197 remove_counter_.add(remove_count);
00198 pass_counter_.add(pass_count);
00199 }
00200
00201 void BoundingBoxFilter::configCallback(Config &config, uint32_t level)
00202 {
00203 boost::mutex::scoped_lock lock(mutex_);
00204 filter_limit_negative_ = config.filter_limit_negative;
00205 use_x_dimension_ = config.use_x_dimension;
00206 use_y_dimension_ = config.use_y_dimension;
00207 use_z_dimension_ = config.use_z_dimension;
00208 x_dimension_min_ = config.x_dimension_min;
00209 x_dimension_max_ = config.x_dimension_max;
00210 y_dimension_min_ = config.y_dimension_min;
00211 y_dimension_max_ = config.y_dimension_max;
00212 z_dimension_min_ = config.z_dimension_min;
00213 z_dimension_max_ = config.z_dimension_max;
00214 }
00215
00216 void BoundingBoxFilter::updateDiagnostic(
00217 diagnostic_updater::DiagnosticStatusWrapper &stat)
00218 {
00219 if (vital_checker_->isAlive()) {
00220 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00221 "BoundingBoxArray running");
00222 stat.add("Number of filtered box (Avg.)", remove_counter_.mean());
00223 stat.add("Number of passed box (Avg.)", pass_counter_.mean());
00224 jsk_topic_tools::addDiagnosticBooleanStat("Use x dimension", use_x_dimension_, stat);
00225 stat.add("minimum x dimension", x_dimension_min_);
00226 stat.add("maximum x dimension", x_dimension_max_);
00227 jsk_topic_tools::addDiagnosticBooleanStat("Use y dimension", use_y_dimension_, stat);
00228 stat.add("minimum y dimension", y_dimension_min_);
00229 stat.add("maximum y dimension", y_dimension_max_);
00230 jsk_topic_tools::addDiagnosticBooleanStat("Use z dimension", use_z_dimension_, stat);
00231 stat.add("minimum z dimension", z_dimension_min_);
00232 stat.add("maximum z dimension", z_dimension_max_);
00233 jsk_topic_tools::addDiagnosticBooleanStat("Filter limit negative",
00234 filter_limit_negative_, stat);
00235 }
00236 DiagnosticNodelet::updateDiagnostic(stat);
00237 }
00238
00239 }
00240
00241 #include <pluginlib/class_list_macros.h>
00242 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::BoundingBoxFilter, nodelet::Nodelet);