laser_people_map_filter_alg_node.cpp

Go to the documentation of this file.

#include "laser_people_map_filter_alg_node.h"

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

  this->mapReceived=false;
  this->peopleReceived=false;
  this->neighborRadius = 0.25; //m

  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="noframe";

  this->public_node_handle_.getParam("neighborRadius", this->neighborRadius);

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

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

void LaserPeopleMapFilterAlgNode::mainNodeThread(void)
{
  if(this->mapReceived && this->peopleReceived)
  {
    //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: in poses number %i", people.detection.size());
    iri_perception_msgs::detectionArray empty;
    this->detectionArray_msg_ = empty; 
    for(unsigned int i=0; i<this->people.detection.size() ; i++)
    {
      float x = this->people.detection[i].pose.pose.position.x;
      float y = this->people.detection[i].pose.pose.position.y;
      float res = this->map.info.resolution;
      int width = this->map.info.width;
      //int height= this->map.info.height;
      float x0 = this->map.info.origin.position.x;
      float y0 = this->map.info.origin.position.y;
      //this->map.info.origin.orientation;
      int grid_x = (unsigned int)((x - x0) / res);
      int grid_y = (unsigned int)((y - y0) / res);
      int index = grid_y*width + grid_x;
      int neighborDistance = int(this->neighborRadius/res);
      if(this->checkCellNearObstacle(index, neighborDistance))
      {
        //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: validated pose %d", i);
        //if it's valid, add pose
        this->detectionArray_msg_.detection.push_back(this->peopleIn.detection[i]);
        this->detectionArray_msg_.header= this->peopleIn.header;
      }
      else
      {
        //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: skipped people pose");
      }
    }
    this->peopleReceived=false;
    //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: out poses number %d", this->detectionArray_msg_.detection.size());
    if(this->detectionArray_msg_.detection.size()!=0)
    {
      this->getPeopleMarkers(this->detectionArray_msg_, this->MarkerArray_msg_);
      this->peopleMarkers_publisher_.publish(this->MarkerArray_msg_);
      this->mapFilteredPeople_publisher_.publish(this->detectionArray_msg_);
    }
  }

  // [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->mapFilteredPeople_publisher_.publish(this->detectionArray_msg_);
}

/*  [subscriber callbacks] */
void LaserPeopleMapFilterAlgNode::map_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg) 
{ 
  ROS_DEBUG("LaserPeopleMapFilterAlgNode::map_callback: New Message Received"); 

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

  //std::cout << msg->data << std::endl; 
  if(!mapReceived){
    //ROS_INFO("LaserPeopleMapFilterAlgNode::map_callback: map Received"); 
    this->map.header = msg->header;
    this->map.info   = msg->info;
    this->map.data   = msg->data;
    this->mapReceived = true;
    this->frame = msg->header.frame_id;
  }

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

void LaserPeopleMapFilterAlgNode::people_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg) 
{ 
  ROS_DEBUG("LaserPeopleMapFilterAlgNode::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;
  if(this->mapReceived)
  {
    this->peopleIn.header = msg->header;
    this->peopleIn.detection  = msg->detection;
    this->people.header   = msg->header;
    this->people.detection    = msg->detection;
    this->peopleReceived  = true;

    //if detections are not in map frame, transform them
    //ROS_INFO("poses frame, map frame: %s, %s",msg->header.frame_id.c_str(), this->frame.c_str());
    if(msg->header.frame_id != this->frame)
    {
      try
      {
        std::string source_frame = msg->header.frame_id;
        std::string target_frame = this->frame;
        ros::Time   target_time  = msg->header.stamp;
        bool tf_exists = tf_listener_.waitForTransform(target_frame,  source_frame, target_time, ros::Duration(5), ros::Duration(0.01));
        if(tf_exists)
        {
          geometry_msgs::PoseStamped poseOut;
          geometry_msgs::PoseStamped poseIn;
          for(unsigned int i=0; i<msg->detection.size(); i++)
          {
            //ROS_INFO("transforming pose");
            poseIn.header = msg->header;
            poseIn.pose   = msg->detection[i].pose.pose;
            poseIn.pose.orientation.w = 1;
            tf_listener_.transformPose( target_frame,     poseIn,     poseOut);
            this->people.detection[i].pose.pose = poseOut.pose;
            //ROS_INFO("LaserPeopleMapFilterAlgNode::people_callback: map pose %d: %f, %f", i , poseOut.pose.position.x, poseOut.pose.position.y);
          }
          this->people.header.frame_id = poseOut.header.frame_id;
        }
        else
        {
          ROS_INFO("LaserPeopleMapFilterAlgNode::No transform: %s-->%s", source_frame.c_str(), target_frame.c_str());
          this->peopleReceived = false;
        }
      }
      catch (tf::TransformException &ex)
      {
        ROS_INFO("LaserPeopleMapFilterAlgNode:: %s",ex.what());
        this->peopleReceived = false;
      }
    }
    if(!peopleReceived)
      ROS_INFO("LaserPeopleMapFilterAlgNode::people_callback: unable to use people detections");
  }

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

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void LaserPeopleMapFilterAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->neighborRadius    = config.neighborRadius;
  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->alg_.unlock();
}

void LaserPeopleMapFilterAlgNode::addNodeDiagnostics(void)
{
}

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

bool LaserPeopleMapFilterAlgNode::checkCellNearObstacle(int index, int neighborDistance)
{
  //returns if the index cell is valid or not (obstacles near = not valid)
  bool valid = true;
  int      W = this->map.info.width;
  int      N = neighborDistance;
  //ROS_INFO("LaserPeopleMapFilterAlgNode::checkCellNearObstacle: neighbor distance %i ", neighborDistance); 

  // go over the distance-N neighbors
  for(int j=-N; j<N; j++)
   {
     for(int i=-N; i<N; i++)
     {
       int indexi = i*W+j+index;
       // check if indexi is in the map.data size
       if(indexi >0 && indexi < signed(map.data.size()))
       {
         //if the indexi cell is not free (!=0) the main cell is not valid
         if(this->map.data[indexi]!=0)
         {
           valid=false;
           break;
         }
       }
     }
     if(!valid)
       break;
   }

  return valid;
}

void LaserPeopleMapFilterAlgNode::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 -0.44;

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

    people.markers[i].id = i;
    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;
  }
}