uncalibvs_sim_alg_node.cpp

Go to the documentation of this file.

#include "uncalibvs_sim_alg_node.h"

using namespace KDL;

Uncalibvs_simAlgNode::Uncalibvs_simAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<Uncalibvs_simAlgorithm>(),
  activate_(false),
  traditional_(false),
  random_points_(false),
  quadrotor_(true),
  output_files_(false),
  arm_unina_(false),
  arm_catec_(false),
  lambda_(0.005),
  dist_to_tag_(1.0),
  path_("/home/asantamaria/Desktop/joint_velocities.txt"),
  lambda_robot_(Eigen::MatrixXf::Zero(9,1)),
  Rquad_inertial_(Eigen::MatrixXf::Zero(3,3)){

  this->init_=true; 
  this->ini_vars_=true;
  //mutex=true;
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->joints_pub_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("arm_joints_states", 10);
  this->output_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 10);

  // [init subscribers]
  this->model_state_subscriber_ = this->public_node_handle_.subscribe("model_state", 10, &Uncalibvs_simAlgNode::model_state_callback, this);
  this->input_tag_subscriber_ = this->public_node_handle_.subscribe("input_tag", 10, &Uncalibvs_simAlgNode::input_tag_callback, this);
 
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]


  if (this->quadrotor_){

    ros::NodeHandle nh;
 
    std::string tip_name, root_name;
    std::string xml_string;
    KDL::Tree tree;
    
    std::string urdf_xml,full_urdf_xml;
    nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
    nh.searchParam(urdf_xml,full_urdf_xml);
       
    ROS_DEBUG("Reading xml file from parameter server\n");
    if (!nh.getParam(full_urdf_xml, xml_string)){
      ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
    }
    if (!nh.getParam("root_name", root_name)){
      ROS_ERROR("No root name found on parameter server");
    }
    if (!nh.getParam("tip_name", tip_name)){
      ROS_ERROR("No tip name found on parameter server");
    }
    if (!kdl_parser::treeFromString(xml_string, tree)){
      ROS_ERROR("Could not initialize tree object");
    }
    if (!tree.getChain(root_name, tip_name, this->kdl_chain_)){
      ROS_ERROR("Could not initialize kdl_chain object");
    }
  }
}

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

void Uncalibvs_simAlgNode::mainNodeThread(void)
{
  
  // [fill msg structures]
  //this->JointState_msg_.data = my_var;
  //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]
}

/*  [subscriber callbacks] */
void Uncalibvs_simAlgNode::model_state_callback(const gazebo_msgs::ModelStates::ConstPtr& msg) 
{ 
  //ROS_INFO("Uncalibvs_simAlgNode::model_state_callback: New Message Received"); 
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->model_state_mutex_.enter(); 
  //std::cout << msg->data << std::endl; 
  
  tf::Transform tf_pose;
  double ya,pi,ro;
  
  for (int s=0; s<int(msg->name.size()); ++s){
     if (msg->name[s]=="quadrotor"){
     this->twist_quad_=msg->twist[s];
     
     //Get quadrotor pose to extract body to inertial rotation
     tf::poseMsgToTF(msg->pose[s], tf_pose);
     tf_pose.getBasis().getRPY(ro,pi,ya);
     // euler convention zyx  
     this->Rquad_inertial_ = Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitZ()) \
         * Eigen::AngleAxisf(pi, Eigen::Vector3f::UnitY()) \
         * Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitX());
     }
   }

  //this->alg_.lock();   
  this->model_state_mutex_.exit(); 
}
 
void Uncalibvs_simAlgNode::input_tag_callback(const ar_pose::ARMarkers::ConstPtr& msg) 
{ 

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock();   
  if (ini_vars_)
  {
    //Quadrotor
    if (!this->arm_unina_ && !this->arm_catec_){
      this->num_joints_=0;
      this->q_=Eigen::MatrixXf::Zero(6+this->num_joints_,1);
      this->q_ << 0, 0, 2.5, 0, 0, 0;    
    }
    //UNINA arm
    else if (this->arm_unina_ && !this->arm_catec_){
      this->num_joints_=5;
      this->JointState_msg_.header.stamp=ros::Time::now();
      this->JointState_msg_.header.frame_id="";
      this->JointState_msg_.name.resize(this->num_joints_);
      this->JointState_msg_.position.resize(this->num_joints_);
      this->JointState_msg_.velocity.resize(this->num_joints_);
      this->JointState_msg_.effort.resize(this->num_joints_);
      this->JointState_msg_.name[0]="quadrotor::arm_g1";
      this->JointState_msg_.name[1]="quadrotor::arm_B1";
      this->JointState_msg_.name[2]="quadrotor::arm_B2";
      this->JointState_msg_.name[3]="quadrotor::arm_B3";
      this->JointState_msg_.name[4]="quadrotor::arm_B4";
      this->q_=Eigen::MatrixXf::Zero(6+this->num_joints_,1);
      //this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 0.2, 1.7, 2, 0;
      //this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 0, 0, 0, 0;
      this->q_ << 0, 0, 2.5, 0, 0, 0, 0, -0.5, 1.7, 1, 0;
    }
    //CATEC arm
    else if (this->arm_catec_ && !this->arm_unina_){
      this->num_joints_=6;
      this->JointState_msg_.header.stamp=ros::Time::now();
      this->JointState_msg_.header.frame_id="";
      this->JointState_msg_.name.resize(this->num_joints_);
      this->JointState_msg_.position.resize(this->num_joints_);
      this->JointState_msg_.velocity.resize(this->num_joints_);
      this->JointState_msg_.effort.resize(this->num_joints_);
      this->JointState_msg_.name[0]="quadrotor::arm_shoulder_0";
      this->JointState_msg_.name[1]="quadrotor::arm_shoulder_1";
      this->JointState_msg_.name[2]="quadrotor::arm_elbow_0";
      this->JointState_msg_.name[3]="quadrotor::arm_elbow_1";
      this->JointState_msg_.name[4]="quadrotor::arm_wirst_0";
      this->JointState_msg_.name[5]="quadrotor::arm_wirst_1";
      this->q_=Eigen::MatrixXf::Zero(6+this->num_joints_,1);
  //     this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 3.14159/2, 0, 0, 0, 0;
      this->q_ << 0, 0, 2.5, 0, 0, 0, 0, 2, -2, 0, 1.5707, 0;
    }
    else if (this->arm_unina_ && this->arm_catec_){
      ROS_ERROR("Two arms detected");
    }
    ini_vars_=false;
  }
    
  this->Twist_msg_.linear.x=0;
  this->Twist_msg_.linear.y=0;
  this->Twist_msg_.linear.z=0;
  this->Twist_msg_.angular.x=0;
  this->Twist_msg_.angular.y=0;
  this->Twist_msg_.angular.z=0;
  
  if (msg->markers.empty()){
    
    for (int s=0; s<this->num_joints_; ++s){
      this->JointState_msg_.position[s]=this->q_(6+s,0);
      this->JointState_msg_.velocity[s]=0.0;
      this->JointState_msg_.effort[s]=1000;
    }    
    this->joints_pub_publisher_.publish(this->JointState_msg_); 
    this->init_=true;  
    this->ini_vars_=true;
    ROS_INFO("No tag detected");
  }

  else if (!msg->markers.empty()){
    //ROS_INFO("Uncalibvs_simAlgNode::input_callback: New Marker Received"); 
 
    this->t_old_=this->t_;
    
    // markers check
    bool markers_ok=true;
    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))){
      markers_ok=true;
      //ROS_ERROR("Markers OK");
    }
    else {
      markers_ok=false;
      //ROS_ERROR("Markers KO");
    }
        
    if (markers_ok) {
      if (1<msg->markers.size()) {
        //ROS_WARN("Two markers zone");
        if (msg->markers[0].pose.pose.position.z<(this->dist_to_tag_+0.24) || (this->fixed_to_id_==1)){
          this->pose_ = msg->markers[1].pose.pose;
          this->t_ = msg->markers[1].header.stamp;
          this->fixed_to_id_=1;
          //ROS_INFO("BAR at z distance: %f \n", msg->markers[1].pose.pose.position.z);
        }
        else if (msg->markers[0].pose.pose.position.z>(this->dist_to_tag_+0.24)){
          this->pose_ = msg->markers[0].pose.pose;
          this->t_ = msg->markers[0].header.stamp;
          this->fixed_to_id_=0;
          //ROS_INFO("BASE at z distance: %f \n", msg->markers[0].pose.pose.position.z);          
        }
      }
      else {
        this->pose_ = msg->markers.back().pose.pose;
        this->t_ = msg->markers.back().header.stamp;
        this->fixed_to_id_=msg->markers.back().id;
      } 
    }    
    //reset time that will reset the algorithm integration
    if (this->init_){
      this->t_old_=this->t_;
      this->init_=false;
    }
    
    ros::Duration Ts;
    Ts=this->t_-this->t_old_;
    
    double secs;
    secs=Ts.toSec();  

    if (this->activate_ && markers_ok){
      //this->input_mutex_.enter();
   
      // Convert pose from marker////////////////////////////////////////////////
      // Convert quaternion to RPY.
      tf::Transform tf_pose;
      Eigen::Matrix3f R;
      double x,y,z,yaw,pitch,roll;
    
      tf::poseMsgToTF(this->pose_, tf_pose);
      tf_pose.getBasis().getRPY(roll,pitch,yaw);
  
      // euler convention zyx  
      R = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitZ()) \
          * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) \
          * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitX());

      x = this->pose_.position.x;
      y = this->pose_.position.y;
      z = this->pose_.position.z;

      Eigen::Matrix4f cTo(4,4);
      cTo=Eigen::Matrix4f::Zero();
      if (!std::isnan(R(0,0)))  cTo.block(0,0,3,3)=R;
     
      cTo.block(0,3,3,1) << x, y, z;
      cTo.row(3) << 0, 0, 0, 1;
    
      // Extract the Non DOF velocities ///////////////////////////////////    
      Eigen::MatrixXf V2(2,1);
      V2=Eigen::MatrixXf::Zero(2,1);
      if (this->quadrotor_ && !std::isnan(this->twist_quad_.angular.x) && !std::isnan(this->twist_quad_.angular.y)){
              V2(0,0)=this->twist_quad_.angular.x;
        V2(1,0)=this->twist_quad_.angular.y; 
        //V2=Eigen::MatrixXf::Zero(2,1);
      }
      Eigen::MatrixXf Vel_quad(6,1);
      Vel_quad=Eigen::MatrixXf::Zero(6,1);
      
      
      double dist_z;
      dist_z=this->dist_to_tag_;
      double dist;
      if (this->fixed_to_id_==0){
        dist=msg->markers[0].pose.pose.position.z-0.24;
        // ROS_INFO("Anchored to BASE at z distance: %f",dist);
        //dist_z=this->dist_to_tag_;
      }
      else if (this->fixed_to_id_==1){
        dist=msg->markers[1].pose.pose.position.z-0.24;
        // ROS_INFO("Anchored to BAR at z distance: %f",dist);
        //dist_z=this->dist_to_tag_/2;
      }

      //ROS_INFO("marker 0: %f",msg->markers[0].pose.pose.position.z);
      //ROS_INFO("marker 1: %f",msg->markers[1].pose.pose.position.z);
      //ROS_ERROR_STREAM(cTo);
      //ROS_ERROR_STREAM(secs);
      //ROS_ERROR_STREAM(V2);
      //ROS_ERROR_STREAM(Rquad_inertial_);

      uncalib_vs_sim_alg.uncalib_vs_sim(this->traditional_,this->random_points_,this->quadrotor_,this->arm_unina_,this->arm_catec_,this->output_files_,this->path_,this->lambda_,this->lambda_robot_,dist_z,cTo,secs,V2,this->kdl_chain_,this->q_,this->Rquad_inertial_,Vel_quad);
      
      this->Twist_msg_.linear.x=Vel_quad(0);
      this->Twist_msg_.linear.y=Vel_quad(1);
      this->Twist_msg_.linear.z=Vel_quad(2);
      this->Twist_msg_.angular.x=Vel_quad(3);  
      this->Twist_msg_.angular.y=Vel_quad(4);  
      this->Twist_msg_.angular.z=Vel_quad(5); 

    }
    else {
     ROS_INFO("Not active"); 
    }
  }

  for (int s=0; s<this->num_joints_; ++s){
    if (this->arm_unina_){
    this->q_(6,0)=-this->q_(6,0);
    // this->q_(8,0)=this->q_(8,0)+3.1415;
    // this->q_(9,0)=this->q_(9,0)+3.1415;

    }
    this->JointState_msg_.position[s]=this->q_(6+s,0);
    this->JointState_msg_.velocity[s]=0.0;
    this->JointState_msg_.effort[s]=100;
  }

  // adding an offset to the thrust to compensate the arm weight
  if (this->arm_unina_){
    this->Twist_msg_.linear.z= this->Twist_msg_.linear.z + 0.15;
  }
  else if (this->arm_catec_){
    this->Twist_msg_.linear.z= this->Twist_msg_.linear.z + 0;
  }
  else if (!this->arm_unina_ && !this->arm_catec_){
    this->Twist_msg_.linear.z= this->Twist_msg_.linear.z;
  }
  
  // [publish messages] 
  this->output_publisher_.publish(this->Twist_msg_);
  this->joints_pub_publisher_.publish(this->JointState_msg_); 

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

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */


void Uncalibvs_simAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->activate_=config.activate;
  this->traditional_=config.traditional;
  this->lambda_=config.lambda;
  double lambda_quadrotor=config.lambda_quadrotor;
  double lambda_arm=config.lambda_arm;
  this->dist_to_tag_=config.dist_to_tag;
  this->random_points_=config.random_points;
  this->quadrotor_=config.quadrotor;
  this->output_files_=config.output_files;
  this->path_=config.path;
  this->arm_unina_=config.arm_unina;
  this->arm_catec_=config.arm_catec;
  this->alg_.unlock();

  for (int i = 0; i < 4; ++i)
  {
    this->lambda_robot_(i,0)=lambda_quadrotor;
  }
  for (int i = 0; i < this->num_joints_; ++i)
  {
    this->lambda_robot_(4+i,0)=lambda_arm;
  }
}

void Uncalibvs_simAlgNode::addNodeDiagnostics(void)
{
}

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