Skip to content

Commit

Permalink
test: fix rgb render test timing
Browse files Browse the repository at this point in the history
  • Loading branch information
@DavidPL1
DavidPL1 committed Nov 28, 2024
1 parent 5fe0b7f commit a111cd7
Showing 1 changed file with 30 additions and 34 deletions.
64 changes: 30 additions & 34 deletions mujoco_ros/test/mujoco_render_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -696,7 +696,7 @@ TEST_F(BaseEnvFixture, SEGMENT_Alternative_StreamName)
env_ptr->shutdown();
}

TEST_F(BaseEnvFixture, RGB_Published_Correctly)
TEST_F(BaseEnvFixture, Cam_Timing_Correct)
{
nh->setParam("no_render", false);
nh->setParam("headless", true);
Expand All @@ -721,6 +721,15 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly)
EXPECT_TRUE(env_ptr->settings_.headless);
EXPECT_TRUE(env_ptr->settings_.render_offscreen);

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

OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext();
ASSERT_EQ(offscreen->cams.size(), 1);

Expand All @@ -729,24 +738,13 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly)
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
float seconds = 0;
while (rgb_images.size() == 0 && seconds < .1) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
seconds += 0.001;
}
EXPECT_LT(seconds, .1) << "RGB image not published within 100ms";

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

ros::Time t1 = ros::Time::now();
EXPECT_EQ(rgb_images[0].header.stamp, t1);
EXPECT_EQ(rgb_images[0].header.frame_id, "test_cam_optical_frame");
EXPECT_STREQ(rgb_images[0].header.frame_id.c_str(), "test_cam_optical_frame");
EXPECT_EQ(rgb_images[0].width, 72);
EXPECT_EQ(rgb_images[0].height, 48);
EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8);
Expand Down Expand Up @@ -785,13 +783,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 +800,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 +825,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 +895,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 +944,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 +993,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 a111cd7

Please sign in to comment.