people_tracking_mht_alg_node.cpp

Go to the documentation of this file.

#include "people_tracking_mht_alg_node.h"

PeopleTrackingMhtAlgNode::PeopleTrackingMhtAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<PeopleTrackingMhtAlgorithm>()
{
  //init class attributes if necessary
  this->loop_rate_ = 24;//in [Hz]
  this->detectionsReceived=false;

  // [init publishers]
  this->tracksMarkers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("tracksMarkers", 100);
  this->tracks_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::detectionArray>("tracks", 100);
  
  // [init subscribers]
  this->detections_subscriber_ = this->public_node_handle_.subscribe("detections", 100, &PeopleTrackingMhtAlgNode::detections_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void PeopleTrackingMhtAlgNode::mainNodeThread(void)
{
  //ROS_INFO("PeopleTrackingMhtAlgNode::mainNodeThread"); 
  this->detections_mutex_.enter();
  if(this->detectionsReceived)
  {
    this->alg_.iteration(this->detections, this->tracks);
    //ROS_INFO("PeopleTrackingMhtAlgNode::mainNodeThread: tracks.size()=%lu", this->tracks.size());
    this->detectionsReceived=false;
    this->fillTracksMsg();
    this->tracks_publisher_.publish(this->detectionArray_msg_);
    this->publishMarkers();
  }
  this->detections_mutex_.exit();
  // [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->tracksMarkers_publisher_.publish(this->MarkerArray_msg_);
  //this->tracks_publisher_.publish(this->detectionArray_msg_);
}

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

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

  //std::cout << msg->data << std::endl;
  if(!this->detectionsReceived)
  {
    this->detectionArray_msg_.header = msg->header;
    double detection_time = msg->header.stamp.toSec();
    this->detections.clear();
    for(unsigned int i=0; i<msg->detection.size(); i++)
    {
      double x    = msg->detection[i].position.x;
      double y    = msg->detection[i].position.y;
      int    type = msg->type;
      double p    = msg->detection[i].probability;
      //Sdetection detection(x,y,type,p);
      Sdetection detection(x,y,detection_time,type,p);
      this->detections.push_back(detection);
      //ROS_INFO("PeopleTrackingMhtAlgNode::detections_callback: x,y,type,prob,time: %f, %f, %d, %f, %f", x, y, type, p, detection_time); 
    }
    if(this->detections.size()>0)
      this->detectionsReceived=true;
  }
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->detections_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

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

  this->alg_.unlock();
}

void PeopleTrackingMhtAlgNode::addNodeDiagnostics(void)
{
}

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

void PeopleTrackingMhtAlgNode::fillTracksMsg()
{
  this->detectionArray_msg_.type=123; //TODO tracker type?
  this->detectionArray_msg_.detection.resize(this->tracks.size());
  for(unsigned int i=0; i<this->tracks.size(); i++)
  {
    double x=tracks[i].x;
    double y=tracks[i].y;
    int   id=tracks[i].id;
    double p=tracks[i].probability_of_detection;
    this->detectionArray_msg_.detection[i].position.x = x;
    this->detectionArray_msg_.detection[i].position.y = y;
    this->detectionArray_msg_.detection[i].id         = id;
    this->detectionArray_msg_.detection[i].probability= p;
  }
}

void PeopleTrackingMhtAlgNode::publishMarkers()
{
  static unsigned int lastSize=0;
  this->MarkerArray_msg_.markers.resize(std::max(uint(this->detectionArray_msg_.detection.size()), lastSize));
  for(unsigned int i=0; i<this->detectionArray_msg_.detection.size(); i++)
  {
    this->MarkerArray_msg_.markers[i].header = this->detectionArray_msg_.header;
    this->MarkerArray_msg_.markers[i].ns     = "id";
    this->MarkerArray_msg_.markers[i].id     = this->detectionArray_msg_.detection[i].id;
    this->MarkerArray_msg_.markers[i].type   = visualization_msgs::Marker::TEXT_VIEW_FACING;
    this->MarkerArray_msg_.markers[i].action = visualization_msgs::Marker::ADD;

    this->MarkerArray_msg_.markers[i].pose.position.x = this->detectionArray_msg_.detection[i].position.x;
    this->MarkerArray_msg_.markers[i].pose.position.y = this->detectionArray_msg_.detection[i].position.y;
    this->MarkerArray_msg_.markers[i].pose.position.z = this->detectionArray_msg_.detection[i].position.z; //0.2

    //double yaw;
    //geometry_msgs::Quaternion myQ = tf::createQuaternionMsgFromYaw(yaw);
    //this->MarkerArray_msg_.markers[i].pose.orientation = myQ;

    this->MarkerArray_msg_.markers[i].scale.x = 0.6;
    this->MarkerArray_msg_.markers[i].scale.y = 0.6;
    this->MarkerArray_msg_.markers[i].scale.z = 0.6;

    this->MarkerArray_msg_.markers[i].color.r = 1.0;
    this->MarkerArray_msg_.markers[i].color.g = 0.0;
    this->MarkerArray_msg_.markers[i].color.b = 0.0;
    this->MarkerArray_msg_.markers[i].color.a = 1.0;
    this->MarkerArray_msg_.markers[i].lifetime = ros::Duration(0.5f);

    std::stringstream idText;
    idText << this->detectionArray_msg_.detection[i].id;
    this->MarkerArray_msg_.markers[i].text = idText.str();
  }

  //Delete extra previous markers;
  for(unsigned int i=this->detectionArray_msg_.detection.size(); i<this->MarkerArray_msg_.markers.size(); i++)
  {
    this->MarkerArray_msg_.markers[i].header = this->detectionArray_msg_.header;
    this->MarkerArray_msg_.markers[i].action = visualization_msgs::Marker::DELETE;
  }

  this->tracksMarkers_publisher_.publish(this->MarkerArray_msg_);
}