kinton_std_msgs_node.cpp

Go to the documentation of this file.

#include "kinton_std_msgs_node.h"

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

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

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

  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->imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("raw_imu", 100);
  this->height_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PointStamped>("raw_height", 100);
  
  // [init subscribers]
  this->imu_subscriber_ = this->public_node_handle_.subscribe("kinton_imu", 100, &KintonOdomAlgNode::imu_callback, this);
  this->height_subscriber_ = this->public_node_handle_.subscribe("kinton_height", 100, &KintonOdomAlgNode::height_callback, this);

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

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

void KintonOdomAlgNode::mainNodeThread(void)
{

  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]

}

/*  [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(); 

  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;

  geometry_msgs::Quaternion rot;
  rot = tf::createQuaternionMsgFromRollPitchYaw(pos_ang_imu(0,0),pos_ang_imu(1,0),pos_ang_imu(2,0));


  //Publish the odometry message over ROS
  this->Imu_msg_.header = msg->header;
  this->Imu_msg_.orientation = rot;
  this->Imu_msg_.orientation_covariance = msg->orientation_covariance;
  this->Imu_msg_.angular_velocity.x = vel_ang_imu(0,0);
  this->Imu_msg_.angular_velocity.y = vel_ang_imu(1,0);
  this->Imu_msg_.angular_velocity.z = vel_ang_imu(2,0);
  this->Imu_msg_.angular_velocity_covariance = msg->angular_velocity_covariance;
  this->Imu_msg_.linear_acceleration.x = acc_lin_imu(0,0);
  this->Imu_msg_.linear_acceleration.y = acc_lin_imu(1,0);
  this->Imu_msg_.linear_acceleration.z = acc_lin_imu(2,0);
  this->Imu_msg_.linear_acceleration_covariance = msg->linear_acceleration_covariance;

  this->imu_publisher_.publish(this->Imu_msg_);

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

void KintonOdomAlgNode::height_callback(const mav_msgs::Height::ConstPtr& msg) 
{
  //ROS_INFO("KintonOdomAlgNode::height_callback: New Message Received"); 

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

  this->Height_msg_.header = msg->header;
  this->Height_msg_.point.x =
  this->Height_msg_.point.y = 0.0;
  this->Height_msg_.point.z = msg->height;

  this->height_publisher_.publish(this->Height_msg_);

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

}
/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

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(roll, Eigen::Vector3d::UnitX()) \
      * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) \
      * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
  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");
}