tibi_dabo_head_tracking_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_head_tracking_alg_node.h"

TibiDaboHeadTrackingAlgNode::TibiDaboHeadTrackingAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboHeadTrackingAlgorithm>(),
  follow_target_aserver_(public_node_handle_, "follow_target")
{
  //init class attributes if necessary
  this->loop_rate_ = 50;//in [Hz]

  // [init publishers]
  this->joint_position_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("joint_position", 10);
  
  // [init subscribers]
  this->target_joints_subscriber_ = this->public_node_handle_.subscribe("target_joints", 10, &TibiDaboHeadTrackingAlgNode::target_joints_callback, this);
  this->joint_feedback_subscriber_ = this->public_node_handle_.subscribe("joint_feedback", 10, &TibiDaboHeadTrackingAlgNode::joint_feedback_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  follow_target_aserver_.registerStartCallback(boost::bind(&TibiDaboHeadTrackingAlgNode::follow_targetStartCallback, this, _1)); 
  follow_target_aserver_.registerStopCallback(boost::bind(&TibiDaboHeadTrackingAlgNode::follow_targetStopCallback, this)); 
  follow_target_aserver_.registerIsFinishedCallback(boost::bind(&TibiDaboHeadTrackingAlgNode::follow_targetIsFinishedCallback, this)); 
  follow_target_aserver_.registerHasSucceedCallback(boost::bind(&TibiDaboHeadTrackingAlgNode::follow_targetHasSucceedCallback, this)); 
  follow_target_aserver_.registerGetResultCallback(boost::bind(&TibiDaboHeadTrackingAlgNode::follow_targetGetResultCallback, this, _1)); 
  follow_target_aserver_.registerGetFeedbackCallback(boost::bind(&TibiDaboHeadTrackingAlgNode::follow_targetGetFeedbackCallback, this, _1)); 
  follow_target_aserver_.start();
  
  // [init action clients]
  this->mode = 0; //0=relative, 1=absolute
  this->target_pan=0.0;
  this->target_tilt=0.0;
  this->pan_angle  = 0.0;
  this->tilt_angle = 0.0;
  this->current_state_=IDLE_STATE;
  this->event_server=CEventServer::instance();
  this->feedback_on_event_id="head_tracking_feedback_event";
  this->event_server->create_event(this->feedback_on_event_id);
  // initialize the state machine events
  this->new_req_event_id_="head_tracking_new_request_event";
  this->event_server->create_event(this->new_req_event_id_);
  // initialize the topic message
  this->JointTrajectoryPoint_msg_.positions.resize(3);
  this->JointTrajectoryPoint_msg_.positions[0]=0.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.positions[1]=0.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.positions[2]=0.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.velocities.resize(3);
  this->JointTrajectoryPoint_msg_.velocities[0]=200.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.velocities[1]=200.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.velocities[2]=200.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.accelerations.resize(3);
  this->JointTrajectoryPoint_msg_.accelerations[0]=10.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.accelerations[1]=10.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.accelerations[2]=10.0*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.time_from_start=ros::Duration(0,0);

  // move the head to the stand by position
  this->joint_position_publisher_.publish(this->JointTrajectoryPoint_msg_);
}

TibiDaboHeadTrackingAlgNode::~TibiDaboHeadTrackingAlgNode(void)
{
  this->current_state_=IDLE_STATE;
  // [free dynamic memory]
  if(this->feedback_on_event_id!="")
  {
    this->event_server->delete_event(this->feedback_on_event_id);
    this->feedback_on_event_id="";
  }
  if(this->new_req_event_id_!="")
  { 
    this->event_server->delete_event(this->new_req_event_id_);
    this->new_req_event_id_!="";
  }
}

void TibiDaboHeadTrackingAlgNode::mainNodeThread(void)
{
  float pan=0.0, tilt=0.0;

  // [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]

  // [publish messages]
  switch(this->current_state_)
  {
    case IDLE_STATE:
//      ROS_INFO("IDLE_STATE");
      //once we receive a follow target request
      //jumpt to request move base request state
      if(this->event_server->event_is_set(this->new_req_event_id_))
        current_state_ = FOLLOW_STATE;
      break;

    case FOLLOW_STATE:
      if(this->event_server->event_is_set(this->feedback_on_event_id))
      {
//        ROS_INFO("FOLLOW_STATE");
        // compute head position
        ROS_INFO("target pan,tilt: %f,%f", this->target_pan, this->target_tilt);
        this->alg_.compute_head_position(this->target_pan,this->target_tilt,&pan,&tilt);
        this->JointTrajectoryPoint_msg_.positions[0]=pan;
        this->JointTrajectoryPoint_msg_.positions[1]=tilt;
        this->joint_position_publisher_.publish(this->JointTrajectoryPoint_msg_);
      }
      if(this->event_server->event_is_set(this->new_req_event_id_))
        this->current_state_=FOLLOW_STATE;
      else
        this->current_state_=IDLE_STATE;
      break;
    case PREEMPT_STATE:
//      ROS_WARN("PREEMPT_STATE");
      follow_target_aserver_.setPreempted();
      if(this->event_server->event_is_set(this->new_req_event_id_))
        this->event_server->reset_event(this->new_req_event_id_);
      current_state_ = IDLE_STATE;
      break;
  }
}

/*  [subscriber callbacks] */
void TibiDaboHeadTrackingAlgNode::target_joints_callback(const trajectory_msgs::JointTrajectoryPoint::ConstPtr& msg) 
{ 
//  ROS_INFO("TibiDaboHeadTrackingAlgNode::target_pos_callback: New Message Received"); 

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

  //  std::cout << "new target position" << std::endl;
  if(this->mode==0) //relative
  {
    this->target_pan=msg->positions[0]+this->pan_angle;
    this->target_tilt=msg->positions[1]+this->tilt_angle;
  }
  else //absolute
  {
    this->target_pan=msg->positions[0];
    this->target_tilt=msg->positions[1];
  }
  //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  //this->target_pos_mutex_.exit(); 
}
void TibiDaboHeadTrackingAlgNode::joint_feedback_callback(const sensor_msgs::JointState::ConstPtr& msg) 
{ 
  unsigned int i=0;
  //double pan_angle=0.0,tilt_angle=0.0;

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

  for(i=0;i<msg->name.size();i++)
  {
    if(msg->name[i]=="pan")
      this->pan_angle=msg->position[i];
    else if(msg->name[i]=="tilt")
      this->tilt_angle=msg->position[i];
  }
  this->alg_.set_current_head_position(this->pan_angle,this->tilt_angle);
  if(!this->event_server->event_is_set(this->feedback_on_event_id))
    this->event_server->set_event(this->feedback_on_event_id);

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

/*  [service callbacks] */

/*  [action callbacks] */
void TibiDaboHeadTrackingAlgNode::follow_targetStartCallback(const iri_nav_msgs::followTargetGoalConstPtr& goal)
{ 
  alg_.lock(); 
    //check goal 
  if(!this->event_server->event_is_set(this->new_req_event_id_))
    this->event_server->set_event(this->new_req_event_id_);
    //execute goal 
  alg_.unlock(); 
} 

void TibiDaboHeadTrackingAlgNode::follow_targetStopCallback(void) 
{ 
  alg_.lock(); 
  if(this->event_server->event_is_set(this->new_req_event_id_))
    this->event_server->reset_event(this->new_req_event_id_);
    //stop action 
  alg_.unlock(); 
} 

bool TibiDaboHeadTrackingAlgNode::follow_targetIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool TibiDaboHeadTrackingAlgNode::follow_targetHasSucceedCallback(void) 
{ 
  bool ret = true; 

  alg_.lock(); 
    //if goal was accomplished 
    //ret = true 
  alg_.unlock(); 

  return ret; 
} 

void TibiDaboHeadTrackingAlgNode::follow_targetGetResultCallback(iri_nav_msgs::followTargetResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void TibiDaboHeadTrackingAlgNode::follow_targetGetFeedbackCallback(iri_nav_msgs::followTargetFeedbackPtr& feedback) 
{ 
  float pan_error,tilt_error;

  alg_.lock(); 
    //keep track of feedback 
  this->alg_.get_current_error(&pan_error,&tilt_error);  
  feedback->current_dist=sqrt(pan_error*pan_error+tilt_error*tilt_error);
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}

/*  [action requests] */

void TibiDaboHeadTrackingAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->JointTrajectoryPoint_msg_.velocities[0]=config.velocity*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.velocities[1]=config.velocity*3.14159/180.0;
  this->JointTrajectoryPoint_msg_.velocities[2]=config.velocity*3.14159/180.0;
  this->mode = config.mode;
  this->alg_.unlock();
}

void TibiDaboHeadTrackingAlgNode::addNodeDiagnostics(void)
{
}

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