syncronize camera_info with images

ubi-agni · Nov 15, 2024 · 442d16f · 442d16f
1 parent c62988c
commit 442d16f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 5 deletions.
diff --git a/mujoco_ros/include/mujoco_ros/offscreen_camera.h b/mujoco_ros/include/mujoco_ros/offscreen_camera.h
@@ -98,7 +98,7 @@ class OffscreenCamera
 private:
 	std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
 
-	void publishCameraInfo();
+	void publishCameraInfo(const ros::Time &t);
 
 	bool renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext *offscreen, const bool rgb, const bool depth,
 	                             const bool segment);

diff --git a/mujoco_ros/src/offscreen_camera.cpp b/mujoco_ros/src/offscreen_camera.cpp
@@ -250,6 +250,9 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext
 
 		depth_pub_.publish(depth_msg);
 	}
+
+	publishCameraInfo(ros::Time(scn_state_.data.time));
+
 	return true;
 }
 
@@ -298,15 +301,15 @@ void OffscreenCamera::renderAndPublish(mujoco_ros::OffscreenRenderContext *offsc
 		rendered                            = renderAndPubIfNecessary(offscreen, segment, true, segment);
 	}
 
-	if (rendered) {
-		publishCameraInfo();
+	if (!rendered) {
+		publishCameraInfo(ros::Time::now());
 	}
 }
 
-void OffscreenCamera::publishCameraInfo()
+void OffscreenCamera::publishCameraInfo(const ros::Time &t)
 {
 	sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo();
-	camera_info_msg.header.stamp            = ros::Time::now();
+	camera_info_msg.header.stamp            = t;
 
 	camera_info_pub_.publish(camera_info_msg);
 }