Skip to content

Commit

Permalink
fix: in tests consider ctrl cb running when paused
Browse files Browse the repository at this point in the history
fixes #40
  • Loading branch information
DavidPL1 committed Nov 19, 2024
1 parent 4efd1c0 commit 568a933
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 3 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c
* 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.
* re-added services for getting and setting gravity, that somehow vanished.
* fixed flaky tests that did not consider control callbacks being called in paused mode, too.

### Changed
* Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h).
Expand Down
13 changes: 10 additions & 3 deletions mujoco_ros/test/mujoco_ros_plugin_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,13 +106,19 @@ class LoadedPluginFixture : public ::testing::Test

TEST_F(LoadedPluginFixture, ControlCallback)
{
EXPECT_FALSE(test_plugin->ran_control_cb.load());
// mjcb_control is called in mj_forward, which is also called when paused
// we can't guarantee that the control callback has not been called yet, if the test's
// timing is too slow
// EXPECT_FALSE(test_plugin->ran_control_cb.load());
EXPECT_TRUE(env_ptr->step());
EXPECT_TRUE(test_plugin->ran_control_cb.load());
}

TEST_F(LoadedPluginFixture, PassiveCallback)
{
// mjcb_passive is called in mj_forward, which is also called when paused
// we can't guarantee that the passive callback has not been called yet, if the test's
// timing is too slow
// EXPECT_FALSE(test_plugin->ran_passive_cb.load());
EXPECT_TRUE(env_ptr->step());
EXPECT_TRUE(test_plugin->ran_passive_cb.load());
Expand Down Expand Up @@ -338,8 +344,9 @@ TEST_F(LoadedPluginFixture, PluginStats_InitialPaused)
EXPECT_EQ(srv.response.stats[0].plugin_type, "mujoco_ros/TestPlugin") << "Should be TestPlugin!";
EXPECT_GT(srv.response.stats[0].load_time, -1) << "Load time should be set!";
EXPECT_EQ(srv.response.stats[0].reset_time, -1) << "Reset time should be unset!";
EXPECT_NEAR(srv.response.stats[0].ema_steptime_control, 0, 1e-8) << "Control time should be unset!";
EXPECT_NEAR(srv.response.stats[0].ema_steptime_passive, 0, 1e-8) << "Passive time should be unset!";
// passive and control are also run when paused
EXPECT_NEAR(srv.response.stats[0].ema_steptime_control, 0, 1e-7) << "Control time should be unset!";
EXPECT_NEAR(srv.response.stats[0].ema_steptime_passive, 0, 1e-7) << "Passive time should be unset!";
EXPECT_NEAR(srv.response.stats[0].ema_steptime_render, 0, 1e-8) << "Render time should be unset!";
EXPECT_NEAR(srv.response.stats[0].ema_steptime_last_stage, 0, 1e-8) << "Last stage time should be unset!";
}
Expand Down

0 comments on commit 568a933

Please sign in to comment.