00001 #include "filter_pointcloud2_z_value_alg_node.h" 00002 00003 FilterPointcloud2ZValueAlgNode::FilterPointcloud2ZValueAlgNode(void) : 00004 algorithm_base::IriBaseAlgorithm<FilterPointcloud2ZValueAlgorithm>() 00005 { 00006 //init class attributes if necessary 00007 //this->loop_rate_ = 2;//in [Hz] 00008 00009 // [init publishers] 00010 this->output_nan_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("output_nan", 1); 00011 this->output_max_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("output_max", 1); 00012 00013 // [init subscribers] 00014 this->input_subscriber_ = this->public_node_handle_.subscribe("input", 1, &FilterPointcloud2ZValueAlgNode::input_callback, this); 00015 00016 // [init services] 00017 00018 // [init clients] 00019 00020 // [init action servers] 00021 00022 // [init action clients] 00023 } 00024 00025 FilterPointcloud2ZValueAlgNode::~FilterPointcloud2ZValueAlgNode(void) 00026 { 00027 // [free dynamic memory] 00028 } 00029 00030 void FilterPointcloud2ZValueAlgNode::mainNodeThread(void) 00031 { 00032 // [fill msg structures] 00033 //this->PointCloud2_msg_.data = my_var; 00034 00035 // [fill srv structure and make request to the server] 00036 00037 // [fill action structure and make request to the action server] 00038 00039 // [publish messages] 00040 //this->output_nan_publisher_.publish(this->PointCloud2_msg_); 00041 //this->output_max_publisher_.publish(this->PointCloud2_msg_); 00042 } 00043 00044 /* [subscriber callbacks] */ 00045 void FilterPointcloud2ZValueAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 00046 { 00047 //ROS_INFO("FilterPointcloud2ZValueAlgNode::input_callback: New Message Received"); 00048 00049 //use appropiate mutex to shared variables if necessary 00050 alg_.lock(); 00051 input_mutex_.enter(); 00052 00053 if (!alg_.apply_z_value_filter(msg)) 00054 { 00055 ROS_INFO("FilterPointcloud2ZValueAlgNode::input_callback: ERROR: alg is not on run mode yet."); 00056 } 00057 00058 output_nan_publisher_.publish(*alg_.get_nan_pc()); 00059 output_max_publisher_.publish(*alg_.get_max_pc()); 00060 00061 //unlock previously blocked shared variables 00062 alg_.unlock(); 00063 input_mutex_.exit(); 00064 } 00065 00066 /* [service callbacks] */ 00067 00068 /* [action callbacks] */ 00069 00070 /* [action requests] */ 00071 00072 void FilterPointcloud2ZValueAlgNode::node_config_update(Config &config, uint32_t level) 00073 { 00074 alg_.lock(); 00075 alg_.set_z_thr(config.z_thr); 00076 alg_.unlock(); 00077 } 00078 00079 void FilterPointcloud2ZValueAlgNode::addNodeDiagnostics(void) 00080 { 00081 00082 } 00083 00084 /* main function */ 00085 int main(int argc,char *argv[]) 00086 { 00087 return algorithm_base::main<FilterPointcloud2ZValueAlgNode>(argc, argv, "filter_pointcloud2_z_value_alg_node"); 00088 }