kinton_vs_control_alg_node.cpp

Go to the documentation of this file.

#include "kinton_vs_control_alg_node.h"

KintonVsControlAlgNode::KintonVsControlAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<KintonVsControlAlgorithm>(),
  estop_(false),
  serial_(false),
  height_fix_(0.0),
  time_(0),
  time_last_(0)
{
  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  //Init Thrust PIDs
  this->nm_pid_thrust_.initPid(this->nm_thrust_kp_, 0, this->nm_thrust_kd_, 0, 0);
  
  //Init Roll PIDs
  this->nm_pid_roll_.initPid(this->nm_roll_kp_, 0, this->nm_roll_kd_, 0, 0);
  
  //Init Pitch PIDs
  this->nm_pid_pitch_.initPid(this->nm_pitch_kp_, 0, this->nm_pitch_kd_, 0, 0);
  
  //Init Yaw PIDs
  this->nm_pid_yaw_.initPid(this->nm_yaw_kp_, 0, this->nm_yaw_kd_, 0, 0);

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

  //Get fixed transforms between the IMU data and the robot frame
  tf::TransformListener listener;
  tf::StampedTransform tf_quad_to_cam;

  //wait 1.0 secs waiting for the other nodes to start
  sleep(1.0);

  try {
    listener.waitForTransform("/base_link","/front_cam_optical_frame", ros::Time::now(), ros::Duration(1.0));
    listener.lookupTransform("/base_link","/front_cam_optical_frame", ros::Time::now(), tf_quad_to_cam);
  } 
  catch (tf::TransformException ex) {
    ROS_ERROR("Quad_to_cam Transform error: %s",ex.what());
  }

  //Homogenous transform between camera and quadrotor base_link frames
  this->T_quad_to_cam_ = getTransform(tf_quad_to_cam);

  // [init publishers]
  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->cam_vel_subscriber_ = this->public_node_handle_.subscribe("cam_vel", 100, &KintonVsControlAlgNode::cam_vel_callback, this);
  this->odom_subscriber_ = this->public_node_handle_.subscribe("odom", 100, &KintonVsControlAlgNode::odom_callback, this);
  this->ll_status_subscriber_ = this->public_node_handle_.subscribe("ll_status", 100, &KintonVsControlAlgNode::ll_status_callback, this);

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

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

void KintonVsControlAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

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

  this->cam_vel_mutex_.enter();
    bool marker_ctrl = this->marker_ctrl_;
  this->cam_vel_mutex_.exit();

  // Marker or Height control
  Eigen::MatrixXd cmd_raw = Eigen::MatrixXd::Zero(4,1);
  Eigen::MatrixXd quad_cmd = Eigen::MatrixXd::Zero(4,1);
  if (marker_ctrl){

    //Transform velocities from camera to base_link frames
    Eigen::MatrixXd quad_vel;
    quad_vel = cam_to_quad_vel();

    cmd_raw(0,0) = quad_vel(0,0);
    cmd_raw(1,0) = quad_vel(1,0);
    cmd_raw(2,0) = quad_vel(5,0);
    cmd_raw(3,0) = quad_vel(2,0);
  }  
  else{
    this->odom_mutex_.enter(); 
      double height = this->height_;
      double height_fix = this->height_fix_;
    this->odom_mutex_.exit(); 
    
    // Height control
    Eigen::MatrixXd error_no_marker = Eigen::MatrixXd::Zero(4,1);
    error_no_marker(2,0) = height - height_fix;

    //TODO: translate error vector from inertial to body

    cmd_raw(0,0) = this->nm_pid_roll_.updatePid(error_no_marker(0,0), this->time_ - this->time_last_);
    cmd_raw(1,0) = this->nm_pid_pitch_.updatePid(error_no_marker(1,0), this->time_ - this->time_last_);
    cmd_raw(2,0) = this->nm_pid_yaw_.updatePid(error_no_marker(2,0), this->time_ - this->time_last_);
    cmd_raw(3,0) = this->nm_pid_thrust_.updatePid(error_no_marker(3,0), this->time_ - this->time_last_);
  }


  //Signal adaptation (trimmers and saturations)
  quad_cmd = signal_adapt(cmd_raw);

  // [publish messages]
  this->cmd_roll_msg_.data = quad_cmd(0,0);
  this->cmd_pitch_msg_.data = quad_cmd(1,0);
  this->cmd_yaw_msg_.data = quad_cmd(2,0);
  this->cmd_thrust_msg_.data = quad_cmd(3,0);

  this->cmd_roll_publisher_.publish(this->cmd_roll_msg_);
  this->cmd_pitch_publisher_.publish(this->cmd_pitch_msg_);
  this->cmd_yaw_publisher_.publish(this->cmd_yaw_msg_);
  this->cmd_thrust_publisher_.publish(this->cmd_thrust_msg_);

  //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_);
  }

  this->time_last_ = this->time_;
}

/*  [subscriber callbacks] */
void KintonVsControlAlgNode::cam_vel_callback(const geometry_msgs::TwistWithCovariance::ConstPtr& msg) 
{ 
  //ROS_INFO("KintonVsControlAlgNode::twist_cov_callback: New Message Received"); 

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

  if (msg->covariance[0]<1000)
  {
    this->height_fix_ = this->height_;
    this->marker_ctrl_ = true;
  }
  else{
    this->marker_ctrl_ = false;
  }

  //Rotate camera velocities such as base_link
  this->cam_twist_(0,0) = msg->twist.linear.x;
  this->cam_twist_(1,0) = msg->twist.linear.y;
  this->cam_twist_(2,0) = msg->twist.linear.z;
  this->cam_twist_(3,0) = msg->twist.angular.x;
  this->cam_twist_(4,0) = msg->twist.angular.y;
  this->cam_twist_(5,0) = msg->twist.angular.z;

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

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->cam_vel_mutex_.exit(); 
}
void KintonVsControlAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg) 
{ 
  //ROS_INFO("KintonVsControlAlgNode::odom_callback: New Message Received"); 

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

  //Check if the twist covariance is correct 

  this->height_ = msg->pose.pose.position.z;

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->odom_mutex_.exit(); 
}
void KintonVsControlAlgNode::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(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

Eigen::MatrixXd KintonVsControlAlgNode::cam_to_quad_vel() 
{

  Eigen::MatrixXd cam_lin = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd cam_ang = Eigen::MatrixXd::Zero(3,1);

  //Camera desired velocities
  this->cam_vel_mutex_.enter();
    cam_lin = this->cam_twist_.block(0,0,3,1);
    cam_ang = this->cam_twist_.block(3,0,3,1);
  this->cam_vel_mutex_.exit();

  //Transform angular velocities from cam_optical_frame to base_link
  //In a rigid body the values does not change, only frame changes
  Eigen::MatrixXd quad_ang = Eigen::MatrixXd::Zero(3,1);
  quad_ang = this->T_quad_to_cam_.block(0,0,3,3) * cam_ang;

  //In a rigid body, the angular velocities add linear velocities
  //Without considering it negligible:
  //Skew symmetric to perform cross product
  // Eigen::Matrix3d skew;
  // skew(0,0) = 0.0;
  // skew(0,1) = -quad_ang(2,0);
  // skew(0,2) = quad_ang(1,0);
  // skew(1,0) = quad_ang(2,0);
  // skew(1,1) = 0.0;
  // skew(1,2) = -quad_ang(0,0);
  // skew(2,0) = -quad_ang(1,0);
  // skew(2,1) = quad_ang(0,0);
  // skew(2,2) = 0.0;

  // Eigen::MatrixXd quad_lin = Eigen::MatrixXd::Zero(3,1);
  // quad_lin = (this->T_quad_to_cam_.block(0,0,3,3) * cam_lin) + (skew * T_quad_to_cam_.block(0,3,3,1));

  //Considering cam and quad frames witht he same origin point
  Eigen::MatrixXd quad_lin = Eigen::MatrixXd::Zero(3,1);
  quad_lin = this->T_quad_to_cam_.block(0,0,3,3) * cam_lin;


  Eigen::MatrixXd quad_vel = Eigen::MatrixXd::Zero(6,1);
  quad_vel.block(0,0,3,1) = quad_lin;
  quad_vel.block(3,0,3,1) = quad_ang;

  return quad_vel;
}

Eigen::Matrix4d KintonVsControlAlgNode::getTransform(const tf::StampedTransform& transform)
{

  double roll,pitch,yaw;
  tf::Quaternion q =  transform.getRotation();
  tf::Matrix3x3(q).getRPY(roll,pitch,yaw); 

  Eigen::Matrix3d Rot;
  Rot = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) \
      * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) \
      * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
  Eigen::Matrix4d T = Eigen::Matrix4d::Zero();

  T.block(0,0,3,3) = Rot;
  T(0,3) = transform.getOrigin().x();
  T(1,3) = transform.getOrigin().y();
  T(2,3) = transform.getOrigin().z();
  T(3,3) = 1.0;  

  //ROS_INFO("x: %f y: %f z: %f roll: %f, pitch: %f, yaw: %f",transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z(),roll,pitch,yaw);

  return T;
}

Eigen::MatrixXd KintonVsControlAlgNode::signal_adapt(const Eigen::MatrixXd& cmd_raw)
{

  Eigen::MatrixXd cmd = Eigen::MatrixXd::Zero(4,1);

  //ROLL (trimmer and saturations)
  cmd(0,0) = cmd_raw(0,0)-this->trimm_y_;
  if (cmd(0,0) > this->rollpitch_sat_max_) cmd(0,0) = this->rollpitch_sat_max_;
  if (cmd(0,0) < this->rollpitch_sat_min_) cmd(0,0) = this->rollpitch_sat_min_;

  //PITCH (trimmer and saturations)
  cmd(1,0) = cmd_raw(1,0)-this->trimm_x_;
  if (cmd(1,0) > this->rollpitch_sat_max_) cmd(1,0) = this->rollpitch_sat_max_;
  if (cmd(1,0) < this->rollpitch_sat_min_) cmd(1,0) = this->rollpitch_sat_min_;

  //YAW (saturations)
  cmd(2,0) = cmd_raw(2,0);
  if (cmd(2,0) > yaw_sat_max_) cmd(2,0) = this->yaw_sat_max_;
  if (cmd(2,0) < yaw_sat_min_) cmd(2,0) = this->yaw_sat_min_;

  //THRUST (offset and saturations)
  cmd(3,0) = cmd_raw(3,0) + this->ctrl_ref_;
  if (cmd(3,0) > this->sat_max_) cmd(3,0) = this->sat_max_;
  if (cmd(3,0) < this->sat_min_) cmd(3,0) = this->sat_min_; 

  return cmd;
}

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

  //signal adaptation parameters
  this->trimm_x_ = config.trimm_x;
  this->trimm_y_ = config.trimm_y;
  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->ctrl_ref_ = config.thrust_ctrl_ref;
  this->sat_min_ = config.thrust_sat_min;
  this->sat_max_ = config.thrust_sat_max;

  //Height control parameters
  this->nm_roll_kp_ = config.nm_roll_kp;
  this->nm_roll_kd_ = config.nm_roll_kd;
  this->nm_pitch_kp_ = config.nm_pitch_kp;
  this->nm_pitch_kd_ = config.nm_pitch_kd;
  this->nm_yaw_kp_ = config.nm_yaw_kp;
  this->nm_yaw_kd_ = config.nm_yaw_kd;
  this->nm_thrust_kp_ = config.nm_thrust_kp;
  this->nm_thrust_kd_ = config.nm_thrust_kd;

  //Emergency stop external signal
  this->estop_=config.EMERGENCY_STOP;
  this->alg_.unlock();
}

void KintonVsControlAlgNode::addNodeDiagnostics(void)
{
}

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