kinton_odom_alg_node.cpp

Go to the documentation of this file.

#include "kinton_odom_alg_node.h"

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

  this->public_node_handle_.param<std::string>("tf_prefix",    this->tf_prefix_,    "");
  this->public_node_handle_.param<std::string>("odom_id",      this->odom_id_,      "odom" );
  this->public_node_handle_.param<std::string>("base_link_id", this->base_link_id_, "base_link" );
  this->public_node_handle_.param<bool>       ("publish_tf",   this->publish_tf_,   true  );
  
  // push down frame ids if necessary
  if( this->tf_prefix_.size()!=0 )
  {
    this->odom_id_      = this->tf_prefix_ + "/" + this->odom_id_;
    this->base_link_id_ = this->tf_prefix_ + "/" + this->base_link_id_;
  }

  ini_zeros();

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

  //Get fixed transforms between the IMU data and the robot frame
  tf::TransformListener listener1,listener2,listener3;
  tf::StampedTransform tf_accelerometers,tf_gyros,tf_compass;

  try {
    listener1.waitForTransform("/base_link","/accelerometers", ros::Time::now(), ros::Duration(1.0));
    listener1.lookupTransform("/base_link","/accelerometers", ros::Time::now(), tf_accelerometers);
  } 
  catch (tf::TransformException ex1) {
    ROS_ERROR("Accelerometers Transform error: %s",ex1.what());
  }

  try {
    listener2.waitForTransform("/base_link","/gyros", ros::Time::now(), ros::Duration(1.0));
    listener2.lookupTransform("/base_link","/gyros", ros::Time::now(), tf_gyros);
  }
  catch (tf::TransformException ex2) {
    ROS_ERROR("Gyros Transform error: %s",ex2.what());
  }

  try {
    listener3.waitForTransform("/base_link","/compass", ros::Time::now(), ros::Duration(1.0));
    listener3.lookupTransform("/base_link","/compass", ros::Time::now(), tf_compass);
  }
  catch (tf::TransformException ex3) {
    ROS_ERROR("Compass Transform error: %s",ex3.what());
  }


  //Transform to eigen 4x4 matrix
  this->T_acc_ = getTransform(tf_accelerometers);
  this->T_gyr_ = getTransform(tf_gyros);
  this->T_comp_ = getTransform(tf_compass);

  // [init publishers]
  this->odom_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("odom", 100);
  
  // [init subscribers]
  this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 100, &KintonOdomAlgNode::imu_callback, this);
  this->height_point_subscriber_ = this->public_node_handle_.subscribe("height_point", 100, &KintonOdomAlgNode::height_point_callback, this);

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

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

void KintonOdomAlgNode::mainNodeThread(void)
{
  geometry_msgs::TransformStamped odom_trans_msg;

  //compute differencial
  double dt = (this->current_time_ - this->last_time_).toSec();
  if (dt > 10) dt = 0.01;
  //ROS_ERROR_STREAM("Times: " << this->last_time_ << " " << this->current_time_ << " dt: " << dt);

  //update the last_time
  this->last_time_ = this->current_time_;

  // Sensors data ///////////////////////////////////////////////////////////
  //get data from the IMU
  Eigen::MatrixXd pos_ang_imu(3,1), vel_ang_imu(3,1), acc_lin_imu(3,1);
  this->imu_mutex_.enter();  
    pos_ang_imu = this->pos_ang_imu_;
    vel_ang_imu = this->vel_ang_imu_;
    acc_lin_imu = this->acc_lin_imu_;
  this->imu_mutex_.exit();  
  //get data from height
  double height;
  this->height_point_mutex_.enter(); 
    height = this->height_;
  this->height_point_mutex_.exit(); 

  //ANGLES r.t ODOM frame (Integrated angular velocities) ///////////////////
  this->pos_ang_odom_ += vel_ang_imu * dt;
  geometry_msgs::Quaternion rot;
  rot = tf::createQuaternionMsgFromRollPitchYaw(this->pos_ang_odom_(0,0),this->pos_ang_odom_(1,0),this->pos_ang_odom_(2,0));
  //Get rotation matrix
  Eigen::Matrix3d R_mov;
  R_mov = Eigen::AngleAxisd(this->pos_ang_odom_(0,0), Eigen::Vector3d::UnitX()) \
      * Eigen::AngleAxisd(this->pos_ang_odom_(1,0), Eigen::Vector3d::UnitY()) \
      * Eigen::AngleAxisd(this->pos_ang_odom_(2,0), Eigen::Vector3d::UnitZ());

  // LINEAR VELOCITY vector (Integrated linear accelerations) ///////////////
  Eigen::MatrixXd vel_lin_imu(3,1);
  vel_lin_imu = acc_lin_imu * dt;
  //r.t. ODOM frame
  this->vel_lin_odom_ += R_mov * vel_lin_imu;

  // LINEAR POSITIONS r.t. ODOM (Integrated lineal velocities) ///////////////
  this->pos_lin_odom_ += this->vel_lin_odom_ * dt;


  // [fill msg structures]

  //update transform message
  this->transform_.translation.x = this->pos_lin_odom_(0,0);
  this->transform_.translation.y = this->pos_lin_odom_(1,0);
  //this->transform_.translation.z = this->pos_lin_odom_(2,0);
  this->transform_.translation.z = height;
  this->transform_.rotation      = rot;

  //update pose
  this->pose_.pose.position.x = this->pos_lin_odom_(0,0);
  this->pose_.pose.position.y = this->pos_lin_odom_(1,0);
  // this->pose_.pose.position.z = this->pos_lin_odom_(2,0);
  this->pose_.pose.position.z = height;
  this->pose_.pose.orientation = rot;
  this->pose_.covariance[0]  = 
  this->pose_.covariance[7]  = 
  this->pose_.covariance[14] = 
  this->pose_.covariance[21] = 
  this->pose_.covariance[28] = 
  this->pose_.covariance[35] = 0.5;

  //update twist
  this->twist_.twist.linear.x  = vel_lin_imu(0,0);
  this->twist_.twist.linear.y  = vel_lin_imu(1,0);
  this->twist_.twist.linear.z  = vel_lin_imu(2,0);
  this->twist_.twist.angular.x = vel_ang_imu(0,0);
  this->twist_.twist.angular.y = vel_ang_imu(1,0);
  this->twist_.twist.angular.z = vel_ang_imu(2,0);

  this->twist_.covariance[0]  = 
  this->twist_.covariance[7]  = 
  this->twist_.covariance[14] = 
  this->twist_.covariance[21] = 
  this->twist_.covariance[28] = 
  this->twist_.covariance[35] = 0.01;

  //send the transform
  if(this->publish_tf_)
  {
    //Publish the transform over tf
    odom_trans_msg.header.stamp    = this->current_time_;
    odom_trans_msg.header.frame_id = this->odom_id_;
    odom_trans_msg.child_frame_id  = this->base_link_id_;
    odom_trans_msg.transform       = this->transform_;

    this->odom_broadcaster_.sendTransform(odom_trans_msg);
  }

  //Publish the odometry message over ROS
  this->Odometry_msg_.header.stamp    = this->current_time_;
  this->Odometry_msg_.header.frame_id = this->odom_id_;
  this->Odometry_msg_.child_frame_id  = this->base_link_id_;
  this->Odometry_msg_.pose            = this->pose_;
  this->Odometry_msg_.twist           = this->twist_;

  //this->Odometry_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]
  this->odom_publisher_.publish(this->Odometry_msg_);
}

/*  [subscriber callbacks] */

void KintonOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg) 
{ 

  //Considering the IMU frame with X(Fwd)-Y(Left)-Z(up), but the gravity positive (it should be removed)


  //ROS_INFO("KintonOdomAlgNode::imu_callback: New Message Received"); 

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

  //get time stamp
  this->current_time_ = msg->header.stamp;

  Eigen::MatrixXd pos_ang_imu(4,1), acc_lin_imu(4,1), vel_ang_imu(4,1);

  //Get vectors /////////////////////////////////////////////////////////////
  //linear accelerations in base_link (roll change of pi/2)
  acc_lin_imu(0,0) = msg->linear_acceleration.x;
  acc_lin_imu(1,0) = msg->linear_acceleration.y; 
  acc_lin_imu(2,0) = msg->linear_acceleration.z; 
  acc_lin_imu(3,0) = 1.0;

  //angular velocities in base_link
  vel_ang_imu(0,0) = msg->angular_velocity.x;
  vel_ang_imu(1,0) = msg->angular_velocity.y;
  vel_ang_imu(2,0) = msg->angular_velocity.z;
  vel_ang_imu(3,0) = 1.0;

  //angles base_link r.t. world
  tf::Quaternion q;
  tf::quaternionMsgToTF(msg->orientation,q);
  tf::Matrix3x3(q).getRPY(pos_ang_imu(0,0), pos_ang_imu(1,0), pos_ang_imu(2,0));
  pos_ang_imu(3,0) = 1.0;

  //Transform data to base_link frame /////////////////////////////////////////
  //To simplify the translation between frames won't be considered
  // acc_lin_imu = this->T_acc_ * acc_lin_imu;
  // vel_ang_imu = this->T_gyr_ * vel_ang_imu;
  // pos_ang_imu = this->T_comp_ * pos_ang_imu;


  //Gravity extraction from accelerations //////////////////////////////////////
  //Quadrotor pitch and roll
  this->Rquad_ = Eigen::AngleAxisd(pos_ang_imu(0,0), Eigen::Vector3d::UnitX()) \
      * Eigen::AngleAxisd(pos_ang_imu(1,0), Eigen::Vector3d::UnitY()) \
      * Eigen::AngleAxisd(pos_ang_imu(2,0), Eigen::Vector3d::UnitZ());

  //gravity vector    
  Eigen::MatrixXd g(3,1), new_g(3,1);
  g(0) = 0;
  g(1) = 0;
  g(2) = 9.8;
  new_g = this->Rquad_ * g;

  //Quadrotor acceleration
  acc_lin_imu(0,0) = (acc_lin_imu(0,0) - new_g(0,0));
  acc_lin_imu(1,0) = (acc_lin_imu(1,0) - new_g(1,0)); 
  acc_lin_imu(2,0) = (acc_lin_imu(2,0) - new_g(2,0)); 
  acc_lin_imu(3,0) = 1.0;

  //TODO: Do not consider IMU values
  acc_lin_imu = Eigen::MatrixXd::Zero(3,1);  
  //vel_ang_imu = Eigen::MatrixXd::Zero(3,1); 
  pos_ang_imu = Eigen::MatrixXd::Zero(3,1);

  this->acc_lin_imu_ = acc_lin_imu.block(0,0,3,1);
  this->vel_ang_imu_ = vel_ang_imu.block(0,0,3,1);
  this->pos_ang_imu_ = pos_ang_imu.block(0,0,3,1);

  // ROS_DEBUG("ACC_X:%f ACC_Y:%f ACC_Z:%f",this->acc_lin_imu_(0,0),this->acc_lin_imu_(1,0),this->acc_lin_imu_(2,0));
  // ROS_DEBUG("VEL_X:%f VEL_Y:%f VEL_Z:%f",this->vel_ang_imu_(0,0),this->vel_ang_imu_(1,0),this->vel_ang_imu_(2,0));
  // ROS_DEBUG("POS_X:%f POS_Y:%f POS_Z:%f",this->pos_ang_imu_(0,0),this->pos_ang_imu_(1,0),this->pos_ang_imu_(2,0));


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

void KintonOdomAlgNode::height_point_callback(const geometry_msgs::PointStamped::ConstPtr& msg) 
{
  //ROS_INFO("KintonOdomAlgNode::height_point_callback: New Message Received"); 

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

  this->height_ = msg->point.z;

  if (this->init_) {
    this->height_init_ = this->height_;
    this->init_ = false;
  }
  this->height_ = this->height_ - this->height_init_;   
        

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->height_point_mutex_.exit(); 
}
/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void KintonOdomAlgNode::ini_zeros() 
{
  ROS_DEBUG("Initialization to Zeros");

  this->last_time_ = ros::Time::now();
  usleep(10);
  this->current_time_ = ros::Time::now();

  //Initialization of positions, velocities and accelerations
  this->pos_ang_imu_ = Eigen::MatrixXd::Zero(3,1);
  this->vel_ang_imu_ = Eigen::MatrixXd::Zero(3,1);
  this->acc_lin_imu_ = Eigen::MatrixXd::Zero(3,1);

  //Initialization of linear velocities
  this->vel_lin_odom_ = Eigen::MatrixXd::Zero(3,1);

  //Initialization of angles
  this->pos_ang_odom_ = Eigen::MatrixXd::Zero(3,1);

  //Initialization of positions
  this->pos_lin_odom_ = Eigen::MatrixXd::Zero(3,1);
 
  //Initial height
  this->height_init_ = 0.0;  

  //Init = true
  this->init_ = true;           
}

Eigen::Matrix4d KintonOdomAlgNode::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;
}

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

  this->alg_.unlock();
}

void KintonOdomAlgNode::addNodeDiagnostics(void)
{
}

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