From dfd165e19bf3be61b80020d00f0fb081d28a010f Mon Sep 17 00:00:00 2001 From: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Date: Tue, 23 Jul 2024 19:36:23 +0900 Subject: [PATCH] Use message timestamp for internal timestamp (#181) --- src/stella_vslam_ros.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/stella_vslam_ros.cc b/src/stella_vslam_ros.cc index b9ab4bd..cb6bb58 100644 --- a/src/stella_vslam_ros.cc +++ b/src/stella_vslam_ros.cc @@ -235,7 +235,7 @@ void mono::callback(const sensor_msgs::ImageConstPtr& msg) { camera_optical_frame_ = msg->header.frame_id; } const auto tp_1 = std::chrono::steady_clock::now(); - const auto timestamp = std::chrono::duration_cast>(tp_1 - tp_0_).count(); + const auto timestamp = msg->header.stamp.toSec(); // input the current frame and estimate the camera pose auto cam_pose_wc = slam_->feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_); @@ -291,7 +291,7 @@ void stereo::callback(const sensor_msgs::ImageConstPtr& left, const sensor_msgs: } const auto tp_1 = std::chrono::steady_clock::now(); - const auto timestamp = std::chrono::duration_cast>(tp_1 - tp_0_).count(); + const auto timestamp = left->header.stamp.toSec(); // input the current frame and estimate the camera pose auto cam_pose_wc = slam_->feed_stereo_frame(leftcv, rightcv, timestamp, mask_); @@ -344,7 +344,7 @@ void rgbd::callback(const sensor_msgs::ImageConstPtr& color, const sensor_msgs:: } const auto tp_1 = std::chrono::steady_clock::now(); - const auto timestamp = std::chrono::duration_cast>(tp_1 - tp_0_).count(); + const auto timestamp = color->header.stamp.toSec(); // input the current frame and estimate the camera pose auto cam_pose_wc = slam_->feed_RGBD_frame(colorcv, depthcv, timestamp, mask_);