artag_pose_based_vs_alg_node.cpp

Go to the documentation of this file.

#include "artag_pose_based_vs_alg_node.h"

ArtagPoseBasedVsAlgNode::ArtagPoseBasedVsAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ArtagPoseBasedVsAlgorithm>()
{
  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  //Initialize vectors
  this->current_pose_ = Eigen::MatrixXd::Zero(6,1);
  this->desired_pose_ = Eigen::MatrixXd::Zero(6,1);
  this->kp_ = Eigen::MatrixXd::Zero(6,1);
  this->kd_ = Eigen::MatrixXd::Zero(6,1);
  this->ki_ = Eigen::MatrixXd::Zero(6,1);
  this->cam_vel_ = Eigen::MatrixXd::Zero(6,1);


  // [init publishers]
  this->cmd_vel_cov_publisher_ = this->public_node_handle_.advertise<geometry_msgs::TwistWithCovariance>("cmd_vel_cov", 1);
  this->target_artag_publisher_ = this->public_node_handle_.advertise<ar_pose::ARMarkers>("target_artag", 1);
  
  // [init subscribers]
  this->input_ARtag_subscriber_ = this->public_node_handle_.subscribe("input_ARtag", 1, &ArtagPoseBasedVsAlgNode::input_ARtag_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void ArtagPoseBasedVsAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->Twist_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 ArtagPoseBasedVsAlgNode::input_ARtag_callback(const ar_pose::ARMarkers::ConstPtr& msg) 
{ 
  //ROS_INFO("ArtagPoseBasedVsAlgNode::input_ARtag_callback: New Message Received"); 

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

  this->cam_vel_ = Eigen::MatrixXd::Zero(6,1);

  if (msg->markers.empty()){

    this->target_artag_msg_.markers.resize(0);

    //Covariance matrix set to high values because there is no marker
    this->TwistWC_msg_.covariance[0]  = 
    this->TwistWC_msg_.covariance[7]  = 
    this->TwistWC_msg_.covariance[14] = 
    this->TwistWC_msg_.covariance[21] = 
    this->TwistWC_msg_.covariance[28] = 
    this->TwistWC_msg_.covariance[35] = 99999;
  }
  else if (!msg->markers.empty()){

    this->time_last_ = this->time_;

    // markers check
    if ((!std::isnan(msg->markers[0].pose.pose.orientation.x) && std::isfinite(msg->markers[0].pose.pose.orientation.x)) || (!std::isnan(msg->markers[1].pose.pose.orientation.x) && std::isfinite(msg->markers[1].pose.pose.orientation.x)))
    {
      
      // Get marker info
      get_marker_info(msg);
      // Avoid detection ambiguities with z axis check  
      if (this->current_pose_(2,0) > 0)
      {
        this->alg_.lock(); 

        this->pose_based_vs_alg_.pose_based_vs(this->desired_pose_, this->current_pose_,this->dt_,
                                               this->kp_,this->kd_,this->ki_,
                                               this->i_lim_,this->cam_vel_);
        this->alg_.unlock();
      }

    
      //Covariance matrix set to low values because there is marker, and 
      double cov = double(100-msg->markers.back().confidence)/100;
      this->TwistWC_msg_.covariance[0]  = 
      this->TwistWC_msg_.covariance[7]  = 
      this->TwistWC_msg_.covariance[14] = 
      this->TwistWC_msg_.covariance[21] = 
      this->TwistWC_msg_.covariance[28] = 
      this->TwistWC_msg_.covariance[35] = cov;
    }
    else ROS_ERROR("Marker Reading error");
  }
  //Twist message to publish with the camera velocities
  this->TwistWC_msg_.twist.linear.x=this->cam_vel_(0,0);
  this->TwistWC_msg_.twist.linear.y=this->cam_vel_(1,0);
  this->TwistWC_msg_.twist.linear.z=this->cam_vel_(2,0);
  this->TwistWC_msg_.twist.angular.x=this->cam_vel_(3,0);
  this->TwistWC_msg_.twist.angular.y=this->cam_vel_(4,0);
  this->TwistWC_msg_.twist.angular.z=this->cam_vel_(5,0);

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

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

  this->cmd_vel_cov_publisher_.publish(this->TwistWC_msg_);
  this->target_artag_publisher_.publish(this->target_artag_msg_);

  this->input_ARtag_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void ArtagPoseBasedVsAlgNode::get_marker_info(const ar_pose::ARMarkers::ConstPtr& msg)
{      
  // Get pose, time and ID
  if (1 < msg->markers.size()) {
    if (msg->markers[0].pose.pose.position.z<(this->desired_pose_(2,3)-0.24) || (this->fixed_to_id_==1))
    {
      this->pose_ = msg->markers[1].pose.pose;
      this->time_ = msg->markers[1].header.stamp;
      this->fixed_to_id_=1;
    }
    else if (msg->markers[0].pose.pose.position.z>(this->desired_pose_(2,3)-0.24))
    {
      this->pose_ = msg->markers[0].pose.pose;
      this->time_ = msg->markers[0].header.stamp;
      this->fixed_to_id_=0;
    }
  }
  else 
  {
    this->pose_ = msg->markers.back().pose.pose;
    this->time_ = msg->markers.back().header.stamp;
    this->fixed_to_id_=msg->markers.back().id;
  } 
  // ROS_INFO("Anchored to marker: %d",this->fixed_to_id_);
  
  //reset time that will reset the algorithm integration
  if (this->init_)
  {
    this->time_last_=this->time_;
    this->init_=false;
  }
  this->dt_=(this->time_-this->time_last_).toSec();  

  // Get pose
  tf::Transform tf_pose;
  double roll,pitch,yaw;
  tf::poseMsgToTF(this->pose_, tf_pose);
  tf_pose.getBasis().getRPY(roll,pitch,yaw);

  this->current_pose_(0,0) = this->pose_.position.x;
  this->current_pose_(1,0) = this->pose_.position.y;
  this->current_pose_(2,0) = this->pose_.position.z;
  this->current_pose_(3,0) = roll;
  this->current_pose_(4,0) = pitch;
  this->current_pose_(5,0) = yaw;

  // Fill target artag message to publish it
  this->target_artag_msg_.header = msg->header;
  this->target_artag_msg_.markers.resize(1);
  this->target_artag_msg_.markers[0] = msg->markers[this->fixed_to_id_];
}


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

  //get values form dynamic reconfigure
  //Desired Position
  this->desired_pose_(0,0) = config.desired_x;
  this->desired_pose_(1,0) = config.desired_y;
  this->desired_pose_(2,0) = config.desired_z;
  this->desired_pose_(3,0) = config.desired_roll-3.14159265359; // Substract pi because the camera should be looking at the marker
  this->desired_pose_(4,0) = config.desired_pitch;
  this->desired_pose_(5,0) = config.desired_yaw;

  this->kp_(0,0) = config.kp_x;
  this->kd_(0,0) = config.kd_x;
  this->ki_(0,0) = config.ki_x;
  this->kp_(1,0) = config.kp_y;
  this->kd_(1,0) = config.kd_y;
  this->ki_(1,0) = config.ki_y;
  this->kp_(2,0) = config.kp_z;
  this->kd_(2,0) = config.kd_z;
  this->ki_(2,0) = config.ki_z;
  this->kp_(3,0) = config.kp_roll;
  this->kd_(3,0) = config.kd_roll;
  this->ki_(3,0) = config.ki_roll;
  this->kp_(4,0) = config.kp_pitch;
  this->kd_(4,0) = config.kd_pitch;
  this->ki_(4,0) = config.ki_pitch;
  this->kp_(5,0) = config.kp_yaw;
  this->kd_(5,0) = config.kd_yaw;
  this->ki_(5,0) = config.ki_yaw;
  this->i_lim_ = config.i_lim;

  this->alg_.unlock();
}

void ArtagPoseBasedVsAlgNode::addNodeDiagnostics(void)
{
}

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