How to show pointcloud with pcl method in ros message callback?

asked 2019-04-15 09:03:14 -0500

七小丘人 gravatar image

updated 2019-04-16 08:55:54 -0500

I want to see the pointcloud by the 2D bounding box, but the function showCloud() is hard to use for me.

The cloud viwer window shows nothing.

Here is the code

  pcl::visualization::CloudViewer viewer("Cloud Viewer");
int main(int argc, char **argv) 
{
   ...
   ros_coord_pixel_sub =
   nh.subscribe("/darknet_ros/bounding_boxes", 1, &CoordinateTran::darknetCallback,this);

   ...
  ros::spin();
  return 0;
}

void CoordinateTran::pointCouldCallback( const sensor_msgs::PointCloud2::ConstPtr &point_cloud_msg) 
    {
        if (get_target == 1)
        {
            pcl::PointCloud<pcl::PointXYZ> point_pcl;
            pcl::fromROSMsg(*point_cloud_msg, point_pcl);
           if (point_pcl.isOrganized ())
           {
              u = (x_min + x_max) / 2;
              v = (y_min + y_max) / 2;                
             pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox (new pcl::PointCloud<pcl::PointXYZ>);
              cloud_inbox->width =x_max-x_min+1;
             cloud_inbox->height =y_max - y_min +1;
               cloud_inbox->points.resize (cloud_inbox->width * cloud_inbox->height);
              for (size_t i=0; i< cloud_inbox->width; ++i)
            {
           for (size_t j=0; j< cloud_inbox->height; ++j)
           {
               cloud_inbox->points[i].x =point_pcl.at(x_min+i,y_min+j).x;
               cloud_inbox->points[i].y =point_pcl.at(x_min+i,y_min+j).y;
               cloud_inbox->points[i].z =point_pcl.at(x_min+i,y_min+j).z;
           }
        }                

       viewer.showCloud(cloud_inbox); 


    }
        else
        std::cout << " the pointcloud is not organized " << std::endl;
        //std::cout << "\033[2J\033[1;1H";     // clear terminal
    }

}
edit retag flag offensive close merge delete

Comments

cross link to similar question #q282923

lucasw gravatar imagelucasw ( 2019-04-15 09:50:36 -0500 )edit