void visualize(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud, pcl::visualization::PCLVisualizer::Ptr pVisualizer)
{
//init visualizer
pVisualizer->setSize(640, 480);
pVisualizer->setPosition(640, 0);
pVisualizer->setBackgroundColor(0x00, 0x00, 0x00);
pVisualizer->initCameraParameters();
pVisualizer->addPointCloud(pCloud, "cloud");
//reload visualizer content
pVisualizer->spinOnce(1000000);
}
virtual void processBufferElement(CloudConstPtr cloud) override {
cloud_viewer_->spinOnce();
if (!cloud_viewer_->updatePointCloud(cloud, "OpenNICloud")) {
cloud_viewer_->setPosition(0, 0);
cloud_viewer_->setSize(cloud->width, cloud->height);
cloud_viewer_->addPointCloud(cloud, "OpenNICloud");
cloud_viewer_->resetCameraViewpoint("OpenNICloud");
cloud_viewer_->setCameraPosition(
0, 0, 0, // Position
0, 0, 1, // Viewpoint
0, -1, 0); // Up
}
// TODO; Figure out how to achieve this
// if(cloud_viewer_->wasStopped()) {
// this->cloud_buffer_.reset(new Buffer<CloudConstPtr>());
// this->stop();
// cloud_viewer_->close();
// }
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"greedy_objrec_ransac_node");
ros::NodeHandle nh;
pcl::console::parse_argument(argc, argv, "--m", method_id);
pcl::console::parse_argument(argc, argv, "--p", path);
pcl::console::parse_argument(argc, argv, "--t", trial_id);
pcl::console::parse_argument(argc, argv, "--link", link_mesh_name);
pcl::console::parse_argument(argc, argv, "--node", node_mesh_name);
pcl::console::parse_argument(argc, argv, "--link-width", linkWidth);
pcl::console::parse_argument(argc, argv, "--node-width", nodeWidth);
pcl::console::parse_argument(argc, argv, "--voxel-size", voxelSize);
linkrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(linkWidth, voxelSize));
noderec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));
objrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));
bool view_flag = false;
if( pcl::console::find_switch(argc, argv, "-v") == true )
view_flag = true;
if( view_flag == true )
{
viewer = pcl::visualization::PCLVisualizer::Ptr (new pcl::visualization::PCLVisualizer());
viewer->initCameraParameters();
viewer->addCoordinateSystem(0.1);
viewer->setSize(640, 480);
viewer->setCameraPosition(0, 0, 0.1, 0, 0, 1, 0, -1, 0);
}
std::stringstream mm;
mm << method_id;
if( method_id >= 3 )
{
ROS_INFO("Loading link mesh from \"%s.obj\"...",link_mesh_name.c_str());
link_mesh = LoadMesh(link_mesh_name+".obj", "link");
ROS_INFO("Loading node mesh from \"%s.obj\"...",node_mesh_name.c_str());
node_mesh = LoadMesh(node_mesh_name+".obj", "node");
mesh_set.push_back(link_mesh);
mesh_set.push_back(node_mesh);
}
switch(method_id)
{
case 0:
case 1:
case 2:
objrec->AddModel(link_mesh_name, "link");
objrec->AddModel(node_mesh_name, "node");
break;
case 3:
case 4:
case 5:
linkrec->AddModel(link_mesh_name, "link");
noderec->AddModel(node_mesh_name, "node");
break;
default:return 0;
}
if (method_id < 3) {
pub_link = nh.advertise<geometry_msgs::PoseArray>("objects",1000);
} else {
pub_link = nh.advertise<geometry_msgs::PoseArray>("links",1000);
pub_node = nh.advertise<geometry_msgs::PoseArray>("nodes",1000);
}
sub = nh.subscribe("points_in",1,callback);
// send and recieve messages
while (ros::ok()) {
ros::spin();
}
return 0;
}