/home/joel/ros_workspace/lar3/utils/points_from_volume/src/points_from_volume_nodelet.cpp

Go to the documentation of this file.

#ifndef _points_from_volume_nodelet_CPP_
#define _points_from_volume_nodelet_CPP_

#include <ros/ros.h>
#include <points_from_volume/points_from_volume.h>

// Global Vars
ros::NodeHandle* p_n;
ros::Publisher cloud_pub;
bool defined_hull=false;
double positive;
double negative;
bool flag;
// Class member
points_from_volume<pcl::PointXYZ> piv;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr & pcmsg_in)
{
                pcl::PointCloud<pcl::PointXYZ> pc_in;
                pcl::fromROSMsg(*pcmsg_in,pc_in);
                pcl::PointCloud<pcl::PointXYZ> pc_filtered;
                piv.convexhull_function(pc_in, positive, negative, flag);
                sensor_msgs::PointCloud2 pcmsg_out;
                pcl::toROSMsg(piv.get_pc_in_volume(), pcmsg_out);
                // Uncomment the next line to print the point cloud size
            // std::cout<<"Num points "<<pc_filtered.points.size()<<std::endl;
                cloud_pub.publish(pcmsg_out);
}

void cb_points_convexhull (const sensor_msgs::PointCloud2ConstPtr & pcmsg_convexhull_in)
{
        printf("-------Flag is now True-------\n");
        piv.set_convex_hull(pcmsg_convexhull_in);
        defined_hull=true;
}


int main(int argc, char **argv)
{
        ros::init(argc, argv, "points_from_volume_nodelet"); 
        ros::NodeHandle n("~"); 
//      tf::TransformListener listener(n,ros::Duration(10));
//      p_listener=&listener;
        p_n=&n;
        
        // Set default values on parameters
        if(!positive)
        {
                ROS_WARN_ONCE("Positive_offset not defined. Setting to default value '2.0'\n");
                n.param("positive_offset", positive, 2.0);
        }
        if(!negative)
        {
                ROS_WARN_ONCE("Negative_offset not defined. Setting to default value '-0.1'\n");
                n.param("negative_offset", negative, -0.1);
        }
        if(!flag)
        {
                ROS_WARN_ONCE("Flag_in_out not defined. Setting to default value 'false'\n");
                n.param("flag_in_out", flag, false);
        }
        
        // Point clouds subscription
        ros::Subscriber sub1 = n.subscribe ("/points_convexhull", 1, cb_points_convexhull);
        ros::Subscriber sub2 = n.subscribe ("/point_cloud_input", 1, cloud_cb);  
        
        if(defined_hull==false)
        {
                ROS_WARN("No point cloud convexhull received yet...");
        }
        
        // Pubishes the point cloud
        cloud_pub = n.advertise<sensor_msgs::PointCloud2>("/pcmsg_out", 1);
        
        ros::Rate loop_rate(30);
        ros::spin();
}
#endif