stamp is dt

src/subscriber_imu.cpp

@@ -283,6 +283,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
             return;
 
         // get current and origin states
+        auto origin_ts = processor_motion_ptr_->getOrigin()->getTimeStamp();
         auto origin_state = processor_motion_ptr_->getOrigin()->getFrame()->getState("PO");
         auto current_state = processor_motion_ptr_->getState("PO");
         auto current_ts = processor_motion_ptr_->getTimeStamp();
@@ -291,7 +292,8 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
         // Fill message
         geometry_msgs::PoseStamped pose_msg;
-        pose_msg.header.stamp = ros::Time(current_ts.getSeconds(), current_ts.getNanoSeconds());
+        auto dt = current_ts - origin_ts;
+        pose_msg.header.stamp = ros::Time(dt);
 
         // 3D
         if(sensor_ptr_->getProblem()->getDim() == 3)