camera_object_learner_alg_node.cpp

Go to the documentation of this file.

#include "camera_object_learner_alg_node.h"

CameraObjectLearnerAlgNode::CameraObjectLearnerAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<CameraObjectLearnerAlgorithm>(),
  it(public_node_handle_)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  this->receiving_images = false;
  this->init_ok          = false;

  // [init publishers]
  this->image_publisher = it.advertise("image_out", 1);
  
  // [init subscribers]
  this->camera_subscriber = it.subscribe("image_in", 1, &CameraObjectLearnerAlgNode::image_callback, this);
  
  // [init services]
  
  // [init clients]
  init_client = this->public_node_handle_.serviceClient<iri_camera_object_learner_server::init>("init_client");
  
  // [init action servers]
  
  // [init action clients]
}

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

void CameraObjectLearnerAlgNode::mainNodeThread(void)
{
  this->alg_.lock();
  if(receiving_images)
  {
    if(!this->init_ok)
    {
      ROS_INFO("CameraObjectLearnerAlgNode::mainNodeThread: Calling init service!");
      if(init_client.call(init_srv))
      {
        if(init_srv.response.width >0 && init_srv.response.height >0)
        {
          this->init_rect = cv::Rect(init_srv.response.x,init_srv.response.y,init_srv.response.width,init_srv.response.height);
          //init alg
          this->alg_.unlock();
          this->init_ok = this->alg_.init_detector(this->input_image,this->init_rect);
          this->alg_.lock();
        }
        else
        {
          ROS_ERROR("CameraObjectLearnerAlgNode::mainNodeThread: Received rectangle empty or wrong!");
        }
      }
      else
      {
        ROS_ERROR("CameraObjectLearnerAlgNode::mainNodeThread: Failed to call init service!");
      }
    }
    else
    {
      ROS_DEBUG("CameraObjectLearnerAlgNode::mainNodeThread: Detect alg iteration!");
      this->alg_.unlock();
      //detect alg
      this->alg_.detect(this->input_image);
      //get image
      this->alg_.get_output_image(this->output_image);
      this->alg_.lock();
      //publish image
      this->image_publisher.publish(this->output_image);
    }
  }
  else
  {
    ROS_WARN("CameraObjectLearnerAlgNode::mainNodeThread: No images received!");
  }
  this->alg_.unlock();
  
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]

}

/*  [subscriber callbacks] */

void CameraObjectLearnerAlgNode::image_callback(const sensor_msgs::ImageConstPtr& image_msg)
{
  static bool first=true;
  ROS_DEBUG("CameraObjectLearnerAlgNode::image_callback: New Message Received");
  this->alg_.lock();
  this->input_image=image_msg;
  if(first)
  {
    this->receiving_images = true;
    first=false;
  }
  this->alg_.unlock();
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void CameraObjectLearnerAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->init_ok = config.init_ok;
  this->alg_.unlock();
}

void CameraObjectLearnerAlgNode::addNodeDiagnostics(void)
{
}

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