Go to the documentation of this file.00001
00063 #include "cob_3d_segmentation/segmentation_visualizer.h"
00064
00065 #include <pcl/visualization/point_cloud_handlers.h>
00066 #include <pcl_conversions/pcl_conversions.h>
00067
00068
00069 using namespace cob_3d_segmentation;
00070
00071 void
00072 SegmentationVisualizer::init()
00073 {
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 v_.createViewPort(0.0, 0.0, 0.5, 1.0, v1_);
00086 v_.setBackgroundColor(0, 1.0, 1.0, v1_);
00087
00088 v_.createViewPort(0.5, 0.0, 1.0, 1.0, v2_);
00089 v_.setBackgroundColor(0, 1.0, 1.0, v2_);
00090
00091 sub_v1_ = nh_.subscribe<sensor_msgs::PointCloud2>
00092 ("segmentation_cloud", 1, boost::bind(&SegmentationVisualizer::update_v1_cb, this, _1));
00093 sub_v2_ = nh_.subscribe<sensor_msgs::PointCloud2>
00094 ("classified_cloud", 1, boost::bind(&SegmentationVisualizer::update_v2_cb, this, _1));
00095 }
00096
00097 void
00098 SegmentationVisualizer::update_v1_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
00099 {
00100 std::cout << "Received Segmented Cloud" << std::endl;
00101 pcl::PCLPointCloud2 blob2;
00102 pcl_conversions::toPCL(*cloud_in, blob2);
00103 pcl::PointCloud<pcl::PointXYZRGB>::Ptr blob(new pcl::PointCloud<pcl::PointXYZRGB>);
00104 pcl::fromPCLPointCloud2(blob2, *blob);
00105
00106 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> col_hdl(blob);
00107 if (!v_.updatePointCloud<pcl::PointXYZRGB>(blob, col_hdl, "segmented"))
00108 v_.addPointCloud<pcl::PointXYZRGB>(blob, col_hdl, "segmented", v1_);
00109 }
00110
00111
00112 void
00113 SegmentationVisualizer::update_v2_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
00114 {
00115 std::cout << "Received Classified Cloud" << std::endl;
00116 pcl::PCLPointCloud2 blob2;
00117 pcl_conversions::toPCL(*cloud_in, blob2);
00118 pcl::PointCloud<pcl::PointXYZRGB>::Ptr blob(new pcl::PointCloud<pcl::PointXYZRGB>);
00119 pcl::fromPCLPointCloud2(blob2, *blob);
00120
00121 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> col_hdl(blob);
00122 if (!v_.updatePointCloud<pcl::PointXYZRGB>(blob, col_hdl, "classified"))
00123 v_.addPointCloud<pcl::PointXYZRGB>(blob, col_hdl, "classified", v2_);
00124 }
00125
00126
00127 int
00128 main(int argc, char **argv)
00129 {
00130 ros::init(argc,argv, "SegmentationVisualizer");
00131 SegmentationVisualizer v;
00132 v.init();
00133
00134 while (ros::ok() && v.isRunning())
00135 {
00136 ros::spinOnce();
00137 v.spinOnce();
00138 usleep(100000);
00139 }
00140 return 0;
00141 }