human_assisted_object_detection_alg_node.cpp

Go to the documentation of this file.

#include "human_assisted_object_detection_alg_node.h"

HumanAssistedObjectDetectionAlgNode::HumanAssistedObjectDetectionAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<HumanAssistedObjectDetectionAlgorithm>(), 
  it_(this->public_node_handle_)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->output_image_publisher_ = it_.advertise("output_image", 1);
  
  // [init subscribers]
  this->face_detect_subscriber_ = this->public_node_handle_.subscribe("face_detect", 100, &HumanAssistedObjectDetectionAlgNode::face_detect_callback, this);
  this->input_image_subscriber_ = it_.subscribe("input_image", 1, &HumanAssistedObjectDetectionAlgNode::input_image_callback, this);
  
  // [init services]
  this->learn_server_ = this->public_node_handle_.advertiseService("learn", &HumanAssistedObjectDetectionAlgNode::learnCallback, this);
  this->detect_server_ = this->public_node_handle_.advertiseService("detect", &HumanAssistedObjectDetectionAlgNode::detectCallback, this);
  this->init_server_ = this->public_node_handle_.advertiseService("init", &HumanAssistedObjectDetectionAlgNode::initCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  this->receiving_detections=false;
  this->receiving_images=false;
}

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

void HumanAssistedObjectDetectionAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->Image_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]
}

/*  [subscriber callbacks] */
void HumanAssistedObjectDetectionAlgNode::face_detect_callback(const face_detector_mono::RectArray::ConstPtr& msg) 
{ 
  //ROS_INFO("HumanAssistedObjectDetectionAlgNode::face_detect_callback: New Message Received");
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->face_detect_mutex_.enter(); 

  if(msg->rects.size()!=0)// there is at least one face detected
  {
    this->face_rects.rects.resize(msg->rects.size());
    for(unsigned int i=0; i<msg->rects.size(); i++)
    {
      this->face_rects.rects[i].x=msg->rects[i].x;
      this->face_rects.rects[i].width=msg->rects[i].width;
      this->face_rects.rects[i].y=msg->rects[i].y;
      this->face_rects.rects[i].height=msg->rects[i].height;
    }
    this->receiving_detections=true;
  } 
  else
  {
    this->receiving_detections=false;
  }
  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->face_detect_mutex_.exit(); 
}
void HumanAssistedObjectDetectionAlgNode::input_image_callback(const sensor_msgs::Image::ConstPtr& msg) 
{ 
  static bool first=true;

  //ROS_INFO("HumanAssistedObjectDetectionAlgNode::input_image_callback: New Message Received"); 

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

  //std::cout << msg->data << std::endl; 
  this->input_image=msg;
  if(first)
  {
    this->receiving_images=true;
    first=false;
  }

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

/*  [service callbacks] */
bool HumanAssistedObjectDetectionAlgNode::learnCallback(iri_human_assisted_object_detection::haod_learn::Request &req, iri_human_assisted_object_detection::haod_learn::Response &res) 
{ 
  ROS_INFO("HumanAssistedObjectDetectionAlgNode::learnCallback: New Request Received!"); 

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

  this->alg_.learn_detector(req.valid);
  this->alg_.get_output_image(this->Image_msg_);
  this->output_image_publisher_.publish(this->Image_msg_);

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

  return true; 
}
bool HumanAssistedObjectDetectionAlgNode::detectCallback(iri_human_assisted_object_detection::haod_detect::Request &req, iri_human_assisted_object_detection::haod_detect::Response &res) 
{ 
  ROS_INFO("HumanAssistedObjectDetectionAlgNode::detectCallback: New Request Received!"); 

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

  if(this->alg_.detect(this->input_image))// need help
  {
    res.success=true;
    ROS_INFO("HumanAssistedObjectDetectionAlgNode::detectCallback: assistance needed!");
  }
  else
  {
    res.success=false;
  }
  this->alg_.get_output_image(this->Image_msg_);
  this->alg_.get_output_rect(this->rect);
  res.x=rect.x;
  res.y=rect.y;
  this->output_image_publisher_.publish(this->Image_msg_);

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

  return true; 
}
bool HumanAssistedObjectDetectionAlgNode::initCallback(iri_human_assisted_object_detection::haod_init::Request &req, iri_human_assisted_object_detection::haod_init::Response &res) 
{ 

  ROS_INFO("HumanAssistedObjectDetectionAlgNode::initCallback: New Request Received!"); 
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->init_mutex_.enter(); 

  this->face_detect_mutex_.enter();
  if(this->receiving_detections && this->receiving_images)
  {
    for(unsigned int i=0;i<this->face_rects.rects.size();i++)
    {
      if(req.x > (this->face_rects.rects[i].x-this->face_rects.rects[i].width/2.0) && 
         req.x < (this->face_rects.rects[i].x+this->face_rects.rects[i].width/2.0) &&
         req.y > (this->face_rects.rects[i].y-this->face_rects.rects[i].height/2.0) && 
         req.y < (this->face_rects.rects[i].y+this->face_rects.rects[i].height/2.0))
      {
        if(this->alg_.init_detector(this->input_image,this->face_rects.rects[i]))// success
          res.success=true;
        else
         res.success=false; 
      }
      this->face_detect_mutex_.exit();
      return true;
    }
    res.success=false;
  }
  else
    res.success=false;

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->init_mutex_.exit(); 
  this->face_detect_mutex_.exit();
  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

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

  this->alg_.unlock();
}

void HumanAssistedObjectDetectionAlgNode::addNodeDiagnostics(void)
{
}

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