laser_people_detection_fusion_alg_node.cpp

Go to the documentation of this file.

#include "laser_people_detection_fusion_alg_node.h"
#include<tf/tf.h>

LaserPeopleDetectionFusionAlgNode::LaserPeopleDetectionFusionAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<LaserPeopleDetectionFusionAlgorithm>(),
  tf_listener_(ros::Duration(10.f)),
  target_frame_("/front_laser"),
  fixed_frame_("/base_link")
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  people1ready=false;
  people2ready=false;

  // [init publishers]
  this->people_publisher_ = this->public_node_handle_.advertise<iri_nav_msgs::PoseWithCovarianceStampedArray>("people", 100);
  
  // [init subscribers]
  this->people2_subscriber_ = this->public_node_handle_.subscribe("people2", 100, &LaserPeopleDetectionFusionAlgNode::people2_callback, this);
  this->people1_subscriber_ = this->public_node_handle_.subscribe("people1", 100, &LaserPeopleDetectionFusionAlgNode::people1_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  
  std::string tf_prefix;
  public_node_handle_.param<std::string>("tf_prefix", tf_prefix, "");
  
  target_frame_ = tf_prefix + target_frame_;
  fixed_frame_  = tf_prefix  + fixed_frame_;

}

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

void LaserPeopleDetectionFusionAlgNode::mainNodeThread(void)
{
  // [fill msg structures]

  if(people1ready)
  {
    this->people1_mutex_.enter(); 
    PoseWithCovarianceStampedArray_msg_.header = people1_.header;
    for(unsigned int i=0; i<people1_.poses.size(); i++)
    {
      PoseWithCovarianceStampedArray_msg_.poses.push_back(people1_.poses[i]);      
    }
    people1ready=false;
    people1_.poses.clear();
    this->people1_mutex_.exit(); 
  }
  if(people2ready)
  {
    this->people2_mutex_.enter(); 
    if(PoseWithCovarianceStampedArray_msg_.poses.size()==0)
    {
      //fill header if it was empty (if no people1 received before), modifying frame_id
      PoseWithCovarianceStampedArray_msg_.header          = people2_.header;
      PoseWithCovarianceStampedArray_msg_.header.frame_id = target_frame_;
    }

    try
    {
      ros::Time target_time;
      target_time = people2_.header.stamp;
      // wait for a transformation between the source frame (from people2) and the target_frame (front_laser) to be available in the target time (people1.stamp)
      bool tf_exists = tf_listener_.waitForTransform(people2_.header.frame_id, target_frame_, target_time, ros::Duration(10), ros::Duration(0.01));
      if(tf_exists)
      {
        for(unsigned int i=0; i<people2_.poses.size(); i++)
        {
          geometry_msgs::PoseStamped poseIn;
          geometry_msgs::PoseStamped poseOut;

          //poseIn is the i pose of people2, with people2 header (frame, stamp)
          poseIn.header = people2_.header;
          poseIn.pose   = people2_.poses[i].pose;
          poseIn.pose.orientation.z = 1.0;

          //transforms poseIn from its frame (rear_laser) to the target frame (front_laser), generating poseOut
          tf_listener_.transformPose(target_frame_, target_time, poseIn, fixed_frame_, poseOut);

          people2_.poses[i].pose = poseOut.pose;
          people2_.poses[i].covariance = people2_.poses[i].covariance;

          PoseWithCovarianceStampedArray_msg_.poses.push_back(people2_.poses[i]);
        }
      }
      else
      {
        ROS_INFO("LaserPeopleDetectionFusionAlgNode::No transform: %s-->%s", fixed_frame_.c_str(), target_frame_.c_str());
      }
    }
    catch (tf::TransformException &ex)
    {
      ROS_INFO("LaserPeopleDetectionFusionAlgNode:: %s",ex.what());
    }
    people2ready=false;
    people2_.poses.clear();
    this->people2_mutex_.exit(); 
  }

  if(PoseWithCovarianceStampedArray_msg_.poses.size()!=0)
  {
    this->people_publisher_.publish(this->PoseWithCovarianceStampedArray_msg_);
    this->PoseWithCovarianceStampedArray_msg_.poses.clear();
  }

  //this->PoseWithCovarianceStampedArray_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->people_publisher_.publish(this->PoseWithCovarianceStampedArray_msg_);
}

/*  [subscriber callbacks] */

void LaserPeopleDetectionFusionAlgNode::people1_callback(const iri_nav_msgs::PoseWithCovarianceStampedArray::ConstPtr& msg) 
{ 
  //ROS_INFO("LaserPeopleDetectionFusionAlgNode::people1_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->people1_mutex_.enter(); 
  if(msg->poses.size()!=0){
    people1_.header = msg->header;
    people1_.poses  = msg->poses;
    people1ready= true;
  }
  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->people1_mutex_.exit(); 
}

void LaserPeopleDetectionFusionAlgNode::people2_callback(const iri_nav_msgs::PoseWithCovarianceStampedArray::ConstPtr& msg) 
{ 
  //ROS_INFO("LaserPeopleDetectionFusionAlgNode::people2_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->people2_mutex_.enter(); 
  if(msg->poses.size()!=0){
    people2_.header = msg->header;
    people2_.poses  = msg->poses;
    people2ready = true;
  }
  //std::cout << msg->data << std::endl;

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->people2_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void LaserPeopleDetectionFusionAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void LaserPeopleDetectionFusionAlgNode::addNodeDiagnostics(void)
{
}

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