Fix time_from_start computation in pose_stamped2joint.cpp

diff --git a/src/pose_stamped2joint.cpp b/src/pose_stamped2joint.cpp
index 2ab63cb..4cc6f0b 100644 (file)
--- a/src/pose_stamped2joint.cpp
+++ b/src/pose_stamped2joint.cpp
@@ -86,8 +86,16 @@ void Pose2Joint::poseCB(const geometry_msgs::msg::PoseStamped::SharedPtr poseSta
        Eigen::VectorXd::Map(&jointTrajPoint.positions[0],jointTrajPoint.positions.size())=q_.data;
 
        if(!t0_) t0_=std::make_unique<builtin_interfaces::msg::Time>(poseStamped->header.stamp);
-       jointTrajPoint.time_from_start.sec=poseStamped->header.stamp.sec-t0_->sec;
-       jointTrajPoint.time_from_start.nanosec=poseStamped->header.stamp.nanosec-t0_->nanosec;
+       if(poseStamped->header.stamp.nanosec >= t0_->nanosec)
+       {
+               jointTrajPoint.time_from_start.nanosec=poseStamped->header.stamp.nanosec-t0_->nanosec;
+               jointTrajPoint.time_from_start.sec=poseStamped->header.stamp.sec-t0_->sec;
+        }
+        else
+        {
+                jointTrajPoint.time_from_start.nanosec=1000000000+poseStamped->header.stamp.nanosec-t0_->nanosec;
+               jointTrajPoint.time_from_start.sec=poseStamped->header.stamp.sec-t0_->sec-1;
+        }
 
        jointTrajPointPub_->publish(jointTrajPoint);
 }