PCL/ROS Cropbox doesn't show anything?
I am trying to crop my PointCloud2 using ROS and PCL_ROS, but unfortunately I can't find my mistake. The mistake being, that Rviz doesn't show my cropped cloud, even though the topic gets published and I can even select it. I even tried to look into the data by iterating over the cloud, (which also, doesn't show in my terminal). I was wondering if you could help me out?
Here is the code:
/All Includes are here....
ros::Publisher pub;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
// Do data processing here...
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::CropBox<pcl::PCLPointCloud2> boxFilter;
boxFilter.setInputCloud(cloudPtr);
boxFilter.setMin(Eigen::Vector4f(1.0, 1.0, 1.0, 0));
boxFilter.setMax(Eigen::Vector4f(4.0, 4.0, 4.0, 0));
boxFilter.setTranslation(Eigen::Vector3f(0.0, 0.0, 0.0));
boxFilter.filter(cloud_filtered);
sensor_msgs::PointCloud2 output;
pcl_conversions::fromPCL(cloud_filtered, output);
for (sensor_msgs::PointCloud2ConstIterator<float> it(output, "x"); it != it.end(); ++it) {
// TODO: do something with the values of x, y, z
std::cout << it[0] << "/ ";
std::cout << it[1]<< "/ ";
std::cout << it[2]<< "/ ";
std::cout << std::endl;
}
// Publish the data.
pub.publish (output);
}
int main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "CROP");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("/kinect2/qhd/points", 1, cloud_cb);
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("/Cloud_Cropped", 1);
// Spin
ros::spin ();
}
Not an answer, but seeing as your node essentially only crops an incoming pointcloud:
pcl_ros
contains a nr of nodelets, one of which wraps the crop box functionality of PCL. Load that into your driver nodelet manager, configure the input and output topics and you have zero-copy crop box.If you're going to debug this code, it may be an idea to print out the number of points in the incoming and cropped points clouds to the terminal. This would quickly check if the filter is removing 100% of the points in the incoming cloud for example.
What is all your includes? Because I try to have your code running on my laptop and catkin errors appear.