mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +08:00
rtabmap_sync tests and doc (#1454)
* rtabmap_sync tests and doc * Added some diagrams * cleanup some diagrams * fixing running tests in parallels
This commit is contained in:
matlabbe
@@ -0,0 +1,315 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "common_data_subscriber_fixture.hpp"
|
||||
|
||||
using namespace rtabmap_sync_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
}
|
||||
|
||||
/// The subscribe_* parameters: what they select, and how conflicts between them resolve.
|
||||
///
|
||||
/// CommonDataSubscriber builds one synchronizer out of whichever inputs are asked for,
|
||||
/// and several of the flags describe the same slot in that synchronizer. Rather than
|
||||
/// refusing to start, it drops one of the two and says so in the log. These tests pin
|
||||
/// down which one survives, because that is what decides the topics a user has to remap.
|
||||
class CommonDataSubscriberConfigTest : public CommonDataSubscriberTest {};
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, DefaultsToAnRGBDCameraWithOdometry)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start();
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToDepth());
|
||||
EXPECT_TRUE(sub->isSubscribedToRGB());
|
||||
EXPECT_TRUE(sub->isSubscribedToOdom());
|
||||
EXPECT_FALSE(sub->isSubscribedToStereo());
|
||||
EXPECT_FALSE(sub->isSubscribedToRGBD());
|
||||
EXPECT_FALSE(sub->isSubscribedToSensorData());
|
||||
EXPECT_FALSE(sub->isSubscribedToScan2d());
|
||||
EXPECT_FALSE(sub->isSubscribedToScan3d());
|
||||
EXPECT_FALSE(sub->isSubscribedToOdomInfo());
|
||||
EXPECT_TRUE(sub->isDataSubscribed());
|
||||
EXPECT_STREQ(sub->name().c_str(), "recording_subscriber");
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, TheGuiFlagSubscribesToNothingButOdometry)
|
||||
{
|
||||
// rtabmap_viz passes gui=true: it renders whatever the SLAM node publishes and has
|
||||
// no reason to subscribe to the raw camera topics unless asked.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({}, /*gui=*/true);
|
||||
|
||||
EXPECT_FALSE(sub->isSubscribedToDepth());
|
||||
EXPECT_FALSE(sub->isSubscribedToRGB());
|
||||
EXPECT_TRUE(sub->isSubscribedToOdom());
|
||||
EXPECT_TRUE(sub->isDataSubscribed()) << "odometry alone still counts as data";
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, StereoWinsOverDepth)
|
||||
{
|
||||
// Both describe the camera slot. Stereo is the more specific request, so it stays
|
||||
// and depth -- along with the rgb flag that comes with it -- is dropped.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_depth", true),
|
||||
rclcpp::Parameter("subscribe_stereo", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToStereo());
|
||||
EXPECT_FALSE(sub->isSubscribedToDepth());
|
||||
EXPECT_FALSE(sub->isSubscribedToRGB());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, StereoWinsOverRGB)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", true),
|
||||
rclcpp::Parameter("subscribe_stereo", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToStereo());
|
||||
EXPECT_FALSE(sub->isSubscribedToRGB());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, RGBDWinsOverDepthRGBAndStereo)
|
||||
{
|
||||
// An RGBDImage already carries color, depth and calibration in one message, so it
|
||||
// replaces every other way of describing the camera.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_depth", true),
|
||||
rclcpp::Parameter("subscribe_rgb", true),
|
||||
rclcpp::Parameter("subscribe_stereo", true),
|
||||
rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToRGBD());
|
||||
EXPECT_FALSE(sub->isSubscribedToDepth());
|
||||
EXPECT_FALSE(sub->isSubscribedToRGB());
|
||||
EXPECT_FALSE(sub->isSubscribedToStereo());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverEveryCameraInput)
|
||||
{
|
||||
// A SensorData is a whole RTAB-Map frame, images and scan together; nothing else is
|
||||
// needed alongside it.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_depth", true),
|
||||
rclcpp::Parameter("subscribe_rgb", true),
|
||||
rclcpp::Parameter("subscribe_stereo", true),
|
||||
rclcpp::Parameter("subscribe_sensor_data", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToSensorData());
|
||||
EXPECT_FALSE(sub->isSubscribedToDepth());
|
||||
EXPECT_FALSE(sub->isSubscribedToRGB());
|
||||
EXPECT_FALSE(sub->isSubscribedToStereo());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverRGBD)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("subscribe_sensor_data", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToSensorData());
|
||||
EXPECT_FALSE(sub->isSubscribedToRGBD());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverEveryScanInput)
|
||||
{
|
||||
// The scan travels inside the SensorData, so a separate scan topic would be a second
|
||||
// copy of the same measurement.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_sensor_data", true),
|
||||
rclcpp::Parameter("subscribe_scan", true),
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToSensorData());
|
||||
EXPECT_FALSE(sub->isSubscribedToScan2d());
|
||||
EXPECT_FALSE(sub->isSubscribedToScan3d());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, TheTwoDScanWinsOverTheThreeDOne)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_scan", true),
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToScan2d());
|
||||
EXPECT_FALSE(sub->isSubscribedToScan3d());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, TheScanDescriptorWinsOverBothPlainScans)
|
||||
{
|
||||
// A ScanDescriptor carries the scan plus the global descriptor computed from it, so
|
||||
// it supersedes the plain scan topics rather than sitting beside them.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_scan", true),
|
||||
rclcpp::Parameter("subscribe_scan_descriptor", true)});
|
||||
EXPECT_FALSE(sub->isSubscribedToScan2d());
|
||||
|
||||
std::shared_ptr<RecordingSubscriber> other = addNode(
|
||||
std::make_shared<RecordingSubscriber>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true),
|
||||
rclcpp::Parameter("subscribe_scan_descriptor", true)})));
|
||||
EXPECT_FALSE(other->isSubscribedToScan3d());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, AnOdomFrameIdReplacesTheOdometryTopic)
|
||||
{
|
||||
// With odom_frame_id set, the pose is read from TF instead. Leaving the topic
|
||||
// subscribed as well would stall the synchronizer on a topic nobody publishes.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_odom", true),
|
||||
rclcpp::Parameter("odom_frame_id", "odom")});
|
||||
|
||||
EXPECT_FALSE(sub->isSubscribedToOdom());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, CamerasDefaultToApproximateSync)
|
||||
{
|
||||
// Color and depth come off the sensor at slightly different instants.
|
||||
EXPECT_TRUE(start()->isApproxSync());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, StereoDefaultsToExactSync)
|
||||
{
|
||||
// A stereo pair is hardware-triggered, so the two frames share a stamp exactly.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_stereo", true)});
|
||||
|
||||
EXPECT_FALSE(sub->isApproxSync());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, AScanOnlyPipelineDefaultsToExactSync)
|
||||
{
|
||||
// With no camera in the picture the remaining inputs are the scan and the odometry
|
||||
// computed from it, which carries the scan's own stamp.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
EXPECT_FALSE(sub->isApproxSync());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, AScanNextToACameraKeepsApproximateSync)
|
||||
{
|
||||
// The exact default only applies when the scan is alone; a camera in the set puts
|
||||
// the default back to approximate.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToDepth());
|
||||
EXPECT_TRUE(sub->isApproxSync());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, ApproxSyncOverridesTheDefault)
|
||||
{
|
||||
// The parameter is declared after the defaults are worked out, so an explicit value
|
||||
// wins in both directions.
|
||||
EXPECT_FALSE(start({rclcpp::Parameter("approx_sync", false)})->isApproxSync());
|
||||
|
||||
std::shared_ptr<RecordingSubscriber> stereo = addNode(
|
||||
std::make_shared<RecordingSubscriber>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("subscribe_stereo", true),
|
||||
rclcpp::Parameter("approx_sync", true)})));
|
||||
EXPECT_TRUE(stereo->isApproxSync());
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, ReportsTheConfiguredQueueSizes)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("topic_queue_size", 3),
|
||||
rclcpp::Parameter("sync_queue_size", 7)});
|
||||
|
||||
EXPECT_EQ(sub->getTopicQueueSize(), 3);
|
||||
EXPECT_EQ(sub->getSyncQueueSize(), 7);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, TheDeprecatedQueueSizeFeedsSyncQueueSize)
|
||||
{
|
||||
// "queue_size" was split into the two above; the old name still has to work.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("queue_size", 4)});
|
||||
|
||||
EXPECT_EQ(sub->getSyncQueueSize(), 4);
|
||||
EXPECT_EQ(sub->getTopicQueueSize(), 10) << "the topic queue keeps its own default";
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, SyncQueueSizeWinsOverTheDeprecatedName)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("queue_size", 4),
|
||||
rclcpp::Parameter("sync_queue_size", 9)});
|
||||
|
||||
EXPECT_EQ(sub->getSyncQueueSize(), 9);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, CountsOneRGBDCamera)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
|
||||
EXPECT_EQ(sub->rgbdCameras(), 1);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, ReportsNoRGBDCamerasOnTheRGBDImagesInterface)
|
||||
{
|
||||
// rgbd_cameras=0 switches to the single RGBDImages topic, whose camera count is only
|
||||
// known per message -- so there is no fixed number to report.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", 0)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToRGBD());
|
||||
EXPECT_EQ(sub->rgbdCameras(), 0);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, ReportsNoRGBDCamerasWhenNotSubscribedToRGBD)
|
||||
{
|
||||
EXPECT_EQ(start()->rgbdCameras(), 0);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberConfigTest, NothingIsSubscribedWhenEveryInputIsOff)
|
||||
{
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
EXPECT_FALSE(sub->isDataSubscribed());
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_TRUE(sub->empty());
|
||||
}
|
||||
|
||||
#ifndef RTABMAP_SYNC_USER_DATA
|
||||
TEST_F(CommonDataSubscriberConfigTest, UserDataIsRefusedUnlessBuiltIn)
|
||||
{
|
||||
// The user-data synchronizers are behind a build option, because they double the
|
||||
// number of synchronizer templates the package has to compile.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_user_data", true)});
|
||||
|
||||
EXPECT_TRUE(sub->isSubscribedToDepth()) << "the rest of the setup must still happen";
|
||||
spinFor(std::chrono::milliseconds(100));
|
||||
EXPECT_EQ(helper()->count_publishers("/user_data"), 0u);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef RTABMAP_SYNC_MULTI_RGBD
|
||||
TEST_F(CommonDataSubscriberConfigTest, MoreThanOneRGBDCameraIsRefusedUnlessBuiltIn)
|
||||
{
|
||||
// Synchronizing several RGBDImage topics is behind a build option for the same
|
||||
// reason. Without it, nothing is subscribed -- rgbd_cameras=0 is the way out.
|
||||
std::shared_ptr<RecordingSubscriber> sub = start({
|
||||
rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", 2)});
|
||||
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_EQ(helper()->count_subscribers("/rgbd_image0"), 0u);
|
||||
EXPECT_EQ(helper()->count_subscribers("/rgbd_image"), 0u);
|
||||
EXPECT_TRUE(sub->empty());
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user