00001 #include "filter_table_alg_node.h" 00002 00003 FilterTableAlgNode::FilterTableAlgNode(void) : 00004 algorithm_base::IriBaseAlgorithm<FilterTableAlgorithm>() 00005 { 00006 //init class attributes if necessary 00007 //this->loop_rate_ = 2;//in [Hz] 00008 00009 // [init publishers] 00010 this->filtered_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("points_filtered", 1); 00011 00012 // [init subscribers] 00013 this->cloud_subscriber_ = this->public_node_handle_.subscribe("points", 1, &FilterTableAlgNode::points_callback, this); 00014 00015 // [init services] 00016 00017 // [init clients] 00018 00019 // [init action servers] 00020 00021 // [init action clients] 00022 } 00023 00024 FilterTableAlgNode::~FilterTableAlgNode(void) 00025 { 00026 // [free dynamic memory] 00027 } 00028 00029 void FilterTableAlgNode::mainNodeThread(void) 00030 { 00031 // [fill msg structures] 00032 00033 // [fill srv structure and make request to the server] 00034 00035 // [fill action structure and make request to the action server] 00036 00037 // [publish messages] 00038 } 00039 00040 /* [subscriber callbacks] */ 00041 00042 /* [service callbacks] */ 00043 00044 /* [action callbacks] */ 00045 00046 /* [action requests] */ 00047 00048 void FilterTableAlgNode::node_config_update(Config &config, uint32_t level) 00049 { 00050 this->alg_.lock(); 00051 00052 this->alg_.unlock(); 00053 } 00054 00055 void FilterTableAlgNode::addNodeDiagnostics(void) 00056 { 00057 } 00058 00059 void FilterTableAlgNode::points_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 00060 { 00061 ROS_INFO("Received pointcloud in subscriber"); 00062 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud=this->alg_.filter_table(msg); 00063 sensor_msgs::PointCloud2 out; 00064 std::cout<<cloud->points.size()<<" "<<cloud->width<<" "<<cloud->height<<std::endl; 00065 pcl::toROSMsg(*cloud, out); 00066 this->filtered_publisher_.publish(out); 00067 } 00068 00069 00070 /* main function */ 00071 int main(int argc,char *argv[]) 00072 { 00073 return algorithm_base::main<FilterTableAlgNode>(argc, argv, "filter_table_alg_node"); 00074 }