segmentation_visualizer.cpp

Go to the documentation of this file.

#include "cob_3d_segmentation/segmentation_visualizer.h"

#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl_conversions/pcl_conversions.h>


using namespace cob_3d_segmentation;

void
SegmentationVisualizer::init()
{

  /* --- Viewports: ---
   *  1y
   *    | 1 | 3 |
   * .5 ----+----
   *    | 2 | 4 |
   *  0    .5    1x
   * 1:
   */
  // xmin, ymin, xmax, ymax

  v_.createViewPort(0.0, 0.0, 0.5, 1.0, v1_);
  v_.setBackgroundColor(0, 1.0, 1.0, v1_);

  v_.createViewPort(0.5, 0.0, 1.0, 1.0, v2_);
  v_.setBackgroundColor(0, 1.0, 1.0, v2_);

  sub_v1_ = nh_.subscribe<sensor_msgs::PointCloud2>
    ("segmentation_cloud", 1, boost::bind(&SegmentationVisualizer::update_v1_cb, this, _1));
  sub_v2_ = nh_.subscribe<sensor_msgs::PointCloud2>
    ("classified_cloud", 1, boost::bind(&SegmentationVisualizer::update_v2_cb, this, _1));
}

void
SegmentationVisualizer::update_v1_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
{
  std::cout << "Received Segmented Cloud" << std::endl;
  pcl::PCLPointCloud2 blob2;
  pcl_conversions::toPCL(*cloud_in, blob2);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr blob(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromPCLPointCloud2(blob2, *blob);
  //pcl::fromROSMsg<pcl::PointXYZRGB>(*cloud_in,*blob);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> col_hdl(blob);
  if (!v_.updatePointCloud<pcl::PointXYZRGB>(blob, col_hdl, "segmented"))
    v_.addPointCloud<pcl::PointXYZRGB>(blob, col_hdl, "segmented", v1_);
}


void
SegmentationVisualizer::update_v2_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
{
  std::cout << "Received Classified Cloud" << std::endl;
  pcl::PCLPointCloud2 blob2;
  pcl_conversions::toPCL(*cloud_in, blob2);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr blob(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromPCLPointCloud2(blob2, *blob);
  //pcl::fromROSMsg<pcl::PointXYZRGB>(*cloud_in,*blob);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> col_hdl(blob);
  if (!v_.updatePointCloud<pcl::PointXYZRGB>(blob, col_hdl, "classified"))
    v_.addPointCloud<pcl::PointXYZRGB>(blob, col_hdl, "classified", v2_);
}


int 
main(int argc, char **argv)
{
  ros::init(argc,argv, "SegmentationVisualizer");
  SegmentationVisualizer v;
  v.init();
  
  while (ros::ok() && v.isRunning())
  {
    ros::spinOnce();
    v.spinOnce();
    usleep(100000);
  }
  return 0;
}