diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index fdbf9eb..665734f 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -44,7 +44,7 @@ MYNTEYE_BEGIN_NAMESPACE
 
 namespace enc = sensor_msgs::image_encodings;
 
-ros::Time HardTime2SoftTime(double _hard_time) {
+ros::Time hardTimeToSoftTime(double _hard_time) {
   static bool isInited = false;
   static double soft_time_begin(0), hard_time_begin(0);
 
@@ -255,11 +255,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   void publishTopics() {
     api_->SetStreamCallback(
         Stream::LEFT, [this](const api::StreamData &data) {
-          ros::Time stamp = HardTime2SoftTime(data.img->timestamp);
-          // static double ros_time_beg = ros::Time::now().toSec();
-          // static double img_time_beg = data.img->timestamp;
-          // ros::Time stamp(
-          //    ros_time_beg + (data.img->timestamp - img_time_beg) * 0.00001f);
+          ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
 
           // static double img_time_prev = -1;
           // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
@@ -282,11 +278,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     api_->SetStreamCallback(
         Stream::RIGHT, [this](const api::StreamData &data) {
-          ros::Time stamp = HardTime2SoftTime(data.img->timestamp);
-          // static double ros_time_beg = ros::Time::now().toSec();
-          // static double img_time_beg = data.img->timestamp;
-          // ros::Time stamp(
-          //    ros_time_beg + (data.img->timestamp - img_time_beg) * 0.00001f);
+          ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
 
           ++right_count_;
           publishCamera(Stream::RIGHT, data, right_count_, stamp);
@@ -318,11 +310,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
         });
 
     api_->SetMotionCallback([this](const api::MotionData &data) {
-      ros::Time stamp = HardTime2SoftTime(data.imu->timestamp);
-      // static double ros_time_beg = ros::Time::now().toSec();
-      // static double imu_time_beg = data.imu->timestamp;
-      // ros::Time stamp(
-      //     ros_time_beg + (data.imu->timestamp - imu_time_beg) * 0.00001f);
+      ros::Time stamp = hardTimeToSoftTime(data.imu->timestamp);
 
       // static double imu_time_prev = -1;
       // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << ros_time_beg