segway_rmp400_odom_fused_alg_node.cpp

Go to the documentation of this file.

#include "segway_rmp400_odom_fused_alg_node.h"
#include <algorithm>

using namespace Eigen;

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm>(),
  r_(0),
  p_(0),
  th_(0),
  dth_(0),
  sigma2_dth_(0),
  first_segway_status_(true),
  first_imu_(true)
{
  //init class attributes if necessary
  loop_rate_ = 10;//in [Hz]

  public_node_handle_.param<std::string>("tf_prefix",        tf_prefix_,        "");
  public_node_handle_.param<std::string>("odom_id",          odom_id_,          "odom" );
  public_node_handle_.param<std::string>("base_link_id",     base_link_id_,     "base_footprint" );
  public_node_handle_.param<bool>       ("six_d",            six_d_,            true );
  public_node_handle_.param<bool>       ("publish_tf",       publish_tf_,       true );
  public_node_handle_.param<bool>       ("publish_odom_rel", publish_odom_rel_, true );
  public_node_handle_.param<double>     ("sigma_dx_fwd",     sigma_dx_fwd_,     0.00425 ); //teo_apps error_icp_mp.py
  public_node_handle_.param<double>     ("sigma_dx_turn",    sigma_dx_turn_,    0.00425 ); //teo_apps error_icp_mp.py
  public_node_handle_.param<double>     ("sigma_thimu",      sigma_thimu_,      0.00453 ); //teo_apps error_imu.py
  public_node_handle_.param<double>     ("offset_fwd",       offset_fwd_,       0.06758 ); //teo_apps error_icp_mp.py
  public_node_handle_.param<double>     ("offset_turn",      offset_turn_,      0.06758 ); //teo_apps error_icp_mp.py

  cov_ = MatrixXf::Zero(3,3);
  Q_   = MatrixXf::Zero(3,3);
  Jp_  = MatrixXf::Identity(3,3);
  Jd_  = MatrixXf::Identity(3,3);

  // push down frame ids if necessary
  if( tf_prefix_.size()!=0 )
  {
    odom_id_      = tf_prefix_ + "/" + odom_id_;
    base_link_id_ = tf_prefix_ + "/" + base_link_id_;
  }

  // [init publishers]
  odom_publisher_ = public_node_handle_.advertise<nav_msgs::Odometry>("odom", 100);
  odom_rel_publisher_ = public_node_handle_.advertise<nav_msgs::Odometry>("odom_rel", 100);

  // [init subscribers]
  segway_status_subscriber_ = public_node_handle_.subscribe("segway_status", 100, &SegwayRmp400OdomAlgNode::segway_status_callback, this);
  imu_subscriber_ = public_node_handle_.subscribe("imu", 100, &SegwayRmp400OdomAlgNode::imu_callback, this);

}

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

void SegwayRmp400OdomAlgNode::mainNodeThread(void)
{
  // Initialize Time
  if(first_segway_status_)
  {
      last_time_ = ros::Time::now();
      first_segway_status_=false;
  }
  ros::Time current_time = ros::Time::now();

  // compute delta time
  double dt = (current_time - last_time_).toSec();
  last_time_ = current_time;

  // Get translational velocity and last theta angle
  segway_status_mutex_.enter();
    double vT = vT_;
  segway_status_mutex_.exit();
  // Get IMU variance2, delta yaw, angular velocities, reset delta theta and sigma^2
  imu_mutex_.enter();
    double sigma2_dth = sigma2_dth_;
    sigma2_dth_ = 0;
    double dth = dth_;
    dth_ = 0;
    double r = r_;
    double p = p_;
    double vrimu  = vrimu_;
    double vpimu  = vpimu_;
    double vthimu = vthimu_;
  imu_mutex_.exit();

  // Kinematic Model = translation + rotation
  // Propagate covariance using the previous angle
  double dx_local,sigma_dx;
  if (fabs(vthimu)>0.05)
  {
    dx_local  = vT*dt*(1+ offset_turn_);
    sigma_dx = sigma_dx_turn_;
  }else{
    dx_local  = vT*dt*(1+offset_fwd_);
    sigma_dx = sigma_dx_fwd_;
  }
  double dy_local = 0;
  // Reference Point Jacobian
  Jp_(0,2) =-sin(th_)*dx_local - cos(th_)*dy_local;
  Jp_(1,2) = cos(th_)*dx_local - sin(th_)*dy_local;
  // Displacement Jacobian
  Jd_(0,0) = cos(th_);
  Jd_(0,1) =-sin(th_);
  Jd_(1,0) = sin(th_);
  Jd_(1,1) = cos(th_);
  // Sensor noise
  Q_(0,0) = sigma_dx*sigma_dx;
  Q_(1,1) = 1e-10;
  Q_(2,2) = sigma2_dth;
  // Covariance propagation
  cov_ = Jp_ * cov_ * Jp_.transpose() + Jd_ * Q_ * Jd_.transpose();
  // Relative Covariance (Local coordinates)
  Eigen::Matrix3f cov_rel = Q_;


  // Get x,y,z displacements (Global coordinates)
  // displacements          TODO 3D (6D)               2D (3D)
  double dx  = six_d_ ? dx_local*cos(th_)*cos(p) : dx_local*cos(th_)-dy_local*sin(th_);
  double dy  = six_d_ ? dx_local*sin(th_)*cos(r) : dx_local*sin(th_)+dy_local*cos(th_);
  double dz  = six_d_ ? dx_local*cos(r)*sin(p)   : 0.0;
  // update and accumulate yaw angle theta
  th_ += dth;

  // Update message
  geometry_msgs::TransformStamped odom_trans_msg;
  Odometry_msg_.header.stamp    = odom_trans_msg.header.stamp    = current_time;
  Odometry_msg_.header.frame_id = odom_trans_msg.header.frame_id = odom_id_;
  Odometry_msg_.child_frame_id  = odom_trans_msg.child_frame_id  = base_link_id_;

  odom_trans_msg.transform.translation.x = Odometry_msg_.pose.pose.position.x  += dx;
  odom_trans_msg.transform.translation.y = Odometry_msg_.pose.pose.position.y  += dy;
  odom_trans_msg.transform.translation.z = Odometry_msg_.pose.pose.position.z  += dz;
  // Calculate rotation quaternion
  geometry_msgs::Quaternion rot,rot_rel;
  if(six_d_) rot = tf::createQuaternionMsgFromRollPitchYaw(r,-p,th_);
  else rot = tf::createQuaternionMsgFromYaw (th_);
  odom_trans_msg.transform.rotation      = Odometry_msg_.pose.pose.orientation  = rot;

  Odometry_msg_.pose.covariance[0]  = cov_(0,0);
  Odometry_msg_.pose.covariance[1]  = cov_(0,1);
  Odometry_msg_.pose.covariance[5]  = cov_(0,2);
  Odometry_msg_.pose.covariance[6]  = cov_(1,0);
  Odometry_msg_.pose.covariance[7]  = cov_(1,1);
  Odometry_msg_.pose.covariance[11] = cov_(1,2);
  Odometry_msg_.pose.covariance[30] = cov_(2,0);
  Odometry_msg_.pose.covariance[31] = cov_(2,1);
  Odometry_msg_.pose.covariance[35] = cov_(2,2);

  Odometry_msg_.twist.twist.linear.x  = vT;
  Odometry_msg_.twist.twist.linear.y  = 0.0;
  Odometry_msg_.twist.twist.linear.z  = 0.0;
  Odometry_msg_.twist.twist.angular.x = vrimu;
  Odometry_msg_.twist.twist.angular.y = vpimu;
  Odometry_msg_.twist.twist.angular.z = vthimu;

  Odometry_msg_.twist.covariance[0]  = sigma_dx*sigma_dx;
  Odometry_msg_.twist.covariance[35] = sigma2_dth_/(dt*dt);

  // Relative Odometry TODO assure which covariance use
  Odometry_rel_msg_ = Odometry_msg_;
  Odometry_rel_msg_.pose.pose.position.x  = dx_local;
  Odometry_rel_msg_.pose.pose.position.y  = dy_local;
  // Relative rotation TODO 6D rotation
  //if(six_d_) rot_rel = tf::createQuaternionMsgFromRollPitchYaw(dr,-dp,dth);
  //else 
  rot_rel = tf::createQuaternionMsgFromYaw (dth);
  Odometry_rel_msg_.pose.pose.orientation = rot_rel;
  Odometry_rel_msg_.pose.covariance[0]    = cov_rel(0,0);
  Odometry_rel_msg_.pose.covariance[1]    = cov_rel(0,1);
  Odometry_rel_msg_.pose.covariance[5]    = cov_rel(0,2);
  Odometry_rel_msg_.pose.covariance[6]    = cov_rel(1,0);
  Odometry_rel_msg_.pose.covariance[7]    = cov_rel(1,1);
  Odometry_rel_msg_.pose.covariance[11]   = cov_rel(1,2);
  Odometry_rel_msg_.pose.covariance[30]   = cov_rel(2,0);
  Odometry_rel_msg_.pose.covariance[31]   = cov_rel(2,1);
  Odometry_rel_msg_.pose.covariance[35]   = cov_rel(2,2);

  // Publish Odometry Message
  odom_publisher_.publish(Odometry_msg_);
  if(publish_odom_rel_)
    odom_rel_publisher_.publish(Odometry_rel_msg_);
  if(publish_tf_)
    odom_broadcaster_.sendTransform(odom_trans_msg);
}

void SegwayRmp400OdomAlgNode::segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg) 
{
  // Get Segway's Translational Velocity
  segway_status_mutex_.enter();
    vT_ = (msg->rmp200[0].left_wheel_velocity + msg->rmp200[0].right_wheel_velocity +
           msg->rmp200[1].left_wheel_velocity + msg->rmp200[1].right_wheel_velocity)/4;
  segway_status_mutex_.exit();
}

void SegwayRmp400OdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
  //use appropiate mutex to shared variables if necessary
  //alg_.lock();
  imu_mutex_.enter();

  if(first_imu_)
  {
    last_time_imu_ = msg->header.stamp;
    first_imu_ = false;
  }

  // get time stamp
  ros::Time current_time_imu = msg->header.stamp;
  // get yaw velocity
  vrimu_ = msg->angular_velocity.x;
  vpimu_ = msg->angular_velocity.y;
  vthimu_ = -msg->angular_velocity.z;

  // TODO get roll and pitch angles straight from the imu
  tf::Quaternion q;
  tf::quaternionMsgToTF(msg->orientation,q);
  tf::Matrix3x3(q).getRPY(rimu_, pimu_, thimu_);

  // Fix roll angle
  rimu_ = rimu_<0 ? rimu_+M_PI : rimu_-M_PI;

  r_ = rimu_;
  p_ = -pimu_;

  double dt_imu = (current_time_imu - last_time_imu_).toSec();
  last_time_imu_ = current_time_imu;

  // delta theta accum
  dth_ += vthimu_ * dt_imu;
  // covariance accum
  sigma2_dth_ += sigma_thimu_ * sigma_thimu_;


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

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void SegwayRmp400OdomAlgNode::node_config_update(Config &config, uint32_t level)
{
  alg_.lock();

  alg_.unlock();
}

void SegwayRmp400OdomAlgNode::addNodeDiagnostics(void)
{
}

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