Point Cloud not updating

Question

I am a newbie to point cloud processing. I am trying to align the lidar point cloud with the image data captured by a camera that has been synchronized to the lidar. As a first step, I would like to render my lidar pointcloud using script below but not update it. viewer-> spin() does not seem to be work. Any help will be appreciated.

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
static const std::string OPENCV_WINDOW = "Image window";
using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image1, const boost::shared_ptr<const sensor_msgs::PointCloud2>& input)
 {
   pcl::PCLPointCloud2 pcl_pc2;
   pcl_conversions::toPCL(*input,pcl_pc2);
   pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
   pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
   viewer.showCloud(temp_cloud);
   while (!viewer.wasStopped ())
   {
   }
 }
int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");
  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image1_sub(nh, "/pylon_camera_node/image_rect_color", 1);
  message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh, "/os1_cloud_node/points", 1);
  typedef sync_policies::ApproximateTime<Image, PointCloud2> MySyncPolicy;
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, lidar_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));
  ros::spin();
  return 0;
}

Have you already validated that it works, i.e., updates, in rviz, or just using rostopic echo? BTW, you are not showing the code where you subscribe to updates. In fact it seems that you are spinning inside the callback that should be called by the subscriber. If that is so, then you won't receive new data, because you are blocking. Can you add your subscriber code, then we can show you where to put the spin/while? — Christian Fritz
@ChristianFritz Thanks very much for the response. I have verified with rviz and rostopic echo and it publishes the data. I have included the entire code. Thanks again — Priya Narayanan
yeah, you definitely want to remove that while loop in the callback. The callback needs to return in order to be called again with the new data. You'll also want to create the viewer outside of the callback and only update the data to show inside of that function. — Christian Fritz
I've added a simplified example as an answer. Hope it helps. — Christian Fritz

Christian Fritz Christian Fritz · Accepted Answer · 2020-09-01T15:54:15

Here is a simplified example of how your control needs to flow. It doesn't contain the synchronization with the images you had (I don't example data to test that with). But I think it shows you what you need. As said in the comments, the key is to create the viewer only once and then update it with the new data as it arrives.

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>

pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");

/** receives the point cloud from the subscriber and updates the viewer with it */
void callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
  pcl_conversions::toPCL(*cloud_msg, *cloud);
  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(*cloud, *temp_cloud);
  viewer.showCloud(temp_cloud);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "vision_node");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("/points", 1, callback);
  ros::spin();
  return 0;
}

Point Cloud not updating

1 Answers