test: ensures pendulum env fixture runs mj_forward

Author: DavidPL1

CHANGELOG.md

@@ -15,6 +15,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c
 * *mujoco_ros_sensors*: now skipping user sensors, as they should be handled in separate, dedicated plugins.
 * When loading takes more than 0.25 seconds the simulation is no longer automatically paused.
 * Fixed fetching of body quaternion in `get_body_state` service.
+* *tests*: PendulumEnvFixture now makes sure `mj_forward` has been run at least once. This ensures the data object is populated with correct initial positions and velocities.
 
 ### Changed
 * Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h).

mujoco_ros/test/mujoco_env_fixture.h

@@ -49,6 +49,7 @@ class MujocoEnvTestWrapper : public MujocoEnv
 	MujocoEnvTestWrapper(const std::string &admin_hash = std::string()) : MujocoEnv(admin_hash) {}
 	mjModel *getModelPtr() { return model_.get(); }
 	mjData *getDataPtr() { return data_.get(); }
+	MujocoEnvMutex *getMutexPtr() { return &physics_thread_mutex_; }
 	int getPendingSteps() { return num_steps_until_exit_; }
 
 	std::string getFilename() { return { filename_ }; }
@@ -124,6 +125,12 @@ class PendulumEnvFixture : public ::testing::Test
 			seconds += 0.001;
 		}
 		EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!";
+
+		// Make sure forward has been run at least once
+		{
+			std::lock_guard<MujocoEnvMutex> lock(*env_ptr->getMutexPtr());
+			mj_forward(env_ptr->getModelPtr(), env_ptr->getDataPtr());
+		}
 	}
 
 	void TearDown() override