diff --git a/src/run_slam_offline.cc b/src/run_slam_offline.cc index 9874982..2bf2385 100644 --- a/src/run_slam_offline.cc +++ b/src/run_slam_offline.cc @@ -49,6 +49,7 @@ void tracking(const std::shared_ptr& slam_ros, const std::string& color_topic, const std::string& depth_topic, const std::string& bag_storage_id, + const double start_offset, const bool no_sleep, const std::string& viewer_string) { auto& SLAM = slam_ros->slam_; @@ -108,6 +109,12 @@ void tracking(const std::shared_ptr& slam_ros, storage_options.storage_id = bag_storage_id; rosbag2_cpp::Reader reader; reader.open(storage_options); + auto metadata = reader.get_metadata(); + auto starting_time = std::chrono::duration_cast( + metadata.starting_time.time_since_epoch()) + .count() + + start_offset * 1e9; + reader.seek(starting_time); rclcpp::Serialization serialization; std::queue> que; double track_time; @@ -290,6 +297,7 @@ int main(int argc, char* argv[]) { auto color_topic = op.add>("", "color", "color image topic name for RGBD", "camera/color/image_raw"); auto depth_topic = op.add>("", "depth", "depth image topic name for RGBD", "camera/depth/image_raw"); auto bag_storage_id = op.add>("", "storage-id", "rosbag2 storage id (default: sqlite3)", "sqlite3"); + auto start_offset = op.add>("", "start-offset", "start offset", 0.0); auto vocab_file_path = op.add>("v", "vocab", "vocabulary file path"); auto setting_file_path = op.add>("c", "config", "setting file path"); auto mask_img_path = op.add>("", "mask", "mask image path", ""); @@ -430,6 +438,7 @@ int main(int argc, char* argv[]) { color_topic->value(), depth_topic->value(), bag_storage_id->value(), + start_offset->value(), no_sleep->is_set(), viewer_string);