Skip to content

Commit

Permalink
test: fix rgb render test timing
Browse files Browse the repository at this point in the history 
For EGL 100ms suffices, OSMesa local manages 200ms
  • Loading branch information
@DavidPL1
DavidPL1 committed Nov 28, 2024
1 parent 5fe0b7f commit f1e22de
Showing 1 changed file with 28 additions and 31 deletions.
59 changes: 28 additions & 31 deletions mujoco_ros/test/mujoco_render_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -701,13 +701,9 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly)
nh->setParam("no_render", false);
nh->setParam("headless", true);
nh->setParam("unpause", false);
nh->setParam("cam_config/test_cam/frequency", 30);
nh->setParam("cam_config/test_cam/width", 72);
nh->setParam("cam_config/test_cam/height", 48);

std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml";
env_ptr = std::make_unique<MujocoEnvTestWrapper>("");

// Clear image buffers
rgb_images.clear();
rgb_infos.clear();
Expand All @@ -716,8 +712,13 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly)
ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb/image_raw", 1, rgb_cb);
ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/rgb/camera_info", 1, rgb_info_cb);

std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml";
env_ptr = std::make_unique<MujocoEnvTestWrapper>("");

env_ptr->startWithXML(xml_path);

env_ptr->step(1);

EXPECT_TRUE(env_ptr->settings_.headless);
EXPECT_TRUE(env_ptr->settings_.render_offscreen);

Expand All @@ -728,21 +729,19 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly)
EXPECT_EQ(offscreen->cams[0]->cam_id_, 0);
EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam");
EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB);
EXPECT_EQ(offscreen->cams[0]->pub_freq_, 30);
EXPECT_EQ(offscreen->cams[0]->width_, 72);
EXPECT_EQ(offscreen->cams[0]->height_, 48);

env_ptr->step(1);
// Wait for image to be published with 100ms timeout
// Wait for image to be published with 200ms timeout
float seconds = 0;
while (rgb_images.size() == 0 && seconds < .1) {
while (rgb_images.size() == 0 && seconds < .2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
EXPECT_LT(seconds, .1) << "RGB image not published within 100ms";
EXPECT_LT(seconds, .2) << "RGB image not published within 200ms";

ASSERT_EQ(rgb_infos.size(), 1);
ASSERT_EQ(rgb_images.size(), 1);
ASSERT_EQ(rgb_infos.size(), 1);

ros::Time t1 = ros::Time::now();
EXPECT_EQ(rgb_images[0].header.stamp, t1);
Expand Down Expand Up @@ -785,13 +784,13 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct)
EXPECT_TRUE(env_ptr->settings_.render_offscreen);

env_ptr->step(1);
// Wait for image to be published with 100ms timeout
// Wait for image to be published with 200ms timeout
float seconds = 0;
while (rgb_images.size() == 0 && seconds < .1) {
while (rgb_images.size() == 0 && seconds < .2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
EXPECT_LT(seconds, .1) << "RGB image not published within 100ms";
EXPECT_LT(seconds, .2) << "RGB image not published within 200ms";

OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext();
ASSERT_EQ(offscreen->cams.size(), 1);
Expand All @@ -802,22 +801,20 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct)
EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB);
EXPECT_EQ(offscreen->cams[0]->pub_freq_, 30);

// Wait for image to be published with 100ms timeout
// Wait for image to be published with 200ms timeout
seconds = 0;
while (rgb_images.size() == 0 && seconds < 0.1) {
while (rgb_images.size() == 0 && seconds < 0.2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms";
EXPECT_LT(seconds, 0.2) << "RGB image not published within 200ms";

ASSERT_EQ(rgb_infos.size(), 1);
ASSERT_EQ(rgb_images.size(), 1);

ros::Time t1 = ros::Time::now();
// Step the simulation to as to trigger the camera rendering
mjModel *m = env_ptr->getModelPtr();
ROS_INFO_STREAM("Next pub time will be " << (1.0 / 30.0) << " seconds from now. Thus in "
<< std::ceil((1.0 / 30.0) / m->opt.timestep) << " steps.");
mjModel *m = env_ptr->getModelPtr();
int n_steps = std::ceil((1.0 / 30.0) / env_ptr->getModelPtr()->opt.timestep);

env_ptr->step(n_steps - 1);
Expand All @@ -829,14 +826,14 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct)
ASSERT_EQ(rgb_images.size(), 1);

env_ptr->step(1);
// Wait for image to be published with 100ms timeout
// Wait for image to be published with 200ms timeout
seconds = 0;
while (rgb_images.size() < 2 && seconds < 0.1) {
while (rgb_images.size() < 2 && seconds < 0.2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
// should now have received new image
EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms";
EXPECT_LT(seconds, 0.2) << "RGB image not published within 200ms";

ASSERT_EQ(rgb_infos.size(), 2);
ASSERT_EQ(rgb_images.size(), 2);
Expand Down Expand Up @@ -899,13 +896,13 @@ TEST_F(BaseEnvFixture, RGB_Image_Dtype)
EXPECT_EQ(offscreen->cams[0]->width_, 72);
EXPECT_EQ(offscreen->cams[0]->height_, 48);

// Wait for image to be published with 100ms timeout
// Wait for image to be published with 200ms timeout
float seconds = 0;
while (rgb_images.size() == 0 && seconds < .1) {
while (rgb_images.size() == 0 && seconds < .2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
EXPECT_LT(seconds, .1) << "RGB image not published within 100ms";
EXPECT_LT(seconds, .2) << "RGB image not published within 200ms";

ASSERT_EQ(rgb_images.size(), 1);
EXPECT_EQ(rgb_images[0].data.size(), 72 * 48 * 3);
Expand Down Expand Up @@ -948,13 +945,13 @@ TEST_F(BaseEnvFixture, DEPTH_Image_Dtype)
EXPECT_EQ(offscreen->cams[0]->width_, 72);
EXPECT_EQ(offscreen->cams[0]->height_, 48);

// Wait for image to be published with 100ms timeout
// Wait for image to be published with 200ms timeout
float seconds = 0;
while (depth_images.size() == 0 && seconds < .1) {
while (depth_images.size() == 0 && seconds < .2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
EXPECT_LT(seconds, .1) << "Depth image not published within 100ms";
EXPECT_LT(seconds, .2) << "Depth image not published within 200ms";

ASSERT_EQ(depth_images.size(), 1);
EXPECT_EQ(depth_images[0].width, 72);
Expand Down Expand Up @@ -997,13 +994,13 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype)
EXPECT_EQ(offscreen->cams[0]->width_, 72);
EXPECT_EQ(offscreen->cams[0]->height_, 48);

// Wait for image to be published with 100ms timeout
// Wait for image to be published with 200ms timeout
float seconds = 0;
while (seg_images.size() == 0 && seconds < .1) {
while (seg_images.size() == 0 && seconds < .2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
EXPECT_LT(seconds, .1) << "Segmentation image not published within 100ms";
EXPECT_LT(seconds, .2) << "Segmentation image not published within 200ms";

ASSERT_EQ(seg_images.size(), 1);
EXPECT_EQ(seg_images[0].data.size(), 72 * 48 * 3);
Expand Down

0 comments on commit f1e22de

Please sign in to comment.