Question

I am new to ROS. pcl::PointCloudpcl::PointNormalCloudout is computed based on pcl::PointCloudpcl::PointXYZ and then published. However, in rostopic list, I cannot find the ROS msg I sent out. P.S. I am sure the message "/d400/depth/color/points" is received and the callback function works until the end of the function. Is there anything I should modify in Cmakefile since I publish pcl::PointCloudpcl::PointNormal

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <boost/foreach.hpp>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef pcl::PointCloud<pcl::PointNormal> PointCloudN;


 void callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& msg)
{   

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudInput;
    cloudInput.reset(new pcl::PointCloud<pcl::PointXYZ> (*msg));

   pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered (new pcl::PointCloud<pcl::PointXYZ>);


// Perform the actual filtering
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloudInput);
sor.setLeafSize (0.25f, 0.25f, 0.25f);
sor.filter (*cloudFiltered); 


    ros::NodeHandle n;
    ros::Publisher pc_pub = n.advertise<PointCloudN>("normal_pointcloud", 1);

pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> normalEstimation;

   // The normal estimation object will use it to find nearest neighbors.
typename pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ> ());
//kdtree->setInputCloud(msg);
normalEstimation.setSearchMethod(kdtree); 
normalEstimation.setInputCloud (cloudFiltered);
normalEstimation.setRadiusSearch(0.3);  

float nx,ny,nz,kurvature;
pcl::PointCloud<pcl::PointNormal> cloud_normals_;

normalEstimation.setViewPoint(0, 0, 1.0);                                                                                                                                                                                           
normalEstimation.compute (cloud_normals_); 

int index = 0;
long temp_size = cloudFiltered->points.size();

for(unsigned int i=0; i < temp_size; i=i+20) {
// x range -35m to 35 m, y ranges -25 to 25m, z ranges 0 to 66
    cloud_normals_.points[index].x = cloudFiltered->points[i].x;
    cloud_normals_.points[index].y = cloudFiltered->points[i].y;
    cloud_normals_.points[index].z = cloudFiltered->points[i].z;
    index ++;
}


//ROS_INFO("test");
pc_pub.publish(cloud_normals_);


}

int main(int argc, char** argv)
{
ros::init(argc, argv, "sub_pcl");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<PointCloud>("/d400/depth/color/points", 1, callback);
    ros::Rate loop_rate(30);

while (ros::ok()) {
        ros::spinOnce();
        loop_rate.sleep();

    }
}

This is the launch file I wrote

<launch>
    <node name="pointcloudlistener" pkg="beginner_tutorials" type="pointcloudlistener">
    <remap from="normal_pointcloud" to="camera/depth_registered/normalpoints"/>
</node>
</launch>

Asked by Chao Chen on 2020-07-30 00:28:15 UTC

Answer 1

Could be your ros node inside the callback might be going out-of-scope. Try ros::NodeHandle n = getNodeHandle()?

But the major issue is, you are recreating

 ros::NodeHandle n;
 ros::Publisher pc_pub = n.advertise<PointCloudN>("normal_pointcloud", 1);

every time you receive the message. I'm not sure if this is the best way to go about it. Instead create a global pointer to the ros::Pubslisher, initialize it in the main, and then keep the rest as it is.

Also, I'm not very familiar with pcl but make sure that PointCloudN is a ros message.

Edit: Ros::Publisher need not be a pointer, just a global variables, so it should look something like this

#include 
...
ros::Publisher global_pc_pub; // global publisher
...
void callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& msg) {
.
.
.
global_pc_pub.publish(cloud_normals_);  // global publisher
}
..
int main() {

ros::init(argc, argv, "sub_pcl");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<PointCloud>("/d400/depth/color/points", 1, callback);
global_pc_pub = nh.advertise<PointCloudN>("normal_pointcloud", 1);  // global publisher
.
.

}

Asked by praskot on 2020-07-30 14:47:54 UTC

Comments

Answer 2

It is fixed, I forgot to add

ros::spin();
oop_rate.sleep();

in callback function

Asked by Chao Chen on 2020-07-31 10:54:33 UTC

Comments

Should

oop_rate.sleep();

be

loop_rate.sleep();

?

Asked by jayess on 2020-07-31 14:50:17 UTC

ROS pcl::PointCloud<pcl::PointNormal> publishment

Comments

Answers

Comments

Comments