feature_map_alg_node.cpp
Go to the documentation of this file.
00001 #include "feature_map_alg_node.h"
00002 #include <std_msgs/Float32.h>
00003 
00004 FeatureMapAlgNode::FeatureMapAlgNode(void) :
00005   algorithm_base::IriBaseAlgorithm<FeatureMapAlgorithm>()
00006 {
00007   //init class attributes if necessary
00008   //this->loop_rate_ = 2;//in [Hz]
00009   ROS_INFO("Feature map init.");
00010 
00011   // [init publishers]
00012   this->heat_map_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("classified_point_cloud", 1);
00013 
00014   this->last_eval_max_score_publisher_ = this->public_node_handle_.advertise<std_msgs::Float32>("last_eval_max_score", 1);
00015   this->last_eval_min_score_publisher_ = this->public_node_handle_.advertise<std_msgs::Float32>("last_eval_min_score", 1);
00016 
00017   // [init subscribers]
00018   this->desc_subscriber_ = this->public_node_handle_.subscribe("descs", 1, &FeatureMapAlgNode::descs_callback, this);
00019   
00020   // [init services]
00021   classifier_updates_service_ = public_node_handle_.advertiseService("classifier_update", &FeatureMapAlgNode::classifier_update_callback, this);
00022   
00023   // [init clients]
00024   
00025   // [init action servers]
00026   
00027   // [init action clients]
00028 }
00029 
00030 FeatureMapAlgNode::~FeatureMapAlgNode(void)
00031 {
00032   // [free dynamic memory]
00033 }
00034 
00035 void FeatureMapAlgNode::mainNodeThread(void)
00036 {
00037   // [fill msg structures]
00038   
00039   // [fill srv structure and make request to the server]
00040   
00041   // [fill action structure and make request to the action server]
00042 
00043   // [publish messages]
00044 }
00045 
00046 /*  [subscriber callbacks] */
00047 void FeatureMapAlgNode::descs_callback(const normal_descriptor_node::ndesc_pc &ndesc_pc_msg)
00048 {
00049   pcl::PointCloud<pcl::PointXYZRGB> hmap;
00050   this->alg_.compute_feature_response_map(ndesc_pc_msg, hmap);
00051   hmap.is_dense=false;
00052   // Publish for logging (if needed)
00053   std_msgs::Float32 msg;
00054   msg.data = this->alg_.get_last_cluster_max_score();
00055   this->last_eval_max_score_publisher_.publish(msg);
00056   msg.data = this->alg_.get_last_cluster_min_score();
00057   this->last_eval_min_score_publisher_.publish(msg);
00058   //for(int a=0;a<hmap.points.size();a++)
00059   //  std::cout<<hmap.points[a]<<std::endl;
00060   pcl::toROSMsg(hmap, this->cloud_out_);
00061   //publish cloud_out
00062   this->cloud_out_.header = ndesc_pc_msg.header;
00063   //this->cloud_out_.is_dense = false;
00064   this->heat_map_publisher_.publish(this->cloud_out_);
00065   ROS_INFO("wrinkle map published");
00066 }
00067 
00068 
00069 bool FeatureMapAlgNode::classifier_update_callback(
00070                    iri_publish_params::classifier_update_service::Request & req,
00071                    iri_publish_params::classifier_update_service::Response & res)
00072 {
00073   ROS_INFO("FeatureMap: New params!");
00074   alg_.update_params(req.params);
00075   ROS_INFO("FeatureMap: Params updated\n");
00076 
00077   res.result = true;
00078   return true;
00079 }
00080 
00081 /*  [service callbacks] */
00082 
00083 /*  [action callbacks] */
00084 
00085 /*  [action requests] */
00086 
00087 void FeatureMapAlgNode::node_config_update(Config &config, uint32_t level)
00088 {
00089   this->alg_.lock();
00090   ROS_INFO("Config update for selected centroid");
00091   alg_.update_selected_vector(config.selected_centroid);
00092   this->alg_.unlock();
00093 }
00094 
00095 void FeatureMapAlgNode::addNodeDiagnostics(void)
00096 {
00097 }
00098 
00099 /* main function */
00100 int main(int argc,char *argv[])
00101 {
00102   return algorithm_base::main<FeatureMapAlgNode>(argc, argv, "feature_map_alg_node");
00103 }
00104 
00105 
00106 


iri_feature_map
Author(s): Arnau Ramisa
autogenerated on Fri Dec 6 2013 21:14:32