// Decide if we should proccess the point cloud
void pointCloudCallback( const sensor_msgs::PointCloud2ConstPtr& msg )
{
// Only process every nth point cloud, unless we are working on a goal inwhich case process all of them
++process_count_;
if( process_count_ > PROCESS_EVERY_NTH )
{
process_count_ = 0;
}
else
{
// Only do this if we're actually actively working on a goal.
if(!action_server_.isActive())
return;
}
processPointCloud( msg );
}
void cloudCB(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
//*prev = *cloud_msg;
pcl::PCLPointCloud2 temp;
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, temp);
cloud_t::Ptr cloud(new cloud_t);
pcl::fromPCLPointCloud2(temp, *cloud);
std::vector<point_t> centroids;
processPointCloud(cloud, centroids);
if (centroids.size() > 0) {
publishDoor(centroids[0]);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "door_pcl");
ros::NodeHandle n;
// Publisher
pub = n.advertise<geometry_msgs::PointStamped>("door2", 1);
cloud_t::Ptr cloud(new cloud_t);
if (argc < 2) {
std::cout << "Now subscribing to ROS topics" << std::endl;
// Run in realtime through ROS
ros::Subscriber sub = n.subscribe("cloud", 1, cloudCB);
ros::spin();
//prev.reset(new sensor_msgs::PointCloud2());
return (0);
} else if (strcmp(argv[1], "test") == 0) {
// Read all files in directory
std::vector<boost::filesystem::path> paths;
boost::filesystem::directory_iterator end_itr;
for ( boost::filesystem::directory_iterator itr("."); itr != end_itr; ++itr )
{
paths.push_back(itr->path());
}
std::sort(paths.begin(), paths.end());
ros::Rate r(2.);
for (int i=0; i < paths.size(); i++) {
if (pcl::io::loadPCDFile<point_t>(paths[i].string(), *cloud) == -1) {
std::cout << "Reading failed for bag: " << paths[i] << std::endl;
return (-1);
}
std::cout << "Loading bag: " << paths[i] << std::endl;
std::vector<point_t> centroids;
processPointCloud(cloud, centroids);
if (centroids.size() > 0) {
if (centroids[0].z > 0) {
publishDoor(centroids[0]);
}
}
r.sleep();
}
std::cout << "Number of possible doors: " << curr_doors.size() << std::endl;
std::cout << "Printing doors with Y > 0" << std::endl;
for (std::vector<doorPoint>::iterator it = curr_doors.begin(); it != curr_doors.end(); it++){
point_t p = it->center;
if (p.y > 0) {
std::cout << p.x << ", " << p.y << ", " << p.z << ": " << it->freq << std::endl;
}
}
} else {
if (pcl::io::loadPCDFile<point_t>(argv[1], *cloud) == -1) {
std::cout << "Cloud reading failed." << std::endl;
return (-1);
}
std::vector<point_t> centroids;
processPointCloud(cloud, centroids);
// Show pointcloud
if (argc > 2) {
if (strcmp(argv[2], "-c") == 0) {
showPointCloudColor(colored_cloud);
} else if (strcmp(argv[2], "-d") == 0) {
showPointCloudSphere(colored_cloud, centroids);
}
}
}
return (0);
}
