uncalibvs_alg_node.cpp

Go to the documentation of this file.

#include "uncalibvs_alg_node.h"

UncalibvsAlgNode::UncalibvsAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<Uncalibvs_Algorithm>(),
  activate_(false),
  traditional_(false),
  random_points_(false),
  quadrotor_(true),
  output_files_(false),
  estop_(false),
  serial_(false),
  calibrated_ctrl_(true),
  lambda_(0.01),
  final_z_(1.0),
  ENUyaw_(0.0),
  ENUyaw_fix_(0.0),
  height_fix_(0.0),
  tag_thrust_kp_(0.05),
  tag_thrust_kd_(0.05),
  tag_roll_kp_(0.1),
  tag_roll_kd_(0.15),
  tag_pitch_kp_(0.1),
  tag_pitch_kd_(0.15),
  tag_yaw_kp_(0.05),
  tag_yaw_kd_(0.05),  
  thrust_kp_(0.06),
  thrust_kd_(0.005),
  roll_kp_(0.2),
  roll_kd_(0),
  pitch_kp_(0.2),
  pitch_kd_(0),
  yaw_kp_(0.2),
  yaw_kd_(0),  
  sat_min_(0.42745),
  sat_max_(0.53737),
  rollpitch_sat_min_(-0.146),
  rollpitch_sat_max_(0.146),
  yaw_sat_min_(-1),
  yaw_sat_max_(1),
  ctrl_ref_(0.447),
  trimm_y_(0.0),
  trimm_x_(0.0),
  buff_speed_(1),
  path_("/home/asantamaria/Desktop/poses.txt"),
  rollpitch_div_(1000),
  Rquad_inertial_(Eigen::MatrixXf::Zero(3,3))
{
  this->init_=true; 
  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  // [init publishers]
  this->body_twist_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("body_twist", 1);
  this->yaw_control_publisher_ = this->public_node_handle_.advertise<iri_uncalibvs::control>("yaw_control", 1);
  this->pitch_control_publisher_ = this->public_node_handle_.advertise<iri_uncalibvs::control>("pitch_control", 1);
  this->roll_control_publisher_ = this->public_node_handle_.advertise<iri_uncalibvs::control>("roll_control", 1);
  this->thrust_control_publisher_ = this->public_node_handle_.advertise<iri_uncalibvs::control>("thrust_control", 1);
  this->state_publisher_ = this->public_node_handle_.advertise<mav_msgs::Status>("state", 10);
  this->cmd_yaw_publisher_ = this->public_node_handle_.advertise<std_msgs::Float64>("cmd_yaw", 10);
  this->cmd_pitch_publisher_ = this->public_node_handle_.advertise<std_msgs::Float64>("cmd_pitch", 10);
  this->cmd_roll_publisher_ = this->public_node_handle_.advertise<std_msgs::Float64>("cmd_roll", 10);
  this->cmd_thrust_publisher_ = this->public_node_handle_.advertise<std_msgs::Float64>("cmd_thrust", 10);
  this->ESTOP_publisher_ = this->public_node_handle_.advertise<std_msgs::Bool>("ESTOP", 1);

  // [init subscribers]
  this->ll_status_subscriber_ = this->public_node_handle_.subscribe("ll_status", 10, &UncalibvsAlgNode::ll_status_callback, this);
  this->height_subscriber_ = this->public_node_handle_.subscribe("height", 10, &UncalibvsAlgNode::height_callback, this);
  this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 10, &UncalibvsAlgNode::imu_callback, this);
  this->model_state_subscriber_ = this->public_node_handle_.subscribe("model_state", 10, &UncalibvsAlgNode::model_state_callback, this);
  this->input_tag_subscriber_ = this->public_node_handle_.subscribe("input_tag", 10, &UncalibvsAlgNode::input_tag_callback, this);
 
  // Get initial time
  this->time_last_ = ros::Time::now();
  
  //Init Thrust PID
  this->tag_pid_thrust_.initPid(this->tag_thrust_kp_, 0, this->tag_thrust_kd_, 0, 0);
  this->pid_thrust_.initPid(this->thrust_kp_, 0, this->thrust_kd_, 0, 0);
  
  //Init Roll PID
  this->tag_pid_roll_.initPid(this->tag_roll_kp_, 0, this->tag_roll_kd_, 0, 0);  
  this->pid_roll_.initPid(this->roll_kp_, 0, this->roll_kd_, 0, 0);
  
  //Init Pitch PID
  this->tag_pid_pitch_.initPid(this->tag_pitch_kp_, 0, this->tag_pitch_kd_, 0, 0);  
  this->pid_pitch_.initPid(this->pitch_kp_, 0, this->pitch_kd_, 0, 0);
  
  //Init Yaw PID
  this->tag_pid_yaw_.initPid(this->tag_yaw_kp_, 0, this->tag_yaw_kd_, 0, 0);  
  this->pid_yaw_.initPid(this->yaw_kp_, 0, this->yaw_kd_, 0, 0);

  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void UncalibvsAlgNode::mainNodeThread(void)
{
  
  // [fill msg structures]
  //this->body_twist_msg_.data = my_var;
  //this->yaw_control_msg_.data = my_var;
  //this->pitch_control_msg_.data = my_var;
  //this->roll_control_msg_.data = my_var;
  //this->thrust_control_msg_.data = my_var;
  //this->Status_msg_.data = my_var;
  //this->cmd_yaw_msg_.data = my_var;
  //this->cmd_pitch_msg_.data = my_var;
  //this->cmd_roll_msg_.data = my_var;
  //this->cmd_thrust_msg_.data = my_var;
  //this->Bool_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]
  
  
  //Emergency STOP enable
  this->Bool_msg_.data=this->estop_;
  if (this->estop_) {
    ROS_ERROR_STREAM("EMERGENCY_STOP ENABLED");
    this->ESTOP_publisher_.publish(this->Bool_msg_);
  }
 
  // [publish messages]
  this->body_twist_publisher_.publish(this->body_twist_msg_);
  this->yaw_control_publisher_.publish(this->yaw_control_msg_);
  this->pitch_control_publisher_.publish(this->pitch_control_msg_);
  this->roll_control_publisher_.publish(this->roll_control_msg_);
  this->thrust_control_publisher_.publish(this->thrust_control_msg_);
  this->state_publisher_.publish(this->Status_msg_);
  this->cmd_yaw_publisher_.publish(this->cmd_yaw_msg_);
  this->cmd_pitch_publisher_.publish(this->cmd_pitch_msg_);
  this->cmd_roll_publisher_.publish(this->cmd_roll_msg_);
  this->cmd_thrust_publisher_.publish(this->cmd_thrust_msg_);
}

/*  [subscriber callbacks] */
void UncalibvsAlgNode::ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg) 
{ 
  //ROS_INFO("UncalibvsAlgNode::ll_status_callback: New Message Received"); 

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

  //std::cout << msg->data << std::endl; 
  int ll_status;
  ll_status=(int)msg->flightMode;
  
  std::bitset<32> flight_mode;
  flight_mode.reset();
  
  
  int *y,x;
  y = reinterpret_cast<int*>(&ll_status);
  x=*y;
  for(short i=0;i<32;++i) {
    flight_mode[i] = x%2;
    x /= 2;
  }

  
  if ((ll_status|0b1111111101111111)!=0b1111111111111111)   ROS_DEBUG("NO Emergency");
  if ((ll_status|0b1111111111111101)!=0b1111111111111111)   ROS_DEBUG("NO Height Control");
  if ((ll_status|0b1111111111111011)!=0b1111111111111111)   ROS_DEBUG("NO GPS Mode");
  if ((ll_status|0b1111111111011111)!=0b1111111111111111) {
    ROS_DEBUG("NO Serial Enable");
    this->serial_=false;
    this->height_fix_=0;
  }
  else {
    this->serial_=true;
  }
  
  if ((ll_status|0b1111111110111111)!=0b1111111111111111)   ROS_DEBUG("NO Serial Active");

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->ll_status_mutex_.exit(); 
}
void UncalibvsAlgNode::height_callback(const mav_msgs::Height::ConstPtr& msg) 
{ 
  //ROS_INFO("UncalibvsAlgNode::height_callback: New Message Received"); 

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

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

  this->height_=msg->height;
  
  // Initialize the height to fix the quadrotor if no tag is detected
  if(this->height_fix_==0 && this->serial_) this->height_fix_=this->height_;
  
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->height_mutex_.exit(); 
}
void UncalibvsAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg) 
{ 
  //ROS_INFO("UncalibvsAlgNode::imu_callback: New Message Received"); 

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

   //std::cout << msg->data << std::endl; 
  this->twist_quad_.linear.x=0.0;
  this->twist_quad_.linear.y=0.0;
  this->twist_quad_.linear.z=0.0;
  this->twist_quad_.angular.x=-msg->angular_velocity.x;
  this->twist_quad_.angular.y=msg->angular_velocity.y;
  this->twist_quad_.angular.z=msg->angular_velocity.z;

  //Get quadrotor pose to extract body to inertial rotation
  geometry_msgs::Pose qpose;
  qpose.position.x=0.0;
  qpose.position.y=0.0;
  qpose.position.z=0.0;
  qpose.orientation=msg->orientation;
  
  tf::Transform tf_pose;
  double ya,pi,ro;
  tf::poseMsgToTF(qpose, tf_pose);
  tf_pose.getBasis().getRPY(ro,pi,ya);


  //   ROS_INFO("comp roll: %2.4f",ro);
  //   ROS_INFO("comp pitch: %2.4f",pi);
  //   ROS_INFO("comp yaw: %2.4f",ya); 
  
  //rotation between body frame and commands frame. Vx, Vy and Vz are commanded in the command frame, angular velocities in the Body frame,
  //thus the linear velocities calculated to the body frame should be rotated to the command frame.
  
  //Roll and pitch lectures readed from compass
  Eigen::Matrix3f Rwb,Rfix,Rwfb,Rfbi;
  // euler convention zyx 
  // Rotation from world to body (sensors) frame
  Rwb = Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitX()) \
      * Eigen::AngleAxisf(pi, Eigen::Vector3f::UnitY()) \
      * Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitZ());
  
  //ROS_WARN_STREAM(Rwb);    
  //Rotation from Body (sensors) to quadrotor front body attahed frame.
  //Yaw fixed to 3pi/4 (relation between the compass readings and the front body frame)
  Rfix=Eigen::MatrixXf::Zero(3,3);
  double zang=0.75*3.1416;
  //   Rfix(0,0)=cos(zang);
  //   Rfix(0,1)=sin(zang);
  //   Rfix(1,0)=-sin(zang);
  //   Rfix(1,1)=cos(zang);
  //   Rfix(2,2)=1.0;
  Rfix = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) \
       * Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) \
       * Eigen::AngleAxisf(zang, Eigen::Vector3f::UnitZ());
  
  //ROS_INFO_STREAM(Rfix);
  //Rotation obtained using sensor reading and fixed yaw relation (from inertial to front body)
  Rwfb=Rwb*Rfix;
  //ROS_WARN_STREAM(Rwfb);
  // Get world to front body angles
  //print angles of pitch and roll inertial
  Eigen::MatrixXf wfbangl(3,1);
  wfbangl=Rwfb.eulerAngles(0,1,2);
  
  // Front body inertial
  Rfbi = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) \
         * Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) \
         * Eigen::AngleAxisf(wfbangl(2), Eigen::Vector3f::UnitZ());
  
  //ROS_INFO_STREAM(Rfbi); 
  // Front body to inertial
  Rquad_inertial_=Rfbi.transpose()*Rwfb;
  
  //ROS_ERROR_STREAM(Rquad_inertial_);
  
  
  this->ENUyaw_=wfbangl(2);
    // Initialize the height to fix the quadrotor if no tag is detected
  if(this->ENUyaw_fix_==0 && this->serial_) this->ENUyaw_fix_=this->ENUyaw_;
  
  
  //   print angles of pitch and roll inertial
  //   Eigen::MatrixXf angl(3,1);
  //   angl=Rquad_inertial_.eulerAngles(0,1,2);
    
  //   ROS_INFO("roll: %2.4f",angl(0));
  //   ROS_INFO("pitch: %2.4f",angl(1));
  //   ROS_INFO("yaw: %2.4f",angl(2));
    
    
  //   double alfa;
  //   Eigen::MatrixXf Norm(3,1),zbody(3,1);
  // 
  //   zbody=Rwfb.col(2);
  // 
  //   Norm(0)=zbody(1);
  //   Norm(1)=-zbody(0);
  //   Norm(2)=0.0;
  //   
  //   Norm=Norm/(sqrt((Norm(0)*Norm(0))+(Norm(1)*Norm(1))+(Norm(2)*Norm(2))));
  // 
  //   alfa=acos(zbody(2));
  // 
  //   Rquad_inertial_(0,0)=cos(alfa)+(Norm(0)*Norm(0)*(1-cos(alfa)));
  //   Rquad_inertial_(0,1)=(Norm(0)*Norm(1)*(1-cos(alfa)))-(Norm(2)*sin(alfa));
  //   Rquad_inertial_(0,2)=(Norm(0)*Norm(2)*(1-cos(alfa)))+(Norm(1)*sin(alfa));
  //   Rquad_inertial_(1,0)=(Norm(0)*Norm(1)*(1-cos(alfa)))+(Norm(2)*sin(alfa));
  //   Rquad_inertial_(1,1)=cos(alfa)+(Norm(1)*Norm(1)*(1-cos(alfa)));
  //   Rquad_inertial_(1,2)=(Norm(1)*Norm(2)*(1-cos(alfa)))-(Norm(0)*sin(alfa));
  //   Rquad_inertial_(2,0)=(Norm(0)*Norm(2)*(1-cos(alfa)))-(Norm(1)*sin(alfa));
  //   Rquad_inertial_(2,1)=(Norm(1)*Norm(2)*(1-cos(alfa)))+(Norm(0)*sin(alfa));
  //   Rquad_inertial_(2,2)=cos(alfa)+(Norm(2)*Norm(2)*(1-cos(alfa)));


  // Convert quaternion to RPY.
  //tf::Quaternion qt;
  //tf::quaternionMsgToTF(msg->orientation, qt);
  //tf::Matrix3x3(qt).getRPY(this->ENUroll_,this->ENUpitch_,this->ENUyaw_);

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

void UncalibvsAlgNode::model_state_callback(const asctec_msgs::IMUCalcData::ConstPtr& msg) 
{ 
  //ROS_INFO("UncalibvsAlgNode::model_state_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->model_state_mutex_.enter(); 
  
  int size=this->buff_speed_;
  std::vector<double> buff_x(size),buff_y(size);
  buff_x.reserve(size);
  buff_y.reserve(size);
  double m_x,m_y;
  int index;
  m_x=0.0;
  m_y=0.0;
  if (index==size) index=0;
  buff_x[index]=msg->speed_x;
  buff_y[index]=msg->speed_y;
  index=index+1; 
  for(int i=0; i<(size-1); ++i){
    m_x += buff_x[i];
    m_y += buff_y[i];
  }
  
  Eigen::MatrixXf body_speed(3,1),inertial_speed(3,1);
  
  //   body_speed(0,0)=m_x/size;
  //   body_speed(1,0)=m_y/size;
  body_speed(0,0)=msg->speed_x;
  body_speed(1,0)=msg->speed_y;
  body_speed(2,0)=msg->speed_z;
  
  Eigen::Matrix3f Rfix;
  // Convert from sensors frame to front frame
  Rfix=Eigen::MatrixXf::Zero(3,3);
  double zang=0.75*3.1416;
  Rfix = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) \
       * Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) \
       * Eigen::AngleAxisf(zang, Eigen::Vector3f::UnitZ());
       
  // convert from sensors frame velocities to front inertial frame velocities
  //inertial_speed=Rquad_inertial_*Rfix*body_speed;
  inertial_speed=Rfix*body_speed;          
  /*  ROS_INFO("spedd_x: %2.4f",this->twist_quad_asctec_.linear.x);
  ROS_INFO("spedd_y: %2.4f",this->twist_quad_asctec_.linear.y); */ 
  
  this->twist_quad_asctec_.linear.x=inertial_speed(0,0);
  this->twist_quad_asctec_.linear.y=inertial_speed(1,0);
  this->twist_quad_asctec_.linear.z=inertial_speed(2,0);
  this->twist_quad_asctec_.angular.x=msg->angvel_roll;
  this->twist_quad_asctec_.angular.y=msg->angvel_nick;
  this->twist_quad_asctec_.angular.z=msg->angvel_yaw;
  
  this->body_twist_msg_=this->twist_quad_asctec_;
  
  
  //   ROS_INFO("spedd_x: %2.4f",this->twist_quad_asctec_.linear.x);
  //   ROS_INFO("spedd_y: %2.4f",this->twist_quad_asctec_.linear.y);
  //   ROS_INFO("spedd_z: %2.4f",this->twist_quad_asctec_.linear.z);
  
  

  //scale angular velocities 64.8=1 degree/s --> [-100..100]  
  //if (twist_quad_.angular.x!=0) twist_quad_.angular.x=(twist_quad_.angular.x/116.64)+100;
  //if (twist_quad_.angular.y!=0) twist_quad_.angular.y=(twist_quad_.angular.y/116.64)+100;      
  //if (twist_quad_.angular.z!=0) twist_quad_.angular.z=(twist_quad_.angular.z/116.64)+100;    

  //this->alg_.lock();   
  this->model_state_mutex_.exit(); 
}

void UncalibvsAlgNode::input_tag_callback(const ar_pose::ARMarkers::ConstPtr& msg) 
{ 

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

  this->time_ = ros::Time::now();

  double x,y,z,yaw,pitch,roll,tag_cmd_pid_roll,tag_cmd_pid_pitch,tag_cmd_pid_yaw,tag_cmd_pid_thrust,cmd_pid_roll,cmd_pid_pitch,cmd_pid_yaw,cmd_pid_thrust;
  x=0;
  y=0;
  z=0;
  roll=0;
  pitch=0;
  yaw=0;
  tag_cmd_pid_roll=0;
  tag_cmd_pid_pitch=0;
  tag_cmd_pid_yaw=0;
  tag_cmd_pid_thrust=0;
  cmd_pid_roll=0;
  cmd_pid_pitch=0;
  cmd_pid_yaw=0;
  cmd_pid_thrust=0;
  
  Eigen::Matrix3f R;
  Eigen::MatrixXf Vel_quad(6,1);
  Vel_quad=Eigen::MatrixXf::Zero(6,1);
  this->error_=Eigen::MatrixXf::Zero(6,1);
  this->tag_error_=Eigen::MatrixXf::Zero(6,1);
 
  if (msg->markers.empty()){
     this->init_=true;  
    
     // thrust control /////////////////////////////////////////////////////////////////////////////////////////////////
     this->error_(2)=this->height_ - this->height_fix_;
     
     // roll control ////////////////////////////////////////////////////////////////////////////////////////////////
     this->error_(1)= -(this->twist_quad_asctec_.linear.y/this->rollpitch_div_) - 0.0;

     // pitch control //////////////////////////////////////////////////////////////////////////////////////////////////
     this->error_(0)=this->twist_quad_asctec_.linear.x/this->rollpitch_div_- 0.0;
     
     // yaw control //////////////////////////////////////////////////////////////////////////////////////////////////
     this->error_(5)=this->ENUyaw_ - this->ENUyaw_fix_;
     
     this->error_(1)=0.0;
     this->error_(0)=0.0;
     this->error_(5)=0.0;
     this->tag_error_=this->error_;
     this->thrust_control_msg_.marker=false;
     this->roll_control_msg_.marker=false;
     this->pitch_control_msg_.marker=false;
     this->yaw_control_msg_.marker=false;
     
  }
  
  else if (!msg->markers.empty()){
    
    geometry_msgs::Pose pose;
    pose = msg->markers.back().pose.pose;
    
    ros::Time mt,mt_old;
    mt_old=mt;
    //Actual reception time
    mt = msg->markers.back().header.stamp;

    //reset time that will reset the algorithm integration
    if (this->init_){
      mt_old=mt;
      this->init_=false;
    }
     
    ros::Duration Ts;  
    Ts=mt-mt_old;
    
    double secs;
    secs=Ts.toSec();  
    
    if (this->activate_){
      //this->input_mutex_.enter(); 
    
      // Convert pose from marker////////////////////////////////////////////////
      // Convert quaternion to RPY.
      tf::Transform tf_pose;
    
      tf::poseMsgToTF(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 = pose.position.x;
      y = pose.position.y;
      z = pose.position.z;

      Eigen::Matrix4f cTo(4,4);
    
      cTo.block(0,0,3,3)=R;  
      cTo.block(0,3,3,1) << x, y, z;
      cTo.row(3) << 0.0, 0.0, 0.0, 1.0;
    
      // Extract the Non DOF velocities ///////////////////////////////////    
      Eigen::MatrixXf V2(2,1);
      V2=Eigen::MatrixXf::Zero(2,1);
      if (this->quadrotor_){
        V2(0,0)=this->twist_quad_.angular.x;
        V2(1,0)=this->twist_quad_.angular.y; 
        //V2(0,0)=0.0;
        //V2(1,0)=0.0; 
      }
    
      // ARM NON existent in the real robot
      bool arm = false;
      KDL::Chain kdl_chain;
      Eigen::MatrixXf q;
      q=Eigen::MatrixXf::Zero(11,1);
      
  //    if (calibrated_ctrl_) {
        this->tag_error_(0)=0.0-y;
        this->tag_error_(1)=0.0-x;
        this->tag_error_(2)=z-this->final_z_;
        this->tag_error_(5)=0.0-yaw;
  //    }      
  //    else {
        uncalib_vs_alg_.uncalib_vs(this->traditional_,this->random_points_,this->quadrotor_,arm,this->output_files_,this->path_,this->lambda_,this->final_z_,cTo,secs,V2,kdl_chain,q,this->Rquad_inertial_,Vel_quad);
        //roll and pitch sign changed
        this->error_=-Vel_quad/lambda_;         
  //    }
    }
    // Get a as heigh to be fixed, the last one when a tag was found
    if (this->serial_) {
      this->height_fix_=this->height_;
      this->ENUyaw_fix_=this->ENUyaw_;
    }
    
     this->thrust_control_msg_.marker=true;
     this->roll_control_msg_.marker=true;
     this->pitch_control_msg_.marker=true;
     this->yaw_control_msg_.marker=true;

  }
  
  tag_cmd_pid_thrust = this->tag_pid_thrust_.updatePid(this->tag_error_(2), this->time_ - this->time_last_);
  tag_cmd_pid_thrust=tag_cmd_pid_thrust+this->ctrl_ref_;
  if (tag_cmd_pid_thrust>this->sat_max_) tag_cmd_pid_thrust=this->sat_max_;
  if (tag_cmd_pid_thrust<this->sat_min_) tag_cmd_pid_thrust=this->sat_min_; 
  
  cmd_pid_thrust = this->pid_thrust_.updatePid(this->error_(2), this->time_ - this->time_last_);
  cmd_pid_thrust=cmd_pid_thrust+this->ctrl_ref_;
  if (cmd_pid_thrust>this->sat_max_) cmd_pid_thrust=this->sat_max_;
  if (cmd_pid_thrust<this->sat_min_) cmd_pid_thrust=this->sat_min_;

  this->thrust_control_msg_.name="Thrust";
  this->thrust_control_msg_.tag_error=this->tag_error_(2);
  this->thrust_control_msg_.tag_satured=tag_cmd_pid_thrust;
  this->thrust_control_msg_.uncal_error=this->error_(2);
  this->thrust_control_msg_.uncal_satured=cmd_pid_thrust;  
  
  if (calibrated_ctrl_) this->cmd_thrust_msg_.data=tag_cmd_pid_thrust;
  else this->cmd_thrust_msg_.data=cmd_pid_thrust;
  
  tag_cmd_pid_roll = this->tag_pid_roll_.updatePid(this->tag_error_(1), this->time_ - this->time_last_);
  tag_cmd_pid_roll=tag_cmd_pid_roll-this->trimm_y_;
  if (tag_cmd_pid_roll>this->rollpitch_sat_max_) tag_cmd_pid_roll=this->rollpitch_sat_max_;
  if (tag_cmd_pid_roll<this->rollpitch_sat_min_) tag_cmd_pid_roll=this->rollpitch_sat_min_;  
    
  cmd_pid_roll = this->pid_roll_.updatePid(this->error_(1), this->time_ - this->time_last_);
  cmd_pid_roll=cmd_pid_roll-this->trimm_y_;
  if (cmd_pid_roll>this->rollpitch_sat_max_) cmd_pid_roll=this->rollpitch_sat_max_;
  if (cmd_pid_roll<this->rollpitch_sat_min_) cmd_pid_roll=this->rollpitch_sat_min_;

  this->roll_control_msg_.name="Roll";
  this->roll_control_msg_.tag_error=this->tag_error_(1);
  this->roll_control_msg_.tag_satured=tag_cmd_pid_roll;
  this->roll_control_msg_.uncal_error=this->error_(1);
  this->roll_control_msg_.uncal_satured=cmd_pid_roll; 
  
  if (calibrated_ctrl_) this->cmd_roll_msg_.data=tag_cmd_pid_roll;
  else this->cmd_roll_msg_.data=cmd_pid_roll;
  
  tag_cmd_pid_pitch = this->tag_pid_pitch_.updatePid(this->tag_error_(0), this->time_ - this->time_last_);
  tag_cmd_pid_pitch=tag_cmd_pid_pitch-this->trimm_x_;
  if (tag_cmd_pid_pitch>this->rollpitch_sat_max_) tag_cmd_pid_pitch=this->rollpitch_sat_max_;
  if (tag_cmd_pid_pitch<this->rollpitch_sat_min_) tag_cmd_pid_pitch=this->rollpitch_sat_min_; 
  
  cmd_pid_pitch = this->pid_pitch_.updatePid(this->error_(0), this->time_ - this->time_last_);
  cmd_pid_pitch=cmd_pid_pitch-this->trimm_x_;
  if (cmd_pid_pitch>this->rollpitch_sat_max_) cmd_pid_pitch=this->rollpitch_sat_max_;
  if (cmd_pid_pitch<this->rollpitch_sat_min_) cmd_pid_pitch=this->rollpitch_sat_min_;

  this->pitch_control_msg_.name="Pitch";
  this->pitch_control_msg_.tag_error=this->tag_error_(0);
  this->pitch_control_msg_.tag_satured=tag_cmd_pid_pitch;
  this->pitch_control_msg_.uncal_error=this->error_(0);
  this->pitch_control_msg_.uncal_satured=cmd_pid_pitch;
  
  if (calibrated_ctrl_) this->cmd_pitch_msg_.data=tag_cmd_pid_pitch;
  else this->cmd_pitch_msg_.data=cmd_pid_pitch;

  tag_cmd_pid_yaw  = this->tag_pid_yaw_.updatePid(this->tag_error_(5), this->time_ - this->time_last_);
  if (tag_cmd_pid_yaw>yaw_sat_max_) tag_cmd_pid_yaw=yaw_sat_max_;
  if (tag_cmd_pid_yaw<yaw_sat_min_) tag_cmd_pid_yaw=yaw_sat_min_;
  
  cmd_pid_yaw = this->pid_yaw_.updatePid(-(this->error_(5)), this->time_ - this->time_last_);
  if (cmd_pid_yaw>yaw_sat_max_) cmd_pid_yaw=yaw_sat_max_;
  if (cmd_pid_yaw<yaw_sat_min_) cmd_pid_yaw=yaw_sat_min_;

  this->yaw_control_msg_.name="Yaw";
  this->yaw_control_msg_.tag_error=this->tag_error_(5);
  this->yaw_control_msg_.tag_satured=tag_cmd_pid_yaw;
  this->yaw_control_msg_.uncal_error=this->error_(5);
  this->yaw_control_msg_.uncal_satured=cmd_pid_yaw;
  
  if (calibrated_ctrl_) this->cmd_yaw_msg_.data=tag_cmd_pid_yaw; 
  else this->cmd_yaw_msg_.data=cmd_pid_yaw;  
  
  
  ros::Time stamp = ros::Time::now();
  this->Status_msg_.header.stamp=stamp;

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

}

/*  [service callbacks] */

/*  [action callbacks] */


void UncalibvsAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  
  this->activate_=config.activate;
  this->traditional_=config.traditional;
  this->lambda_=config.lambda;
  this->final_z_=config.final_z;
  this->random_points_=config.random_points;
  this->quadrotor_=config.quadrotor;
  this->output_files_=config.output_files;
  this->path_=config.path;
  this->tag_thrust_kp_=config.tag_thrust_kp;
  this->tag_thrust_kd_=config.tag_thrust_kd;
  this->tag_roll_kp_=config.tag_roll_kp;
  this->tag_roll_kd_=config.tag_roll_kd;
  this->tag_pitch_kp_=config.tag_pitch_kp;
  this->tag_pitch_kd_=config.tag_pitch_kd;
  this->tag_yaw_kp_=config.tag_yaw_kp;
  this->tag_yaw_kd_=config.tag_yaw_kd;   
  this->thrust_kp_=config.thrust_kp;
  this->thrust_kd_=config.thrust_kd;
  this->roll_kp_=config.roll_kp;
  this->roll_kd_=config.roll_kd;
  this->pitch_kp_=config.pitch_kp;
  this->pitch_kd_=config.pitch_kd;
  this->yaw_kp_=config.yaw_kp;
  this->yaw_kd_=config.yaw_kd;     
  this->sat_min_=config.thrust_sat_min;
  this->sat_max_=config.thrust_sat_max;
  this->ctrl_ref_=config.thrust_ctrl_ref;
  this->trimm_y_=config.trimm_y;
  this->trimm_x_=config.trimm_x;
  this->rollpitch_sat_min_=config.rollpitch_sat_min;
  this->rollpitch_sat_max_=config.rollpitch_sat_max;
  this->yaw_sat_min_=config.yaw_sat_min;
  this->yaw_sat_max_=config.yaw_sat_max;
  this->rollpitch_div_=config.rollpitch_div;
  this->buff_speed_=config.buff_speed;
  this->estop_=config.EMERGENCY_STOP;
  this->calibrated_ctrl_=config.calibrated_ctrl;
  
  
  //Thrust PID
  this->tag_pid_thrust_.setGains(this->tag_thrust_kp_, 0, this->tag_thrust_kd_, 0, 0);
  this->pid_thrust_.setGains(this->thrust_kp_, 0, this->thrust_kd_, 0, 0);
    
  //Roll PID
  this->tag_pid_roll_.setGains(this->tag_roll_kp_, 0, this->tag_roll_kd_, 0, 0);
  this->pid_roll_.setGains(this->roll_kp_, 0, this->roll_kd_, 0, 0);
  
  //Pitch PID
  this->tag_pid_pitch_.setGains(this->tag_pitch_kp_, 0, this->tag_pitch_kd_, 0, 0);  
  this->pid_pitch_.setGains(this->pitch_kp_, 0, this->pitch_kd_, 0, 0);
  
  //Yaw PID
  this->tag_pid_yaw_.setGains(this->tag_yaw_kp_, 0, this->tag_yaw_kd_, 0, 0);  
  this->pid_yaw_.setGains(this->yaw_kp_, 0, this->yaw_kd_, 0, 0);
  
  this->alg_.unlock();
}

void UncalibvsAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{

  return algorithm_base::main<UncalibvsAlgNode>(argc, argv, "uncalibvs_alg_node");
}