int
main (int argc, char **argv)
{
std::cout << "main function" << std::endl;
pcl::Grabber* interface;
ros::NodeHandle *nh;
ros::Subscriber sub;
ros::Publisher clusterPub, normalPub, planePub,msgPub;
bool spawnObject = true;
K2G *k2g;
processor freenectprocessor = OPENGL;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud;
std::cout << "ros node initialized" << std::endl;
ros::init(argc, argv, "hlpr_single_plane_segmentation",ros::init_options::NoSigintHandler);
nh = new ros::NodeHandle("~");
// nh->getParam("segmentation/viz", viz_);
// std::cout<<"viz is set to " << viz_ << endl;
parsedArguments pA;
if(parseArguments(argc, argv, pA, *nh) < 0)
return 0;
viz_ = pA.viz;
ridiculous_global_variables::ignore_low_sat = pA.saturation_hack;
ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value;
ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value;
RansacSinglePlaneSegmentation multi_plane_app;
pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr ncloud_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Label>::Ptr label_ptr (new pcl::PointCloud<pcl::Label>);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh);
if(viz_) {
viewer = cloudViewer(cloud_ptr);
multi_plane_app.setViewer(viewer);
}
float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0};
multi_plane_app.setWorkingVolumeThresholds(workSpace);
msgPub = nh->advertise<hlpr_perception_msgs::SegClusters>(segOutRostopic,5);
switch (pA.pc_source)
{
case 0:
{
std::cout << "Using ros topic as input" << std::endl;
sub = nh->subscribe<sensor_msgs::PointCloud2>(pA.ros_topic, 1, cloud_cb_ros_);
break;
}
case 1:
{
std::cout << "Using the openni device" << std::endl;
interface = new pcl::OpenNIGrabber ();
boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
break;
}
case 2:
default:
{
std::cout << "Using kinect v2" << std::endl;
freenectprocessor = static_cast<processor>(pA.freenectProcessor);
k2g = new K2G(freenectprocessor, true);
cloud = k2g->getCloud();
prev_cloud = cloud;
gotFirst = true;
break;
}
}
signal(SIGINT, interruptFn);
while (!interrupt && (!viz_ || !viewer->wasStopped ()))
{
if(pA.ros_node)
{
ros::spinOnce();
}
if(viz_)
viewer->spinOnce(20);
if(!gotFirst)
continue;
int selected_cluster_index = -1;
if(cloud_mutex.try_lock ())
{
if(pA.pc_source == 2)
{
cloud = k2g->getCloud();
fake_cloud_cb_kinectv2_(cloud);
}
if(viz_ && pA.justViewPointCloud)
{
pcl::PointCloud<PointT>::Ptr filtered_prev_cloud(new pcl::PointCloud<PointT>(*prev_cloud));
multi_plane_app.preProcPointCloud(filtered_prev_cloud);
if (!viewer->updatePointCloud<PointT> (filtered_prev_cloud, "cloud"))
{
viewer->addPointCloud<PointT> (filtered_prev_cloud, "cloud");
}
selected_cluster_index = -1;
}
else
{
pcl::PointCloud<PointT>::CloudVectorType clusters;
std::vector<pcl::PointCloud<pcl::Normal> > clusterNormals;
Eigen::Vector4f plane;
std::vector<size_t> clusterIndices;
std::vector<std::vector<int>> clusterIndicesStore;
selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore,
pA.pre_proc,
pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer
hlpr_perception_msgs::SegClusters msg;
//msg.header.stamp = ros::Time::now();
// Pull out the cluster indices and put in msg
for (int ti = 0; ti < clusterIndicesStore.size(); ti++){
hlpr_perception_msgs::ClusterIndex cluster_idx_msg;
for (int j = 0; j < clusterIndicesStore[ti].size(); j++){
std_msgs::Int32 temp_msg;
temp_msg.data = clusterIndicesStore[ti][j];
cluster_idx_msg.indices.push_back(temp_msg);
}
msg.cluster_ids.push_back(cluster_idx_msg);
//clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points
}
for(int i = 0; i < clusters.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusters[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.clusters.push_back(out);
}
for(int i = 0; i < clusterNormals.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusterNormals[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.normals.push_back(out);
}
std_msgs::Float32MultiArray planeMsg;
planeMsg.data.clear();
for(int i = 0; i < 4; i++)
planeMsg.data.push_back(plane[i]);
//planePub.publish(planeMsg);
msg.plane = planeMsg;
msgPub.publish(msg);
//clusterPub.publish(clusters[0]);
//normalPub.publish(clusterNormals[0]);
}
cloud_mutex.unlock();
/*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do
* some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color)
* or sending all the plausible ones to c6 and decide there
*/
}
if(selected_cluster_index < 0)
continue;
}
if(pA.pc_source == 1)
{
interface->stop ();
delete interface;
}
delete nh;
ros::shutdown();
return 0;
}
label_ptr label_factory::create(const std::string& text) const
{
return label_ptr(new label(text,color_,size_));
}
