void ExtractCuboidParticlesTopN::extract(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
typename pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr cloud(new pcl::PointCloud<pcl::tracking::ParticleCuboid>);
typename pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr cloud_filtered(new pcl::PointCloud<pcl::tracking::ParticleCuboid>);
pcl::fromROSMsg(*msg, *cloud);
// Sort particles
std::sort(cloud->points.begin(), cloud->points.end(),
compareParticleWeight<pcl::tracking::ParticleCuboid>);
// Take top-n points
double sum = 0;
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
size_t i = 0;
while (sum < top_n_ratio_ && i < cloud->points.size()) {
//new_particles.push_back(cloud->points[i]);
indices->indices.push_back((int)i);
sum += cloud->points[i].weight;
i++;
}
pcl::ExtractIndices<pcl::tracking::ParticleCuboid> ex;
ex.setInputCloud(cloud);
ex.setIndices(indices);
ex.filter(*cloud_filtered);
publishPointIndices(pub_, *indices, msg->header);
publishBoxArray(*cloud_filtered, msg->header);
publishPoseArray(*cloud_filtered, msg->header);
}
// run the particle
void ParticleFilter::start() {
// the asynchronous spinner
ros::AsyncSpinner spinner(1);
// start spinning
spinner.start();
// wait for Control + C
while(ros::ok()) {
// publish the pose array
publishPoseArray();
// broadcast the mean pose
broadcastMeanPose();
// sleep
loop_rate.sleep();
}
}
