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 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/extract_cuboid_particles_top_n.h"
00038 #include "jsk_pcl_ros/pcl_conversion_util.h"
00039 #include "jsk_pcl_ros/pcl_ros_util.h"
00040
00041 namespace jsk_pcl_ros
00042 {
00043
00044 void ExtractCuboidParticlesTopN::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00048 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00049 boost::bind (&ExtractCuboidParticlesTopN::configCallback, this, _1, _2);
00050 srv_->setCallback (f);
00051 pub_ = advertise<pcl_msgs::PointIndices>(*pnh_, "output", 1);
00052 pub_box_array_ = advertise<jsk_recognition_msgs::BoundingBoxArray>(*pnh_, "output/box_array", 1);
00053 }
00054
00055 void ExtractCuboidParticlesTopN::subscribe()
00056 {
00057 sub_ = pnh_->subscribe("input", 1, &ExtractCuboidParticlesTopN::extract, this);
00058 }
00059
00060 void ExtractCuboidParticlesTopN::unsubscribe()
00061 {
00062 sub_.shutdown();
00063 }
00064
00065 void ExtractCuboidParticlesTopN::configCallback(Config& config, uint32_t level)
00066 {
00067 boost::mutex::scoped_lock lock(mutex_);
00068 top_n_ratio_ = config.top_n_ratio;
00069 }
00070
00071 void ExtractCuboidParticlesTopN::extract(const sensor_msgs::PointCloud2::ConstPtr& msg)
00072 {
00073 vital_checker_->poke();
00074 boost::mutex::scoped_lock lock(mutex_);
00075 typename pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr cloud(new pcl::PointCloud<pcl::tracking::ParticleCuboid>);
00076 typename pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr cloud_filtered(new pcl::PointCloud<pcl::tracking::ParticleCuboid>);
00077 pcl::fromROSMsg(*msg, *cloud);
00078
00079 std::sort(cloud->points.begin(), cloud->points.end(),
00080 compareParticleWeight<pcl::tracking::ParticleCuboid>);
00081
00082 double sum = 0;
00083 pcl::PointIndices::Ptr indices(new pcl::PointIndices);
00084 size_t i = 0;
00085 while (sum < top_n_ratio_ && i < cloud->points.size()) {
00086
00087 indices->indices.push_back((int)i);
00088 sum += cloud->points[i].weight;
00089 i++;
00090 }
00091 pcl::ExtractIndices<pcl::tracking::ParticleCuboid> ex;
00092 ex.setInputCloud(cloud);
00093 ex.setIndices(indices);
00094 ex.filter(*cloud_filtered);
00095 publishPointIndices(pub_, *indices, msg->header);
00096 publishBoxArray(*cloud_filtered, msg->header);
00097 }
00098
00099 void ExtractCuboidParticlesTopN::publishBoxArray(
00100 const pcl::PointCloud<pcl::tracking::ParticleCuboid>& particles,
00101 const std_msgs::Header& header)
00102 {
00103 if (pub_box_array_.getNumSubscribers() > 0) {
00104 jsk_recognition_msgs::BoundingBoxArray box_array;
00105 box_array.header = header;
00106 box_array.boxes.resize(particles.points.size());
00107 for (size_t i = 0; i < particles.points.size(); i++) {
00108 pcl::tracking::ParticleCuboid p = particles.points[i];
00109 jsk_recognition_msgs::BoundingBox box;
00110 box.header = header;
00111 Eigen::Affine3f pose = p.toEigenMatrix();
00112 tf::poseEigenToMsg(pose, box.pose);
00113 box.dimensions.x = p.dx;
00114 box.dimensions.y = p.dy;
00115 box.dimensions.z = p.dz;
00116 box.value = p.weight;
00117 box_array.boxes[i] = box;
00118 }
00119 pub_box_array_.publish(box_array);
00120 }
00121 }
00122
00123 }
00124
00125 #include <pluginlib/class_list_macros.h>
00126 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ExtractCuboidParticlesTopN, nodelet::Nodelet);