   unsigned long time_now = millis();
   unsigned long step_time = time_now - prev_update_time;
   prev_update_time = time_now;
-  ros::Time stamp_now = nh.now();
+  ros::Time stamp_now = ros_now();
 
   // odom
   updateOdometry((double)(step_time * 0.001));

void updateTF(geometry_msgs::TransformStamped& odom_tf)
{
  odom.header.frame_id = "odom";
  odom_tf.header = odom.header;
  odom_tf.child_frame_id = "base_footprint";
  odom_tf.transform.translation.x = odom.pose.pose.position.x;




/*******************************************************************************
* Time Interpolation function
*******************************************************************************/
ros::Time add_micros(ros::Time & t, uint32_t _micros)
{
  uint32_t sec, nsec;

  /*
   * the interpolation floors the current time
   * stamp for integrity of precision.
   */
  sec  = _micros / 1000000 + t.sec;
  nsec = _micros % 1000000 + 1000 * (t.nsec / 1000);
  if (nsec >= 1e9) {
    sec++, nsec--;
  }
  return ros::Time(sec, nsec);
}

/*******************************************************************************
* Update the base time for interpolation
*******************************************************************************/
void update_time()
{
  uint32_t n_micros = micros();
  ros::Time next = nh.now();
  if (next.sec < current_time.sec) {
    /*
     * bad time was received, so we
     * continue to interpolate the
     * existing time.
     */
    return;
  }
  current_time = next;
  current_offset = n_micros;
}

/*******************************************************************************
* ros::Time::now() implementation
*******************************************************************************/
ros::Time ros_now()
{
  return add_micros(current_time, micros() - current_offset);
}

/*******************************************************************************
* Setup function
