average_point_cloud_alg_node.cpp

Go to the documentation of this file.

#include "average_point_cloud_alg_node.h"
#include <boost/foreach.hpp>


AveragePointCloudAlgNode::AveragePointCloudAlgNode(void) :iBuffer_ (5), iCurrent_ (0), init_ (false)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  
  //TODO:read this value!!
  cloudBuffer_.resize(iBuffer_);
  // [init publishers]
  pcl2_pub_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("output", 1);
  
  // [init subscribers]
  pcl2_sub_ = public_node_handle_.subscribe<sensor_msgs::PointCloud2>("input", 10, &AveragePointCloudAlgNode::input_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void AveragePointCloudAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->PointCloud2_msg.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  //this->pcl2_pub_.publish(this->pcl2_final_msg_);
  //this->pcl2_pub_.publish(this->PointCloud_msg_);
}

/*  [subscriber callbacks] */
void AveragePointCloudAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{ 

  if (pcl2_pub_.getNumSubscribers () > 0) 
  {
    //ROS_INFO("AveragePointCloudAlgNode::input_callback: New Message Received %d %d %d", iCurrent_, msg->width, msg->height);

    //pcl::fromROSMsg(*msg, pcl2_final_msg_);
    //pcl::fromROSMsg(*msg, pcl2_current_msg_);
    pcl2_final_msg_ = *msg;
    pcl2_current_msg_ = *msg;

    if (!init_) {
        // ROS_INFO("INIT"); 
        alg_.lock();
        input_mutex_.enter();
        
        cloudBuffer_.push_back(pcl2_current_msg_);
        if (++iCurrent_ == iBuffer_ - 1 ) {
            init_ = true;
            iCurrent_ = 0;
        }
        alg_.unlock();
        input_mutex_.exit();
    } else {
        // ROS_INFO("AVERAGING"); 
        alg_.lock();
        input_mutex_.enter();

        cloudBuffer_.push_back(pcl2_current_msg_);

        // clear final pcl2
        for (unsigned int rr=0; rr < msg->height; rr++){
            for (unsigned int cc=0; cc < msg->width; cc++){
                int index = rr * pcl2_final_msg_.width + cc;
                float *pstep = (float*)&pcl2_final_msg_.data[(index) * pcl2_final_msg_.point_step];
                pstep[0] = 0;
                pstep[1] = 0;
                pstep[2] = 0;
                pstep[3] = 0;
            }
        }

        // fill the final pcl2 with the averaged buffer pcls
        for (int k=0; k < iBuffer_; k++){
            for (unsigned int rr=0; rr < msg->height; rr++){
                for (unsigned int cc=0; cc < msg->width; cc++){
                    int index = rr * pcl2_final_msg_.width + cc;
                    float *pstep_final = (float*)&pcl2_final_msg_.data[(index) * pcl2_final_msg_.point_step];
                    float *pstep_current =(float*)&cloudBuffer_[k].data[(index) * cloudBuffer_[k].point_step];
                    pstep_final[0] += pstep_current[0]/(iBuffer_);
                    pstep_final[1] += pstep_current[1]/(iBuffer_);
                    pstep_final[2] += pstep_current[2]/(iBuffer_);
                    pstep_final[3] += pstep_current[3]/(iBuffer_);
                }
            }
            //PointCloud::iterator pcl2_current_it = cloudBuffer_[k].begin();
            //pcl2_final_it = pcl2_final_msg_.begin();
            //for (unsigned int rr=0; rr < msg->height; rr++){
            //    for (unsigned int cc=0; cc < msg->width; cc++, ++pcl2_current_it, ++pcl2_final_it){
            //        pcl2_final_it->x += pcl2_current_it->x/(iBuffer_);
            //        pcl2_final_it->y += pcl2_current_it->y/(iBuffer_);
            //        pcl2_final_it->z += pcl2_current_it->z/(iBuffer_);
                //    }
            //}
        }

        alg_.unlock();
        input_mutex_.exit();
        pcl2_pub_.publish(pcl2_final_msg_);
    }
  }
 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void AveragePointCloudAlgNode::node_config_update(Config &config, uint32_t level)
{
  //update driver with new_cfg data
  alg_.lock();
  ROS_INFO ("Reconfigure request :%d, level %d", config.iBuffer, level);
  try
  {
    iBuffer_ = config.iBuffer;
    cloudBuffer_.resize(iBuffer_);
    init_ =false;
  }
  catch (CException & e)
  {
    std::cout << e.what () << std::endl;
  }
  alg_.unlock();
  // save the current configuration
  //this->config_ = config;
}

void AveragePointCloudAlgNode::addNodeDiagnostics(void)
{
}

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