fix: make cams publish first image on init

ubi-agni · Jun 30, 2023 · ff8d420 · ff8d420
1 parent 4ce0589
commit ff8d420
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 1 deletion.
diff --git a/mujoco_ros/CHANGELOG.md b/mujoco_ros/CHANGELOG.md
@@ -13,6 +13,7 @@
 * Fixed publish rate of camera images (#22).
 * Now publishing clock on reload.
 * Fixed memory leak caused by missing deallocation of opengl contexts on reload.
+* Cameras now publish their first image at timestep 0 instead of waiting for `t = 1/pub_frequency`.
 
 ### Refactored
 * Deprecated `getEnv`, `getEnvById`, and `unregisterEnv` functions.

diff --git a/mujoco_ros/src/rendering/utils.cpp b/mujoco_ros/src/rendering/utils.cpp
@@ -392,7 +392,8 @@ void offScreenRenderEnv(MujocoEnvPtr env)
 		env->vis_.cam.type       = mjCAMERA_FIXED;
 		env->vis_.cam.fixedcamid = stream->cam_id_;
 
-		if (ros::Duration(1 / stream->pub_freq_) > ros::Time::now() - stream->last_pub_) {
+		if (stream->last_pub_ != ros::Time(0) &&
+		    ros::Duration(1 / stream->pub_freq_) > ros::Time::now() - stream->last_pub_) {
 			continue;
 		}