laser_people_area_filter_alg_node.cpp

Go to the documentation of this file.

#include "laser_people_area_filter_alg_node.h"

LaserPeopleAreaFilterAlgNode::LaserPeopleAreaFilterAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<LaserPeopleAreaFilterAlgorithm>(),
  tf_listener_(ros::Duration(10.f))
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  this->markerWidth=0.5;
  this->markerHeight=0.2;
  this->markerR=1.0;
  this->markerG=1.0;
  this->markerB=0.0;
  this->markerA=0.75;
  this->frame="/map";
  this->xmin=0;
  this->xmax=10;
  this->ymin=-5;
  this->ymax=5;

  // [init publishers]
  this->peopleMarkers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("peopleMarkers", 100);
  this->peopleFiltered_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::detectionArray>("peopleFiltered", 100);
  
  // [init subscribers]
  this->people_subscriber_ = this->public_node_handle_.subscribe("people", 100, &LaserPeopleAreaFilterAlgNode::people_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void LaserPeopleAreaFilterAlgNode::mainNodeThread(void)
{
  alg_.lock();
  if(this->peopleReceived)
  {
    iri_perception_msgs::detectionArray empty;
    this->detectionArray_msg_ = empty; 
    for(unsigned int i=0; i<this->people.detection.size() ; i++)
    {
      geometry_msgs::PoseStamped pose;
      pose.header=this->people.header;
      pose.pose = this->people.detection[i].pose.pose;
      ROS_DEBUG("LaserPeopleAreaFilterAlgNode::mainNodeThread: validating detection x,y,frame: %f, %f, %s",pose.pose.position.x, pose.pose.position.y, pose.header.frame_id.c_str());
      if(!this->validateByArea(pose))
      {
        ROS_DEBUG("LaserPeopleAreaFilterAlgNode::mainNodeThread: discarded detection x,y,frame: %f, %f, %s",pose.pose.position.x, pose.pose.position.y, pose.header.frame_id.c_str());
        continue;
      }
      this->detectionArray_msg_.detection.push_back(this->people.detection[i]);
      this->detectionArray_msg_.header= this->people.header;
    }
    this->peopleReceived=false;

    //this->peopleMarkers_publisher_.publish(this->MarkerArray_msg_);
    if(this->detectionArray_msg_.detection.size()!=0)
    {
      this->peopleFiltered_publisher_.publish(this->detectionArray_msg_);
      this->getPeopleMarkers(detectionArray_msg_, this->MarkerArray_msg_);
      this->peopleMarkers_publisher_.publish(this->MarkerArray_msg_);
    }
  }
  alg_.unlock();

  // [fill msg structures]
  //this->MarkerArray_msg_.data = my_var;
  //this->detectionArray_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->peopleMarkers_publisher_.publish(this->MarkerArray_msg_);
//   this->peopleFiltered_publisher_.publish(this->detectionArray_msg_);
}

/*  [subscriber callbacks] */
void LaserPeopleAreaFilterAlgNode::people_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg) 
{ 
  //ROS_INFO("LaserPeopleAreaFilterAlgNode::people_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
  //this->people_mutex_.enter(); 

  //std::cout << msg->data << std::endl;
  this->people.header     = msg->header;
  this->people.detection  = msg->detection;
  this->peopleReceived    = true;

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

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void LaserPeopleAreaFilterAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->xmax              = config.xmax;
  this->xmin              = config.xmin;
  this->ymax              = config.ymax;
  this->ymin              = config.ymin;

  this->markerWidth       = config.markerWidth;
  this->markerHeight      = config.markerHeight;
  this->markerR           = config.markerR;
  this->markerG           = config.markerG;
  this->markerB           = config.markerB;
  this->markerA           = config.markerA;
  this->frame             = config.frame; 
  this->showArea          = config.showArea;

  this->alg_.unlock();
}

void LaserPeopleAreaFilterAlgNode::addNodeDiagnostics(void)
{
}

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

bool LaserPeopleAreaFilterAlgNode::validateByArea(geometry_msgs::PoseStamped pose)
{
  //TODO transform area limits instead of each people pose
  if(pose.header.frame_id != this->frame)
  {
    try
    {
      std::string source_frame = pose.header.frame_id;
      ros::Time   target_time  = pose.header.stamp;

                                  //waitForTransform(      target_frame, target_time, source_frame, source_time,       fixed_frame,          timeout, polling_sleep_duration);
      //bool tf_exists = tf_listener_.waitForTransform(this->target_frame, target_time, source_frame, source_time, this->fixed_frame, ros::Duration(5), ros::Duration(0.01));

                                  //waitForTransform(target_frame,  source_frame,        time,          timeout, polling_sleep_duration);
      bool tf_exists = tf_listener_.waitForTransform( this->frame,  source_frame, target_time, ros::Duration(5), ros::Duration(0.01));

      if(tf_exists)
      {
        geometry_msgs::PoseStamped poseOut;
                    //transformPose(target_frame, stamped_in, stamped_out)
        tf_listener_.transformPose( this->frame,     pose,     poseOut);
        pose = poseOut;
      }
      else
      {
        ROS_INFO("LaserPeopleAreaFilterAlgNode::No transform: %s-->%s", source_frame.c_str(), this->frame.c_str());
      }
    }
    catch (tf::TransformException &ex)
    {
      ROS_INFO("LaserPeopleAreaFilterAlgNode:: %s",ex.what());
    }
  }
  bool valid;
  if(pose.header.frame_id == this->frame)
  {
    valid = pose.pose.position.x>this->xmin && pose.pose.position.x<this->xmax && pose.pose.position.y>this->ymin && pose.pose.position.y<this->ymax;
  }
  else
  {
    ROS_INFO("LaserPeopleAreaFilterAlgNode:: Unable to validate people position with area, different frames: %s, %s", this->frame.c_str(), pose.header.frame_id.c_str());
    valid = false;
  }

  return valid;

}

void LaserPeopleAreaFilterAlgNode::getPeopleMarkers(iri_perception_msgs::detectionArray & peopleList, visualization_msgs::MarkerArray & people)
{
  static unsigned int lastSize=0;
  people.markers.resize(std::max(uint(peopleList.detection.size()), lastSize));
  lastSize = peopleList.detection.size();

  for(unsigned int i=0; i<peopleList.detection.size(); i++)
  {
    people.markers[i].scale.x = this->markerWidth;
    people.markers[i].scale.y = this->markerWidth;
    people.markers[i].scale.z = this->markerHeight + peopleList.detection[i].probability;

    people.markers[i].pose.position.x = peopleList.detection[i].pose.pose.position.x;
    people.markers[i].pose.position.y = peopleList.detection[i].pose.pose.position.y;
    people.markers[i].pose.position.z = people.markers[i].scale.z/2.0;

    people.markers[i].header.frame_id = peopleList.header.frame_id;
    people.markers[i].header.stamp    = peopleList.header.stamp;

    people.markers[i].id = i+1;
    people.markers[i].type = visualization_msgs::Marker::CYLINDER;
    people.markers[i].action = visualization_msgs::Marker::ADD;

    people.markers[i].lifetime = ros::Duration(0.5f);

    people.markers[i].pose.orientation.x = 0.0;
    people.markers[i].pose.orientation.y = 0.0;
    people.markers[i].pose.orientation.z = 0.0;
    people.markers[i].pose.orientation.w = 1.0;

    people.markers[i].color.r = this->markerR;
    people.markers[i].color.g = this->markerG;
    people.markers[i].color.b = this->markerB;
    people.markers[i].color.a = this->markerA;

  }

  //Delete extra previous markers;
  for(unsigned int i=peopleList.detection.size(); i<people.markers.size(); i++)
  {
    people.markers[i].action = visualization_msgs::Marker::DELETE;
  }

  if(this->showArea)
  {
    visualization_msgs::Marker area;

    area.scale.x = this->xmax-this->xmin;
    area.scale.y = this->ymax-this->ymin;
    area.scale.z = 0.1;

    area.pose.position.x = area.scale.x/2.0 + xmin;
    area.pose.position.y = area.scale.y/2.0 + ymin;
    area.pose.position.z = area.scale.z/2.0;

    area.header.frame_id = this->frame;
    area.header.stamp    = peopleList.header.stamp;

    area.id = 0; //people.markers.size();
    area.type = visualization_msgs::Marker::CUBE;
    area.action = visualization_msgs::Marker::ADD;

    area.lifetime = ros::Duration(0.5f);

    area.pose.orientation.x = 0.0;
    area.pose.orientation.y = 0.0;
    area.pose.orientation.z = 0.0;
    area.pose.orientation.w = 1.0;

    area.color.r = this->markerR;
    area.color.g = this->markerG;
    area.color.b = this->markerB;
    area.color.a = this->markerA/4.0;
    
    people.markers.push_back(area);
    
  }
  

  
  
}