From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sat, 23 Oct 2021 06:01:59 +0000 (-0300)
Subject: Fix time_from_start computation in pose_stamped2joint.cpp
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=1c2ef7ff74bcd5ecbef5ba24834deb2d379e8e74;p=trajectory_conversions.git

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
--- 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);
 }