tibi_dabo_face_tracking_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_face_tracking_alg_node.h"

TibiDaboFaceTrackingAlgNode::TibiDaboFaceTrackingAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboFaceTrackingAlgorithm>(),
  follow_target_client_("follow_target", true)
{
  //init class attributes if necessary
  this->loop_rate_ = 20;//in [Hz]

  // [init publishers]
  this->target_joints_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("target_joints", 10);
  
  // [init subscribers]
  this->face_detections_subscriber_ = this->public_node_handle_.subscribe("face_detections", 100, &TibiDaboFaceTrackingAlgNode::face_detections_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  this->JointTrajectoryPoint_msg_.positions.resize(2);
  this->JointTrajectoryPoint_msg_.positions[0]=0;
  this->JointTrajectoryPoint_msg_.positions[1]=0;
  this->JointTrajectoryPoint_msg_.velocities.resize(2);
  this->JointTrajectoryPoint_msg_.velocities[0]=2.61;
  this->JointTrajectoryPoint_msg_.velocities[1]=2.61;
  
  //TODO: read from camera_info
  this->image_width    = 512;
  this->image_height   = 384;
  this->focus_length_x = 833;
  this->focus_length_y = 835;

  this->first=true;
}

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

void TibiDaboFaceTrackingAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->JointTrajectoryPoint_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]
}

/*  [subscriber callbacks] */
void TibiDaboFaceTrackingAlgNode::face_detections_callback(const face_detector_mono::RectArray::ConstPtr& msg) 
{ 
  //static bool first=true;
  double x_mm=0.0, pan_angle=0.0, y_mm=0.0, tilt_angle=0.0;
  ROS_INFO("TibiDaboFaceTrackingAlgNode::face_detections_callback: New Message Received"); 

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

  //std::cout << msg->data << std::endl;

  if(msg->rects.size()!=0)
  {
    x_mm = 2*(msg->rects[0].x + msg->rects[0].width/2.0  - this->image_width/2.0);
    pan_angle = -atan2(x_mm, focus_length_x);

    y_mm = 2*(msg->rects[0].y  +msg->rects[0].height/2.0 - this->image_height/2.0);
    tilt_angle = -atan2(y_mm, focus_length_y);

    this->JointTrajectoryPoint_msg_.positions[0]=pan_angle;
    this->JointTrajectoryPoint_msg_.positions[1]=tilt_angle;

    ROS_INFO("TibiDaboFaceTrackingAlgNode::face_detections_callback: pan %f, tilt %f", pan_angle, tilt_angle);

    if(this->first==true)
    {
      this->target_joints_publisher_.publish(this->JointTrajectoryPoint_msg_);
      this->first=false;
      follow_targetMakeActionRequest();
    }
    else
    {
      this->target_joints_publisher_.publish(this->JointTrajectoryPoint_msg_);
    }
  }

  /*if(first==true)
  {
    if(msg->rects.size()!=0)
    {
      x_mm=2*(msg->rects[0].x+msg->rects[0].width/2.0-256);
      pan_angle=-atan2(x_mm,832);
      y_mm=2*(msg->rects[0].y+msg->rects[0].height/2.0-192);
      tilt_angle=-atan2(y_mm,832);
      this->JointTrajectoryPoint_msg_.positions[0]=pan_angle;
      this->JointTrajectoryPoint_msg_.positions[1]=tilt_angle;
      // [publish messages]
      this->target_joints_publisher_.publish(this->JointTrajectoryPoint_msg_);
      first=false;
      follow_targetMakeActionRequest();
    }
  }
  else
  {
    if(msg->rects.size()!=0)
    {
      x_mm=2*(msg->rects[0].x+msg->rects[0].width/2.0-256);
      pan_angle=-atan2(x_mm,832);
      y_mm=2*(msg->rects[0].y+msg->rects[0].height/2.0-192);
      tilt_angle=-atan2(y_mm,832);
      this->JointTrajectoryPoint_msg_.positions[0]=pan_angle;
      this->JointTrajectoryPoint_msg_.positions[1]=tilt_angle;
      // [publish messages]
      this->target_joints_publisher_.publish(this->JointTrajectoryPoint_msg_);
    }
  }
  */

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

/*  [service callbacks] */

/*  [action callbacks] */
void TibiDaboFaceTrackingAlgNode::follow_targetDone(const actionlib::SimpleClientGoalState& state,  const iri_nav_msgs::followTargetResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void TibiDaboFaceTrackingAlgNode::follow_targetActive() 
{ 
  //ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetActive: Goal just went active!"); 
} 

void TibiDaboFaceTrackingAlgNode::follow_targetFeedback(const iri_nav_msgs::followTargetFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 
  
  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    follow_target_client_.cancelGoal(); 
    //ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetFeedback: Cancelling Action!"); 
  } 
}

/*  [action requests] */
void TibiDaboFaceTrackingAlgNode::follow_targetMakeActionRequest() 
{ 
  ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  follow_target_client_.waitForServer();  
  ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //follow_target_goal_.data = my_desired_goal; 
  follow_target_client_.sendGoal(follow_target_goal_, 
              boost::bind(&TibiDaboFaceTrackingAlgNode::follow_targetDone,     this, _1, _2), 
              boost::bind(&TibiDaboFaceTrackingAlgNode::follow_targetActive,   this), 
              boost::bind(&TibiDaboFaceTrackingAlgNode::follow_targetFeedback, this, _1)); 
  ROS_INFO("TibiDaboFaceTrackingAlgNode::follow_targetMakeActionRequest: Goal Sent. Wait for Result!"); 
}

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

void TibiDaboFaceTrackingAlgNode::addNodeDiagnostics(void)
{
}

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