test: fix tests for colcon

mujoco_ros/test/CMakeLists.txt

@@ -71,5 +71,6 @@ install(FILES
   empty_world.xml
   equality_world.xml
   camera_world.xml
+  pendulum_world.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test
 )

mujoco_ros/test/mujoco_env_fixture.h

@@ -40,7 +40,6 @@
 #include <mujoco_ros/mujoco_env.h>
 #include <dynamic_reconfigure/server.h>
 #include <mujoco_ros/SimParamsConfig.h>
-#include "test_util.h"
 
 using namespace mujoco_ros;
 namespace mju = ::mujoco::sample_util;
@@ -68,10 +67,21 @@ class MujocoEnvTestWrapper : public MujocoEnv
 	int getNumCBReadyPlugins() { return cb_ready_plugins_.size(); }
 	void notifyGeomChange() { notifyGeomChanged(0); }
 
+	void load_queued_model()
+	{
+		settings_.load_request = 2;
+		float seconds          = 0;
+		while (getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
+			seconds += 0.001;
+		}
+		EXPECT_LT(seconds, 2) << "Model could not be loaded in time, ran into 2 second timeout!";
+	}
+
 	void load_filename(const std::string &filename)
 	{
 		mju::strcpy_arr(queued_filename_, filename.c_str());
-		settings_.load_request = 2;
+		load_queued_model();
 	}
 
 	void shutdown()
@@ -150,7 +160,7 @@ class PendulumEnvFixture : public ::testing::Test
 			std::this_thread::sleep_for(std::chrono::milliseconds(1));
 			seconds += 0.001;
 		}
-		EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!";
+		ASSERT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!";
 
 		// Make sure forward has been run at least once
 		{

mujoco_ros/test/mujoco_env_test.cpp

@@ -49,7 +49,21 @@ int main(int argc, char **argv)
 {
 	::testing::InitGoogleTest(&argc, argv);
 	ros::init(argc, argv, "mujoco_env_test");
-	return RUN_ALL_TESTS();
+
+	// Uncomment to enable debug output (useful for debugging failing tests)
+	// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
+	// ros::console::notifyLoggerLevelsChanged();
+
+	// Create spinner to communicate with ROS
+	ros::AsyncSpinner spinner(1);
+	spinner.start();
+	ros::NodeHandle nh;
+	int ret = RUN_ALL_TESTS();
+
+	// Stop spinner and shutdown ROS before returning
+	spinner.stop();
+	ros::shutdown();
+	return ret;
 }
 
 using namespace mujoco_ros;
@@ -58,9 +72,11 @@ namespace mju = ::mujoco::sample_util;
 // This needs to be listed first, otherwise the throw is not detected
 TEST_F(BaseEnvFixture, EvalModeWithoutHashThrow)
 {
+	ROS_WARN("###### [START] EvalModeWithoutHashThrow ######");
 	nh->setParam("eval_mode", true);
 	std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml";
-	EXPECT_THROW(env_ptr = std::make_unique<MujocoEnvTestWrapper>(""), std::runtime_error);
+	EXPECT_THROW(env_ptr = std::make_unique<MujocoEnvTestWrapper>(), std::runtime_error);
+	ROS_WARN("###### [END] EvalModeWithoutHashThrow ######");
 }
 
 TEST_F(BaseEnvFixture, RunEvalMode)
@@ -437,17 +453,15 @@ TEST_F(BaseEnvFixture, Reload)
 	env_ptr->startWithXML(xml_path);
 
 	// Load same model again in unpaused state
-	load_queued_model(env_ptr.get());
+	env_ptr->load_queued_model();
 	EXPECT_FALSE(env_ptr->settings_.run) << "Model should stay paused on init!";
 	EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Wrong content in filename_!";
 	EXPECT_EQ(env_ptr->getDataPtr()->time, 0) << "Time should have been reset to 0!";
 	EXPECT_EQ(env_ptr->settings_.run, 0) << "Model should stay paused after reset!";
 
 	// Load new model in paused state
 	std::string xml_path2 = ros::package::getPath("mujoco_ros") + "/test/pendulum_world.xml";
-	mju::strcpy_arr(env_ptr->queued_filename_, xml_path2.c_str());
-
-	load_queued_model(env_ptr.get());
+	env_ptr->load_filename(xml_path2);
 	EXPECT_EQ(env_ptr->getFilename(), xml_path2) << "Wrong content in filename_!";
 
 	env_ptr->settings_.run.store(1);
@@ -458,7 +472,7 @@ TEST_F(BaseEnvFixture, Reload)
 	}
 
 	// Load same model in unpaused state
-	load_queued_model(env_ptr.get());
+	env_ptr->load_queued_model();
 	EXPECT_EQ(env_ptr->getFilename(), xml_path2) << "Wrong content in filename_!";
 
 	// Let some time pass

mujoco_ros/test/mujoco_ros_plugin_test.cpp

@@ -67,7 +67,7 @@ class LoadedPluginFixture : public ::testing::Test
 {
 protected:
 	std::unique_ptr<ros::NodeHandle> nh;
-	TestPlugin *test_plugin;
+	TestPlugin *test_plugin = nullptr;
 	MujocoEnvTestWrapper *env_ptr;
 
 	void SetUp() override
@@ -96,6 +96,8 @@ class LoadedPluginFixture : public ::testing::Test
 				break;
 			}
 		}
+
+		ASSERT_NE(test_plugin, nullptr) << "TestPlugin not found!";
 	}
 
 	void TearDown() override
@@ -160,6 +162,8 @@ TEST_F(BaseEnvFixture, RenderCallback)
 		}
 	}
 
+	ASSERT_NE(test_plugin, nullptr) << "TestPlugin not found!";
+
 	// wait for render callback to be called
 	float seconds = 0;
 	while (!test_plugin->ran_render_cb.load() && seconds < 1.) {
@@ -308,7 +312,7 @@ TEST_F(BaseEnvFixture, FailedLoad)
 			}
 		}
 
-		EXPECT_NE(test_plugin, nullptr) << "Dummy plugin was not loaded!";
+		ASSERT_NE(test_plugin, nullptr) << "Dummy plugin was not loaded!";
 
 		EXPECT_FALSE(test_plugin->ran_control_cb.load());
 		EXPECT_FALSE(test_plugin->ran_passive_cb.load());
@@ -350,6 +354,8 @@ TEST_F(BaseEnvFixture, FailedLoadRecoverReload)
 			}
 		}
 
+		ASSERT_NE(test_plugin, nullptr) << "Dummy plugin was not loaded!";
+
 		nh->setParam("should_fail", false);
 
 		env_ptr->settings_.load_request = 2;
@@ -397,6 +403,8 @@ TEST_F(BaseEnvFixture, FailedLoadReset)
 			}
 		}
 
+		ASSERT_NE(test_plugin, nullptr) << "Dummy plugin was not loaded!";
+
 		env_ptr->settings_.reset_request = 1;
 		float seconds                    = 0;
 		while (env_ptr->settings_.reset_request != 0 && seconds < 2) {

mujoco_ros/test/ros_interface_test.cpp

@@ -50,6 +50,7 @@
 #include <geometry_msgs/TwistStamped.h>
 
 #include "mujoco_env_fixture.h"
+#include "test_util.h"
 
 #include <mujoco_ros/SimParamsConfig.h>
 

mujoco_ros/test/test_util.h

@@ -39,17 +39,6 @@
 #include <mujoco_ros/common_types.h>
 using namespace mujoco_ros;
 
-void load_queued_model(MujocoEnv *env)
-{
-	env->settings_.load_request = 2;
-	float seconds               = 0;
-	while (env->getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout
-		std::this_thread::sleep_for(std::chrono::milliseconds(1));
-		seconds += 0.001;
-	}
-	EXPECT_LT(seconds, 2) << "Model could not be loaded in time, ran into 2 second timeout!";
-}
-
 void compare_qpos(mjData *d, int qpos_adr_int, const std::string &joint_name, const std::vector<double> &values,
                   const std::vector<double> &tolerances = {})
 {

mujoco_ros_sensors/test/mujoco_env_fixture.h

@@ -0,0 +1 @@
+../../mujoco_ros/test/mujoco_env_fixture.h

mujoco_ros_sensors/test/mujoco_sensors_test.cpp

@@ -37,6 +37,7 @@
 #include <gtest/gtest.h>
 
 #include <mujoco_ros/render_backend.h>
+#include "mujoco_env_fixture.h"
 #include <mujoco_ros/mujoco_env.h>
 #include <mujoco_ros/common_types.h>
 #include <mujoco_ros_sensors/mujoco_sensor_handler_plugin.h>
@@ -67,6 +68,10 @@ int main(int argc, char **argv)
 	::testing::InitGoogleTest(&argc, argv);
 	ros::init(argc, argv, "mujoco_ros_sensors_test");
 
+	// Uncomment to enable debug output (useful for debugging failing tests)
+	ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
+	ros::console::notifyLoggerLevelsChanged();
+
 	ros::AsyncSpinner spinner(1);
 	spinner.start();
 	int ret = RUN_ALL_TESTS();
@@ -75,37 +80,6 @@ int main(int argc, char **argv)
 	return ret;
 }
 
-class MujocoEnvTestWrapper : public MujocoEnv
-{
-public:
-	MujocoEnvTestWrapper(const std::string &admin_hash = std::string()) : MujocoEnv(admin_hash) {}
-	mjModel *getModelPtr() { return model_.get(); }
-	mjData *getDataPtr() { return data_.get(); }
-	int getPendingSteps() { return num_steps_until_exit_; }
-
-	std::string getFilename() { return std::string(filename_); }
-	int isPhysicsRunning() { return is_physics_running_; }
-	int isEventRunning() { return is_event_running_; }
-	int isRenderingRunning() { return is_rendering_running_; }
-
-	void shutdown()
-	{
-		settings_.exit_request = 1;
-		waitForPhysicsJoin();
-		waitForEventsJoin();
-	}
-
-	const std::string &getHandleNamespace() { return nh_->getNamespace(); }
-
-	void startWithXML(const std::string &xml_path)
-	{
-		mju::strcpy_arr(queued_filename_, xml_path.c_str());
-		settings_.load_request = 2;
-		startPhysicsLoop();
-		startEventLoop();
-	}
-};
-
 static constexpr int NUM_SAMPLES = 20000;
 
 class TrainEnvFixture : public ::testing::Test
@@ -121,7 +95,7 @@ class TrainEnvFixture : public ::testing::Test
 		nh = std::make_unique<ros::NodeHandle>("~");
 		nh->setParam("eval_mode", false);
 		nh->setParam("unpause", true);
-		nh->setParam("no_x", true);
+		nh->setParam("no_render", true);
 		nh->setParam("use_sim_time", true);
 
 		env_ptr = new MujocoEnvTestWrapper();
@@ -134,7 +108,7 @@ class TrainEnvFixture : public ::testing::Test
 			std::this_thread::sleep_for(std::chrono::milliseconds(1));
 			seconds += 0.001;
 		}
-		EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!";
+		ASSERT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!";
 
 		m = env_ptr->getModelPtr();
 		d = env_ptr->getDataPtr();
@@ -144,6 +118,9 @@ class TrainEnvFixture : public ::testing::Test
 	{
 		env_ptr->shutdown();
 		delete env_ptr;
+
+		// clear all parameters
+		ros::param::del(nh->getNamespace());
 	}
 };
 
@@ -160,7 +137,7 @@ class EvalEnvFixture : public ::testing::Test
 		nh = std::make_unique<ros::NodeHandle>("~");
 		nh->setParam("eval_mode", true);
 		nh->setParam("unpause", true);
-		nh->setParam("no_x", true);
+		nh->setParam("no_render", true);
 		nh->setParam("use_sim_time", true);
 
 		env_ptr = new MujocoEnvTestWrapper("some_hash");
@@ -173,7 +150,7 @@ class EvalEnvFixture : public ::testing::Test
 			std::this_thread::sleep_for(std::chrono::milliseconds(1));
 			seconds += 0.001;
 		}
-		EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!";
+		ASSERT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!";
 
 		m = env_ptr->getModelPtr();
 		d = env_ptr->getDataPtr();
@@ -183,6 +160,9 @@ class EvalEnvFixture : public ::testing::Test
 	{
 		env_ptr->shutdown();
 		delete env_ptr;
+
+		// clear all parameters
+		ros::param::del(nh->getNamespace());
 	}
 };