finddd_alg_node.cpp

Go to the documentation of this file.

#include "finddd_alg_node.h"

FindddAlgNode::FindddAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<FindddAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->descriptor_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::DescriptorSet>("ndesc", 1); 
  
  // [init subscribers]
  this->points_subscriber_ = this->public_node_handle_.subscribe("points", 1, &FindddAlgNode::points_callback, this);
  
  // [init services]
  this->get_finddd_from_pointcloud_server_ = this->public_node_handle_.advertiseService("get_finddd_from_pointcloud", &FindddAlgNode::get_finddd_from_pointcloudCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  this->accumulated_time=0;
}

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

void FindddAlgNode::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] */

/*  [service callbacks] */
bool FindddAlgNode::get_finddd_from_pointcloudCallback(iri_perception_msgs::PclToDescriptorSet::Request &req, iri_perception_msgs::PclToDescriptorSet::Response &res) 
{ 
  ROS_DEBUG("FindddAlgNode::get_finddd_from_pointcloudCallback: New Request Received!"); 

//   sensor_msgs::PointCloud2::Ptr pointcloud(&(req.pointcloud));
  res.descriptor_set = get_finddd_from_pointcloud(req.pointcloud);

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

void FindddAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  ROS_INFO("Config update");

  this->alg_.unlock();
}


void FindddAlgNode::points_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  iri_perception_msgs::DescriptorSet descriptor_set = get_finddd_from_pointcloud(*msg);
  this->descriptor_publisher_.publish(descriptor_set);
}


void FindddAlgNode::addNodeDiagnostics(void)
{
}

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


// other functions

iri_perception_msgs::DescriptorSet FindddAlgNode::get_finddd_from_pointcloud(const sensor_msgs::PointCloud2& ros_pointcloud)
{
  clock_t start=clock();
  ROS_INFO("Received pointcloud in subscriber");
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(ros_pointcloud, cloud);  
  
  iri_perception_msgs::DescriptorSet descriptor_set;
  this->alg_.compute_ndescs_integral_spatial_interpolation(cloud, descriptor_set);
  descriptor_set.header = pcl_conversions::fromPCL(cloud.header);
  
  this->accumulated_time+=( (double)clock() - start ) / CLOCKS_PER_SEC;
  ROS_INFO("Accumulated time %f", this->accumulated_time);
  
  return descriptor_set;
}