From 1c2ef7ff74bcd5ecbef5ba24834deb2d379e8e74 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Sat, 23 Oct 2021 03:01:59 -0300 Subject: [PATCH] Fix time_from_start computation in pose_stamped2joint.cpp --- src/pose_stamped2joint.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) 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(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); } -- 2.12.0