segway_rmp400_odom_nokalman_alg_node.cpp

Go to the documentation of this file.

#include "segway_rmp400_odom_nokalman_alg_node.h"

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm>(),
  r_(0),
  p_(0),
  y_(0)
{
  //init class attributes if necessary
  loop_rate_ = 100;//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_,        false );
  public_node_handle_.param<bool>       ("publish_tf",   publish_tf_,   true  );
  
  // 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_;
  }

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

  rk_.resize(3,0);
  pk_.resize(3,0);
  vyk_.resize(3,0);

  // HARDCODED COVARIANCES
  // Segway (Base 0):  
  //   r:  0.00013767
  //   p:  0.00003088
  //   vy: 0.00093412 
  // Segway (Base 1):  
  //   r:  0.00018028
  //   p:  0.00002974
  //   vy: 0.00065963
  // IMU:  
  //   r:  0.00000676
  //   p:  0.00002375
  //   vy: 0.00003790 
  //double tmp[] ={0.00013767,0.00018028,0.00000676};
  //double tmp2[]={0.00003088,0.00002974,0.00002375};
  //double tmp3[]={0.00093412,0.00065963,0.00003790};
  double tmp[] ={100,100,0.00000676};
  double tmp2[]={100,100,0.00002375};
  double tmp3[]={100,100,0.00003790};
  rkc_.assign(tmp, tmp+3);
  pkc_.assign(tmp2, tmp2+3);
  vykc_.assign(tmp3, tmp3+3);

  // [init publishers]
  odom_publisher_ = public_node_handle_.advertise<nav_msgs::Odometry>("odom", 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);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

double SegwayRmp400OdomAlgNode::pseudokalman3(std::vector<double> v, std::vector<double> c)
{
   return (v[0]*c[1]*c[2] + v[1]*c[0]*c[2] + v[2]*c[0]*c[1] ) / ( c[1]*c[2] + c[0]*c[2] + c[0]*c[1] );
}

void SegwayRmp400OdomAlgNode::mainNodeThread(void)
{
//  ros::Time current_time = ros::Time::now();
  geometry_msgs::TransformStamped odom_trans_msg;
  //static double accum_th=0;

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

  last_time_ = current_time_;


  //get current translational velocity and component velocities
  segway_status_mutex_.enter(); 
  double vT = (left_wheels_velocity_ + right_wheels_velocity_) / 2;
  segway_status_mutex_.exit(); 
  
  // First, we will only use the half of time. See header file for info.

  imu_mutex_.enter(); 
  float vrimu = vrimu_;
  float vpimu = vpimu_;
  float vyimu = vyimu_;
  //float rimu = rimu_;
  //float pimu = pimu_;
  //float yimu = yimu_;
  imu_mutex_.exit(); 
  
  r_ = pseudokalman3(rk_,rkc_);
  p_ = pseudokalman3(pk_,pkc_);
  y_ += pseudokalman3(vyk_,vykc_) * dt/2;

  // split translational velocity in cartesian components
  double vTx  = six_d_ ? vT*cos(y_)*cos(p_) : vT*cos(y_);
  double vTy  = six_d_ ? vT*sin(y_)*cos(r_) : vT*sin(y_);
  double vTz  = six_d_ ? vT*cos(r_)*sin(p_) : 0.0;

  // Integrate velocities with time to get current position
  double dx  = vTx * dt;
  double dy  = vTy * dt;
  double dz  = vTz * dt;

  //update second half angles
  y_ += pseudokalman3(vyk_,vykc_) * dt/2;

  // [fill msg structures]
  geometry_msgs::Quaternion rot;
  if(six_d_) rot = tf::createQuaternionMsgFromRollPitchYaw(r_,-p_,y_);
  else rot = tf::createQuaternionMsgFromYaw (y_);

  //update transform message
  transform_.translation.x += dx;
  transform_.translation.y += dy;
  transform_.translation.z += dz;
  transform_.rotation       = rot;

  ROS_INFO("r %f p %f y %f | vTx %f vTy %f vTz %f | x %f y %f z %f",r_, p_, y_, vTx, vTy, vTz, transform_.translation.x, transform_.translation.y, transform_.translation.z);

  //update pose
  pose_.pose.position.x += dx;
  pose_.pose.position.y += dy;
  pose_.pose.position.z += dz;
  pose_.pose.orientation = rot;

  pose_.covariance[0]  =
  pose_.covariance[7]  =
  pose_.covariance[14] =
  pose_.covariance[21] =
  pose_.covariance[28] =
  pose_.covariance[35] = 0.5;

  //update twist
  twist_.twist.linear.x  = vT;
  twist_.twist.linear.y  = 0.0;
  twist_.twist.linear.z  = 0.0;
  twist_.twist.angular.x = vrimu;
  twist_.twist.angular.y = vpimu;
  twist_.twist.angular.z = vyimu;

  // Add the covariances to use with EKF
  // for x and y velocity, Error ~= +-10%
  twist_.covariance[0]  = 
  twist_.covariance[7]  = 
  twist_.covariance[14] =
  twist_.covariance[21] =
  twist_.covariance[28] = 
  twist_.covariance[35] = 0.0001;
  
  //send the transform
  if(publish_tf_)
  {
    //first, we'll publish the transform over tf
    odom_trans_msg.header.stamp    = current_time_;
    odom_trans_msg.header.frame_id = odom_id_;
    odom_trans_msg.child_frame_id  = base_link_id_;
    odom_trans_msg.transform       = transform_;

    odom_broadcaster_.sendTransform(odom_trans_msg);
  }

  //next, we'll publish the odometry message over ROS
  Odometry_msg_.header.stamp    = current_time_;
  Odometry_msg_.header.frame_id = odom_id_;
  Odometry_msg_.child_frame_id  = base_link_id_;
  Odometry_msg_.pose            = pose_;
  Odometry_msg_.twist           = twist_;

  // [fill msg structures]
  //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]
  odom_publisher_.publish(Odometry_msg_);

}

/*  [a callbacks] */
void SegwayRmp400OdomAlgNode::segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg) 
{ 
  ROS_DEBUG("SegwayRmp400OdomAlgNode::segway_status_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //alg_.lock(); 
  segway_status_mutex_.enter(); 

  //std::cout << msg->data << std::endl;
  left_wheels_velocity_  = (msg->rmp200[0].left_wheel_velocity + msg->rmp200[1].left_wheel_velocity)/2;
  right_wheels_velocity_ = (msg->rmp200[0].right_wheel_velocity + msg->rmp200[1].right_wheel_velocity)/2;
  
  rk_[0]  = msg->rmp200[0].roll_angle;
  rk_[1]  = msg->rmp200[1].roll_angle;
  pk_[0]  = msg->rmp200[0].pitch_angle;
  pk_[1]  = msg->rmp200[1].pitch_angle;
  vyk_[0] = msg->rmp200[0].yaw_rate;
  vyk_[1] = msg->rmp200[1].yaw_rate;

  segway_status_mutex_.exit(); 
}

void SegwayRmp400OdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg) 
{ 
  ROS_DEBUG("SegwayRmp400OdomAlgNode::imu_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //alg_.lock(); 
  imu_mutex_.enter(); 
  vrimu_ = msg->angular_velocity.x;
  vpimu_ = msg->angular_velocity.y;
  vyimu_ = msg->angular_velocity.z;
  // get time stamp
  current_time_ = msg->header.stamp;
  // get roll and pitch angles straight from the imu
  tf::Quaternion q;
  tf::quaternionMsgToTF(msg->orientation,q);
  tf::Matrix3x3(q).getRPY(rimu_,pimu_,yimu_);
  rk_[2]  = rimu_;
  pk_[2]  = pimu_;
  vyk_[2] = -vyimu_;

  // Fix roll angle
  rk_[2] = rk_[2]<0 ? rk_[2]+M_PI : rk_[2]-M_PI;

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