stamp is dt

src/publisher_odom_icp.cpp

@@ -71,7 +71,13 @@ void PublisherOdomIcp::publishDerived()
     if (publisher_.getNumSubscribers() == 0 )
         return;
 
-    Eigen::Vector3d pose_origin_last = processor_odom_icp_->getOriginLastTransform();
+    double dt;
+    Eigen::Vector3d pose_origin_last = processor_odom_icp_->getOriginLastTransform(dt);
+
+    if (dt < 0)
+        return;
+        
+    msg_.header.stamp = ros::Time(dt);
 
     auto q_origin_last = Eigen::Quaterniond(Eigen::AngleAxisd(pose_origin_last(2),
                                             Eigen::Vector3d::UnitZ()));