From 1404187958975d1e9d098f98fa823fb8935bc2b2 Mon Sep 17 00:00:00 2001 From: David Leins Date: Thu, 5 Dec 2024 16:45:42 +0100 Subject: [PATCH] test: fix tests for colcon --- mujoco_ros/test/CMakeLists.txt | 1 + mujoco_ros/test/mujoco_env_fixture.h | 16 ++++-- mujoco_ros/test/mujoco_env_test.cpp | 28 ++++++++--- mujoco_ros/test/mujoco_ros_plugin_test.cpp | 12 ++++- mujoco_ros/test/ros_interface_test.cpp | 1 + mujoco_ros/test/test_util.h | 11 ---- mujoco_ros_sensors/test/mujoco_env_fixture.h | 1 + .../test/mujoco_sensors_test.cpp | 50 ++++++------------- 8 files changed, 62 insertions(+), 58 deletions(-) create mode 120000 mujoco_ros_sensors/test/mujoco_env_fixture.h diff --git a/mujoco_ros/test/CMakeLists.txt b/mujoco_ros/test/CMakeLists.txt index 6808f71..45839aa 100644 --- a/mujoco_ros/test/CMakeLists.txt +++ b/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 ) diff --git a/mujoco_ros/test/mujoco_env_fixture.h b/mujoco_ros/test/mujoco_env_fixture.h index 6ce3c87..ee49c94 100644 --- a/mujoco_ros/test/mujoco_env_fixture.h +++ b/mujoco_ros/test/mujoco_env_fixture.h @@ -40,7 +40,6 @@ #include #include #include -#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 { diff --git a/mujoco_ros/test/mujoco_env_test.cpp b/mujoco_ros/test/mujoco_env_test.cpp index a42a050..241adb5 100644 --- a/mujoco_ros/test/mujoco_env_test.cpp +++ b/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(""), std::runtime_error); + EXPECT_THROW(env_ptr = std::make_unique(), std::runtime_error); + ROS_WARN("###### [END] EvalModeWithoutHashThrow ######"); } TEST_F(BaseEnvFixture, RunEvalMode) @@ -437,7 +453,7 @@ 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!"; @@ -445,9 +461,7 @@ TEST_F(BaseEnvFixture, Reload) // 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 diff --git a/mujoco_ros/test/mujoco_ros_plugin_test.cpp b/mujoco_ros/test/mujoco_ros_plugin_test.cpp index ba3e468..e8a2298 100644 --- a/mujoco_ros/test/mujoco_ros_plugin_test.cpp +++ b/mujoco_ros/test/mujoco_ros_plugin_test.cpp @@ -67,7 +67,7 @@ class LoadedPluginFixture : public ::testing::Test { protected: std::unique_ptr 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) { diff --git a/mujoco_ros/test/ros_interface_test.cpp b/mujoco_ros/test/ros_interface_test.cpp index fd8ef04..8c69cbf 100644 --- a/mujoco_ros/test/ros_interface_test.cpp +++ b/mujoco_ros/test/ros_interface_test.cpp @@ -50,6 +50,7 @@ #include #include "mujoco_env_fixture.h" +#include "test_util.h" #include diff --git a/mujoco_ros/test/test_util.h b/mujoco_ros/test/test_util.h index f337c0b..7693e66 100644 --- a/mujoco_ros/test/test_util.h +++ b/mujoco_ros/test/test_util.h @@ -39,17 +39,6 @@ #include 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 &values, const std::vector &tolerances = {}) { diff --git a/mujoco_ros_sensors/test/mujoco_env_fixture.h b/mujoco_ros_sensors/test/mujoco_env_fixture.h new file mode 120000 index 0000000..9c5e487 --- /dev/null +++ b/mujoco_ros_sensors/test/mujoco_env_fixture.h @@ -0,0 +1 @@ +../../mujoco_ros/test/mujoco_env_fixture.h \ No newline at end of file diff --git a/mujoco_ros_sensors/test/mujoco_sensors_test.cpp b/mujoco_ros_sensors/test/mujoco_sensors_test.cpp index 49362ba..2780e22 100644 --- a/mujoco_ros_sensors/test/mujoco_sensors_test.cpp +++ b/mujoco_ros_sensors/test/mujoco_sensors_test.cpp @@ -37,6 +37,7 @@ #include #include +#include "mujoco_env_fixture.h" #include #include #include @@ -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("~"); 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("~"); 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()); } };