From 5fe0b7f4504274a03c2e73d138d36269b9daa1e0 Mon Sep 17 00:00:00 2001 From: David Leins Date: Thu, 28 Nov 2024 17:47:39 +0100 Subject: [PATCH] test: adapt render tests to additional params --- mujoco_ros/test/mujoco_render_test.cpp | 373 ++++++++++++++++++++----- 1 file changed, 308 insertions(+), 65 deletions(-) diff --git a/mujoco_ros/test/mujoco_render_test.cpp b/mujoco_ros/test/mujoco_render_test.cpp index 1324a2e..41558e2 100644 --- a/mujoco_ros/test/mujoco_render_test.cpp +++ b/mujoco_ros/test/mujoco_render_test.cpp @@ -76,9 +76,13 @@ using namespace mujoco_ros; namespace mju = ::mujoco::sample_util; std::vector rgb_images; +std::vector rgb_infos; + std::vector depth_images; +// std::vector depth_infos; + std::vector seg_images; -std::vector cam_infos; +// std::vector seg_infos; // callbacks to save an image in a buffer void rgb_cb(const sensor_msgs::Image::ConstPtr &msg) @@ -93,10 +97,18 @@ void seg_cb(const sensor_msgs::Image::ConstPtr &msg) { seg_images.emplace_back(*msg); } -void cam_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) +void rgb_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) { - cam_infos.emplace_back(*msg); + rgb_infos.emplace_back(*msg); } +// void depth_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) +// { +// depth_infos.emplace_back(*msg); +// } +// void seg_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) +// { +// seg_infos.emplace_back(*msg); +// } TEST_F(BaseEnvFixture, Not_Headless_Warn) { @@ -148,7 +160,7 @@ TEST_F(BaseEnvFixture, Headless_params_correct) env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_Topic_Available) +TEST_F(BaseEnvFixture, RGB_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -170,19 +182,22 @@ TEST_F(BaseEnvFixture, RGB_Topic_Available) ros::master::V_TopicInfo master_topics; ros::master::getTopics(master_topics); - bool found = false; + bool img = false, info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { - found = true; - break; + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + info = true; } + if (img && info) + break; } - EXPECT_TRUE(found); + EXPECT_TRUE(img && info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, DEPTH_Topic_Available) +TEST_F(BaseEnvFixture, DEPTH_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -204,19 +219,22 @@ TEST_F(BaseEnvFixture, DEPTH_Topic_Available) ros::master::V_TopicInfo master_topics; ros::master::getTopics(master_topics); - bool found = false; + bool img = false, info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { - found = true; - break; + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + info = true; } + if (img && info) + break; } - EXPECT_TRUE(found); + EXPECT_TRUE(img && info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -238,18 +256,21 @@ TEST_F(BaseEnvFixture, SEGMENTATION_Topic_Available) ros::master::V_TopicInfo master_topics; ros::master::getTopics(master_topics); - bool found = false; + bool img = false, info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { - found = true; - break; + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + info = true; } + if (img && info) + break; } - EXPECT_TRUE(found); + EXPECT_TRUE(img && info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_DEPTH_Topic_Available) +TEST_F(BaseEnvFixture, RGB_DEPTH_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -272,21 +293,26 @@ TEST_F(BaseEnvFixture, RGB_DEPTH_Topic_Available) ros::master::getTopics(master_topics); bool found_rgb = false, found_depth = false; + bool found_rgb_info = false, found_depth_info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { found_rgb = true; - } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + found_depth_info = true; } - if (found_rgb && found_depth) + if (found_rgb && found_depth && found_rgb_info && found_depth_info) break; } - EXPECT_TRUE(found_rgb && found_depth); + EXPECT_TRUE(found_rgb && found_depth && found_rgb_info && found_depth_info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -309,21 +335,26 @@ TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topic_Available) ros::master::getTopics(master_topics); bool found_rgb = false, found_seg = false; + bool found_rgb_info = false, found_seg_info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { found_rgb = true; - } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + found_seg_info = true; } - if (found_rgb && found_seg) + if (found_rgb && found_seg && found_rgb_info && found_seg_info) break; } - EXPECT_TRUE(found_rgb && found_seg); + EXPECT_TRUE(found_rgb && found_seg && found_rgb_info && found_seg_info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -346,20 +377,25 @@ TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topic_Available) ros::master::getTopics(master_topics); bool found_depth = false, found_seg = false; + bool found_depth_info = false, found_seg_info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { found_depth = true; - } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + found_depth_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + found_seg_info = true; } - if (found_depth && found_seg) + if (found_depth && found_seg && found_depth_info && found_seg_info) break; } - EXPECT_TRUE(found_depth && found_seg); + EXPECT_TRUE(found_depth && found_seg && found_depth_info && found_seg_info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -382,19 +418,25 @@ TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topic_Available) ros::master::getTopics(master_topics); bool found_rgb = false, found_depth = false, found_seg = false; + bool found_rgb_info = false, found_depth_info = false, found_seg_info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { found_rgb = true; - } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { found_depth = true; - } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + found_depth_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + found_seg_info = true; } - if (found_rgb && found_depth && found_seg) + if (found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info) break; } - EXPECT_TRUE(found_rgb && found_depth && found_seg); - env_ptr->shutdown(); + EXPECT_TRUE(found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info); env_ptr->shutdown(); } @@ -454,6 +496,206 @@ TEST_F(BaseEnvFixture, Resolution_Settings) env_ptr->shutdown(); } +TEST_F(BaseEnvFixture, Stream_BaseTopic_Relative) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB_D_S); + nh->setParam("cam_config/test_cam/topic", "alt_topic"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_STREQ(offscreen->cams[0]->topic_.c_str(), "alt_topic"); + + bool found_rgb = false, found_depth = false, found_seg = false; + bool found_rgb_info = false, found_depth_info = false, found_seg_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/rgb/image_raw") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/depth/image_raw") { + found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/segmented/image_raw") { + found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/depth/camera_info") { + found_depth_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/segmented/camera_info") { + found_seg_info = true; + } + if (found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info) + break; + } + EXPECT_TRUE(found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, Stream_BaseTopic_Absolute) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB_D_S); + nh->setParam("cam_config/test_cam/topic", "/alt_topic"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_STREQ(offscreen->cams[0]->topic_.c_str(), "/alt_topic"); + + bool found_rgb = false, found_depth = false, found_seg = false; + bool found_rgb_info = false, found_depth_info = false, found_seg_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == "/alt_topic/rgb/image_raw") { + found_rgb = true; + } else if (t.name == "/alt_topic/depth/image_raw") { + found_depth = true; + } else if (t.name == "/alt_topic/segmented/image_raw") { + found_seg = true; + } else if (t.name == "/alt_topic/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == "/alt_topic/depth/camera_info") { + found_depth_info = true; + } else if (t.name == "/alt_topic/segmented/camera_info") { + found_seg_info = true; + } + if (found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info) + break; + } + EXPECT_TRUE(found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_Alternative_StreamName) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB); + nh->setParam("cam_config/test_cam/name_rgb", "alt_rgb"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + + bool img = false, found_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_rgb/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_rgb/camera_info") { + found_info = true; + } + if (img && found_info) + break; + } + EXPECT_TRUE(img && found_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, DEPTH_Alternative_StreamName) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::DEPTH); + nh->setParam("cam_config/test_cam/name_depth", "alt_depth"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + + bool img = false, found_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_depth/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_depth/camera_info") { + found_info = true; + } + if (img && found_info) + break; + } + EXPECT_TRUE(img && found_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, SEGMENT_Alternative_StreamName) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::SEGMENTED); + nh->setParam("cam_config/test_cam/name_segment", "alt_seg"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + + bool img = false, found_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_seg/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_seg/camera_info") { + found_info = true; + } + if (img && found_info) + break; + } + EXPECT_TRUE(img && found_info); + + env_ptr->shutdown(); +} + TEST_F(BaseEnvFixture, RGB_Published_Correctly) { nh->setParam("no_render", false); @@ -468,11 +710,11 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly) // Clear image buffers rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); // Subscribe to topic - ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); - ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/camera_info", 1, cam_info_cb); + 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); env_ptr->startWithXML(xml_path); @@ -499,7 +741,7 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly) } EXPECT_LT(seconds, .1) << "RGB image not published within 100ms"; - ASSERT_EQ(cam_infos.size(), 1); + ASSERT_EQ(rgb_infos.size(), 1); ASSERT_EQ(rgb_images.size(), 1); ros::Time t1 = ros::Time::now(); @@ -509,10 +751,10 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly) EXPECT_EQ(rgb_images[0].height, 48); EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8); - EXPECT_EQ(cam_infos[0].header.stamp, t1); + EXPECT_EQ(rgb_infos[0].header.stamp, t1); rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); env_ptr->shutdown(); } @@ -531,11 +773,11 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) // Clear image buffers rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); // Subscribe to topic - ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); - ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/camera_info", 1, cam_info_cb); + 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); env_ptr->startWithXML(xml_path); @@ -568,7 +810,7 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) } EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms"; - ASSERT_EQ(cam_infos.size(), 1); + ASSERT_EQ(rgb_infos.size(), 1); ASSERT_EQ(rgb_images.size(), 1); ros::Time t1 = ros::Time::now(); @@ -583,7 +825,7 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) std::this_thread::sleep_for(std::chrono::milliseconds(5)); // should not have received a new image yet - ASSERT_EQ(cam_infos.size(), 1); + ASSERT_EQ(rgb_infos.size(), 1); ASSERT_EQ(rgb_images.size(), 1); env_ptr->step(1); @@ -596,15 +838,15 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) // should now have received new image EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms"; - ASSERT_EQ(cam_infos.size(), 2); + ASSERT_EQ(rgb_infos.size(), 2); ASSERT_EQ(rgb_images.size(), 2); ros::Time t2 = ros::Time::now(); ASSERT_EQ(rgb_images[0].header.stamp, t1); ASSERT_EQ(rgb_images[1].header.stamp, t2); - ASSERT_EQ(cam_infos[0].header.stamp, t1); - ASSERT_EQ(cam_infos[1].header.stamp, t2); + ASSERT_EQ(rgb_infos[0].header.stamp, t1); + ASSERT_EQ(rgb_infos[1].header.stamp, t2); // int n_steps = std::ceil((1.0 / 30.0) / env_ptr->getModelPtr()->opt.timestep); ros::Time t3 = t2 + ros::Duration((std::ceil((1.0 / 30.0) / m->opt.timestep)) * m->opt.timestep); @@ -612,16 +854,16 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) // Step over next image trigger but before trigger after that env_ptr->step(2 * n_steps - 1); - ASSERT_EQ(cam_infos.size(), 3); + ASSERT_EQ(rgb_infos.size(), 3); ASSERT_EQ(rgb_images.size(), 3); // Check that the timestamps are as expected - EXPECT_EQ(rgb_images[2].header.stamp, cam_infos[2].header.stamp); + EXPECT_EQ(rgb_images[2].header.stamp, rgb_infos[2].header.stamp); EXPECT_EQ(rgb_images[2].header.stamp, t3); - EXPECT_EQ(cam_infos[2].header.stamp, t3); + EXPECT_EQ(rgb_infos[2].header.stamp, t3); rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); env_ptr->shutdown(); } @@ -641,7 +883,7 @@ TEST_F(BaseEnvFixture, RGB_Image_Dtype) rgb_images.clear(); // Subscribe to topic - ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb/image_raw", 1, rgb_cb); env_ptr->startWithXML(xml_path); @@ -690,7 +932,7 @@ TEST_F(BaseEnvFixture, DEPTH_Image_Dtype) depth_images.clear(); // Subscribe to topic - ros::Subscriber depth_sub = nh->subscribe("cameras/test_cam/depth", 1, depth_cb); + ros::Subscriber depth_sub = nh->subscribe("cameras/test_cam/depth/image_raw", 1, depth_cb); env_ptr->startWithXML(xml_path); @@ -739,7 +981,7 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) seg_images.clear(); // Subscribe to topic - ros::Subscriber seg_sub = nh->subscribe("cameras/test_cam/segmented", 1, seg_cb); + ros::Subscriber seg_sub = nh->subscribe("cameras/test_cam/segmented/image_raw", 1, seg_cb); env_ptr->startWithXML(xml_path); @@ -764,7 +1006,8 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) EXPECT_LT(seconds, .1) << "Segmentation image not published within 100ms"; ASSERT_EQ(seg_images.size(), 1); - EXPECT_EQ(seg_images[0].data.size(), 72 * 48 * 1); + EXPECT_EQ(seg_images[0].data.size(), 72 * 48 * 3); + EXPECT_EQ(seg_images[0].encoding, sensor_msgs::image_encodings::RGB8); seg_images.clear(); env_ptr->shutdown();