camera_object_learner_server_alg_node.cpp

Go to the documentation of this file.

#include "camera_object_learner_server_alg_node.h"

CameraObjectLearnerServerAlgNode::CameraObjectLearnerServerAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<CameraObjectLearnerServerAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  this->received_camera_info = false;
  this->received_person      = false;
  this->init = false;
  // [init publishers]
  
  // [init subscribers]
  this->camera_info_subscriber_ = this->public_node_handle_.subscribe("camera_info", 100, &CameraObjectLearnerServerAlgNode::camera_info_callback, this);
  this->people_subscriber_ = this->public_node_handle_.subscribe("people", 100, &CameraObjectLearnerServerAlgNode::people_callback, this);
  
  // [init services]
  this->init_server_ = this->public_node_handle_.advertiseService("init", &CameraObjectLearnerServerAlgNode::initCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void CameraObjectLearnerServerAlgNode::mainNodeThread(void)
{
  // [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 CameraObjectLearnerServerAlgNode::camera_info_callback(const sensor_msgs::CameraInfo::ConstPtr& msg) 
{ 
  static bool first=true;
  ROS_DEBUG("CameraObjectLearnerServerAlgNode::camera_info_callback: New Message Received"); 

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

  //std::cout << msg->data << std::endl;
  if(first)
  {
    ROS_INFO("CameraObjectLearnerServerAlgNode::camera_info_callback: camera_info loaded!"); 
    this->camera_model.fromCameraInfo(msg);
    this->received_camera_info = true;
    first=false;
  }

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

void CameraObjectLearnerServerAlgNode::people_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg) 
{ 
  ROS_DEBUG("CameraObjectLearnerServerAlgNode::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;
  double mindist = 100;
  double dist    = 100;
  for(unsigned int i=0; i<msg->detection.size(); i++)
  {
    double x = msg->detection[i].pose.pose.position.x;
    double y = msg->detection[i].pose.pose.position.y;
    double z = msg->detection[i].pose.pose.position.z;
    //TODO: subscribe to tracker and choose person with a subscriber to target_id
    //for the moment: chosen nearest person within limits
    dist = x*x+y*y;
    if(dist < mindist*mindist && x>0.5 && x<1.5 && y>-0.25 && y<0.25)
    {
      mindist = dist;
      this->init_point.header  = msg->header;
      this->init_point.point.x = x;
      this->init_point.point.y = y;
      this->init_point.point.z = z;
    }
  }
  if(mindist==100)
    this->received_person = false;
  else
    this->received_person = true;
  //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  //this->people_mutex_.exit(); 
}

/*  [service callbacks] */
bool CameraObjectLearnerServerAlgNode::initCallback(iri_camera_object_learner_server::init::Request &req, iri_camera_object_learner_server::init::Response &res) 
{ 
  ROS_INFO("CameraObjectLearnerServerAlgNode::initCallback: New Request Received!"); 

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock();
  res.x      = 0;
  res.y      = 0;
  res.width  = 0;
  res.height = 0;
  //this->init_mutex_.enter();
  if(this->init)
  {
    if(this->received_camera_info && this->received_person)
    {
      if(this->compute_init_rect())
      {
        res.x      = this->init_rect.x;
        res.y      = this->init_rect.y;
        res.width  = this->init_rect.width;
        res.height = this->init_rect.height;
        ROS_INFO("CameraObjectLearnerServerAlgNode::initCallback: res x,y,w,h: %d, %d, %d, %d", res.x, res.y, res.width, res.height);
      }
      else
      {
        ROS_INFO("CameraObjectLearnerServerAlgNode::initCallback: Rectangle not loaded!"); 
      }
    }
    else
    {
      ROS_INFO("CameraObjectLearnerServerAlgNode::initCallback: camera_info or person_detection not received yet!"); 
    }
  }
  else
  {
    ROS_INFO("CameraObjectLearnerServerAlgNode::initCallback: init param false!"); 
  }
  //if(this->alg_.isRunning()) 
  //{ 
    //ROS_INFO("CameraObjectLearnerServerAlgNode::initCallback: Processin New Request!"); 
    //do operations with req and output on res 
    //res.data2 = req.data1 + my_var; 
  //} 
  //else 
  //{ 
    //ROS_INFO("CameraObjectLearnerServerAlgNode::initCallback: ERROR: alg is not on run mode yet."); 
  //} 

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

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

void CameraObjectLearnerServerAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->init = config.init;
  this->window_width  = config.window_width;
  this->window_height = config.window_height;
  this->window_offset = config.window_offset;
  this->alg_.unlock();
}

void CameraObjectLearnerServerAlgNode::addNodeDiagnostics(void)
{
}

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

bool CameraObjectLearnerServerAlgNode::compute_init_rect()
{
  ROS_INFO("CameraObjectLearnerServerAlgNode::compute_init_rect");
  bool ok = false;
  try
  {
    tf::StampedTransform laser_to_camera_transf;
    ros::Time now = ros::Time::now();
    ros::Duration timeout(5.0f);
    std::string target_frame = this->camera_model.tfFrame();
    std::string source_frame = init_point.header.frame_id;
    bool tf_exists = tf_listener.waitForTransform(target_frame, source_frame, now, timeout, ros::Duration(0.01));
    if(tf_exists)
    {
      //transform laser detection point from point_frame to camera_frame
      tf_listener.lookupTransform(target_frame, source_frame, now, laser_to_camera_transf);
      //ROS_INFO("laser_point: %f, %f, %f", this->init_point.point.x,this->init_point.point.y,this->init_point.point.z);
      tf::Point laser_point(this->init_point.point.x,this->init_point.point.y,this->init_point.point.z);
      tf::Point cam_point(laser_to_camera_transf * laser_point);
      //transform camera point to image point
      //window width and height in meters
      double width_m         = this->window_width; //0.6;
      double height_m        = this->window_height; //0.9;
      double height_offset_m = this->window_offset; //0.4; //distance from leg point to bottom of window
      ROS_INFO("cam_point: %f, %f, %f", cam_point.x(), cam_point.y(), cam_point.z());
      //cv::Point3d cv_cam_point(cam_point.x(),             cam_point.y(), cam_point.z());  
      cv::Point3d cv_cam_point0(cam_point.x(),             cam_point.y()-height_offset_m-height_m/2.0, cam_point.z()); //0 center 
      cv::Point3d cv_cam_point1(cam_point.x()-width_m/2.0, cam_point.y()-height_offset_m-height_m,     cam_point.z()); //1 top left 
      cv::Point3d cv_cam_point2(cam_point.x()+width_m/2.0, cam_point.y()-height_offset_m,              cam_point.z()); //2 bottom right
      //cv::Point2d image_point = this->camera_model.project3dToPixel(cv_cam_point);
      cv::Point2d image_point0 = this->camera_model.project3dToPixel(cv_cam_point0);
      cv::Point2d image_point1 = this->camera_model.project3dToPixel(cv_cam_point1);
      cv::Point2d image_point2 = this->camera_model.project3dToPixel(cv_cam_point2);
      //ROS_INFO("image_point: %f, %f", image_point.x, image_point.y);
      //ROS_INFO("image_point0: %f, %f", image_point0.x, image_point0.y);
      //ROS_INFO("image_point1: %f, %f", image_point1.x, image_point1.y);
      //ROS_INFO("image_point2: %f, %f", image_point2.x, image_point2.y);
      int width  = abs(image_point1.x - image_point2.x);
      int height = abs(image_point1.y - image_point2.y);
      this->init_rect.width  = width;
      this->init_rect.height = height;
      this->init_rect.x      = image_point0.x;
      this->init_rect.y      = image_point0.y;
      ok = true;
    }
    else
    {
      ROS_INFO("CameraObjectLearnerServerAlgNode::compute_init_rect::No transform: %s-->%s", target_frame.c_str(), source_frame.c_str());
    }
  }
  catch (tf::TransformException& ex)
  {
    ROS_WARN("TF exception:\n%s", ex.what());
  }
  return ok;
}