ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

PCL/ROS Cropbox doesn't show anything?

asked 2019-02-08 08:42:02 -0500

Arthur_Ace gravatar image

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 ();
}
edit retag flag offensive close merge delete

Comments

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.

gvdhoorn gravatar image gvdhoorn  ( 2019-02-08 09:23:33 -0500 )edit
1

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.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-02-08 10:10:33 -0500 )edit

What is all your includes? Because I try to have your code running on my laptop and catkin errors appear.

Nikkolas Edi gravatar image Nikkolas Edi  ( 2020-01-30 12:10:22 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-02-11 06:19:04 -0500

Arthur_Ace gravatar image

Found the mistake! I set the Min and Max wrong. I thought the Pointcloud would automatically know that I want to see only stuff that is at least one meter away and at most 4 meters away but what it actually did, was showing me a corner that doesn't have any data. Fixed Min/Max settings with negative numbers to widen the space and it works

edit flag offensive delete link more

Comments

Glad you got this sorted. But have to say:

automatically know that I want. . .

Is never ever a safe assumption to make.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-02-11 06:46:58 -0500 )edit

Question Tools

Stats

Asked: 2019-02-08 08:42:02 -0500

Seen: 1,391 times

Last updated: Feb 11 '19