feature_map_alg_node.cpp

Go to the documentation of this file.

#include "feature_map_alg_node.h"
#include <std_msgs/Float32.h>

FeatureMapAlgNode::FeatureMapAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<FeatureMapAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  ROS_INFO("Feature map init.");

  // [init publishers]
  this->heat_map_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("classified_point_cloud", 1);

  this->last_eval_max_score_publisher_ = this->public_node_handle_.advertise<std_msgs::Float32>("last_eval_max_score", 1);
  this->last_eval_min_score_publisher_ = this->public_node_handle_.advertise<std_msgs::Float32>("last_eval_min_score", 1);

  // [init subscribers]
  this->desc_subscriber_ = this->public_node_handle_.subscribe("descs", 1, &FeatureMapAlgNode::descs_callback, this);
  
  // [init services]
  classifier_updates_service_ = public_node_handle_.advertiseService("classifier_update", &FeatureMapAlgNode::classifier_update_callback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

FeatureMapAlgNode::~FeatureMapAlgNode(void)
{
  // [free dynamic memory]
}

void FeatureMapAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */
void FeatureMapAlgNode::descs_callback(const normal_descriptor_node::ndesc_pc &ndesc_pc_msg)
{
  pcl::PointCloud<pcl::PointXYZRGB> hmap;
  this->alg_.compute_feature_response_map(ndesc_pc_msg, hmap);
  hmap.is_dense=false;
  // Publish for logging (if needed)
  std_msgs::Float32 msg;
  msg.data = this->alg_.get_last_cluster_max_score();
  this->last_eval_max_score_publisher_.publish(msg);
  msg.data = this->alg_.get_last_cluster_min_score();
  this->last_eval_min_score_publisher_.publish(msg);
  //for(int a=0;a<hmap.points.size();a++)
  //  std::cout<<hmap.points[a]<<std::endl;
  pcl::toROSMsg(hmap, this->cloud_out_);
  //publish cloud_out
  this->cloud_out_.header = ndesc_pc_msg.header;
  //this->cloud_out_.is_dense = false;
  this->heat_map_publisher_.publish(this->cloud_out_);
  ROS_INFO("wrinkle map published");
}


bool FeatureMapAlgNode::classifier_update_callback(
                   iri_publish_params::classifier_update_service::Request & req,
                   iri_publish_params::classifier_update_service::Response & res)
{
  ROS_INFO("FeatureMap: New params!");
  alg_.update_params(req.params);
  ROS_INFO("FeatureMap: Params updated\n");

  res.result = true;
  return true;
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void FeatureMapAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  ROS_INFO("Config update for selected centroid");
  alg_.update_selected_vector(config.selected_centroid);
  this->alg_.unlock();
}

void FeatureMapAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<FeatureMapAlgNode>(argc, argv, "feature_map_alg_node");
}