mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 01:07:49 +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,540 @@
|
||||
/*
|
||||
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"
|
||||
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
using namespace rtabmap_sync_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
}
|
||||
|
||||
/// End-to-end: real messages in on the topics each mode subscribes to, one callback out.
|
||||
///
|
||||
/// Every set below is published with identical stamps, so the result does not depend on
|
||||
/// which sync policy the mode defaults to. What each test pins down is the wiring: which
|
||||
/// topics a given combination of subscribe_* flags listens on, which of the four
|
||||
/// callbacks fires, and which slots of it are filled.
|
||||
class CommonDataSubscriberSyncTest : public CommonDataSubscriberTest {};
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, DepthModeDeliversOneCameraToTheMultiCameraCallback)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
const RecordingSubscriber::Record & got = sub_->back();
|
||||
EXPECT_EQ(got.kind, RecordingSubscriber::Record::kMultiCamera);
|
||||
EXPECT_EQ(got.images, 1u);
|
||||
EXPECT_EQ(got.depths, 1u);
|
||||
EXPECT_EQ(got.cameraInfos, 1u);
|
||||
EXPECT_EQ(got.frameId, "camera_link");
|
||||
EXPECT_DOUBLE_EQ(got.stamp, 1000.0);
|
||||
EXPECT_FALSE(got.hasOdom);
|
||||
EXPECT_FALSE(got.hasOdomInfo);
|
||||
EXPECT_FALSE(got.hasScan2d);
|
||||
EXPECT_FALSE(got.hasScan3d);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, DepthModeWithOdometryWaitsForThePose)
|
||||
{
|
||||
start(); // subscribe_odom defaults to true
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
|
||||
advertise<nav_msgs::msg::Odometry>("odom");
|
||||
|
||||
// The camera alone is not a complete set.
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_TRUE(sub_->empty()) << "without the pose the frame cannot be placed in the map";
|
||||
|
||||
odom->publish(makeOdometry("odom", 1000.0, 1.5));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
EXPECT_TRUE(sub_->back().hasOdom);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, DepthModeCanAlsoTakeTheOdometryInfo)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_odom_info", true)});
|
||||
EXPECT_TRUE(sub_->isSubscribedToOdomInfo());
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfo =
|
||||
advertise<rtabmap_msgs::msg::OdomInfo>("odom_info");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
odomInfo->publish(makeOdomInfo("odom", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(sub_->back().hasOdomInfo);
|
||||
EXPECT_FALSE(sub_->back().hasOdom) << "the info is not the pose";
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, DepthModeCarriesATwoDScanAlongside)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_scan", true)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
|
||||
advertise<sensor_msgs::msg::LaserScan>("scan");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
scan->publish(makeLaserScan("base_scan", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_EQ(sub_->back().images, 1u);
|
||||
EXPECT_TRUE(sub_->back().hasScan2d);
|
||||
EXPECT_FALSE(sub_->back().hasScan3d);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, DepthModeCarriesAThreeDScanAlongside)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
|
||||
advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
cloud->publish(makeScanCloud("lidar_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(sub_->back().hasScan3d);
|
||||
EXPECT_FALSE(sub_->back().hasScan2d);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, AScanDescriptorIsUnpackedIntoScanAndDescriptor)
|
||||
{
|
||||
// The descriptor topic replaces the scan topic and carries the scan inside it, plus
|
||||
// the global descriptor computed from that same scan.
|
||||
start({rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_scan_descriptor", true)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::ScanDescriptor>::SharedPtr descriptor =
|
||||
advertise<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
descriptor->publish(makeScanDescriptor("base_scan", 1000.0,
|
||||
/*with2d=*/true, /*with3d=*/false, /*withGlobalDescriptor=*/true));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(sub_->back().hasScan2d) << "the scan inside the descriptor must be used";
|
||||
EXPECT_EQ(sub_->back().globalDescriptors, 1u);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, AnEmptyGlobalDescriptorIsNotForwarded)
|
||||
{
|
||||
// An empty descriptor is "none computed", not a descriptor of length zero.
|
||||
start({rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_scan_descriptor", true)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::ScanDescriptor>::SharedPtr descriptor =
|
||||
advertise<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
descriptor->publish(makeScanDescriptor("base_scan", 1000.0,
|
||||
/*with2d=*/true, /*with3d=*/false, /*withGlobalDescriptor=*/false));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(sub_->back().hasScan2d);
|
||||
EXPECT_EQ(sub_->back().globalDescriptors, 0u);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, RGBModeDeliversNoDepth)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", true),
|
||||
rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_EQ(sub_->back().images, 1u);
|
||||
EXPECT_EQ(sub_->back().depths, 0u)
|
||||
<< "an empty depth vector is how the callback learns there is no depth";
|
||||
EXPECT_EQ(sub_->back().cameraInfos, 1u);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, StereoModeDeliversTheRightImageInTheDepthSlot)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_stereo", true),
|
||||
rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr left =
|
||||
advertise<sensor_msgs::msg::Image>("left/image_rect");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr right =
|
||||
advertise<sensor_msgs::msg::Image>("right/image_rect");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfo =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("left/camera_info");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfo =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("right/camera_info");
|
||||
|
||||
left->publish(makeMonoImage("left_frame", 1000.0));
|
||||
right->publish(makeMonoImage("left_frame", 1000.0));
|
||||
leftInfo->publish(makeCameraInfo("left_frame", 1000.0));
|
||||
rightInfo->publish(makeCameraInfo("left_frame", 1000.0, 8, 8, /*tx=*/-12.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_EQ(sub_->back().images, 1u);
|
||||
EXPECT_EQ(sub_->back().depths, 1u);
|
||||
EXPECT_EQ(sub_->back().frameId, "left_frame");
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, RGBDModeUnpacksTheMessageIntoImages)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
|
||||
advertise<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
|
||||
|
||||
rgbd->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
const RecordingSubscriber::Record & got = sub_->back();
|
||||
EXPECT_EQ(got.kind, RecordingSubscriber::Record::kMultiCamera);
|
||||
EXPECT_EQ(got.images, 1u);
|
||||
EXPECT_EQ(got.depths, 1u);
|
||||
EXPECT_EQ(got.cameraInfos, 1u);
|
||||
EXPECT_EQ(got.frameId, "camera_link");
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, RGBDModeCarriesAScanAlongside)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
|
||||
advertise<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
|
||||
advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
|
||||
|
||||
rgbd->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
cloud->publish(makeScanCloud("lidar_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_EQ(sub_->back().images, 1u);
|
||||
EXPECT_TRUE(sub_->back().hasScan3d);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, TheRGBDImagesInterfaceDeliversEveryCamera)
|
||||
{
|
||||
// rgbd_cameras=0 takes a pre-grouped RGBDImages -- what rgbdx_sync publishes -- so
|
||||
// any number of cameras works without the multi-RGBD build option.
|
||||
start({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", 0),
|
||||
rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdx =
|
||||
advertise<rtabmap_msgs::msg::RGBDImages>("rgbd_images");
|
||||
|
||||
rtabmap_msgs::msg::RGBDImages msg;
|
||||
msg.header.frame_id = "camera0_link";
|
||||
msg.header.stamp = stampOf(1000.0);
|
||||
msg.rgbd_images.push_back(makeRGBDImage("camera0_link", 1000.0));
|
||||
msg.rgbd_images.push_back(makeRGBDImage("camera1_link", 1000.0));
|
||||
msg.rgbd_images.push_back(makeRGBDImage("camera2_link", 1000.0));
|
||||
rgbdx->publish(msg);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
const RecordingSubscriber::Record & got = sub_->back();
|
||||
EXPECT_EQ(got.images, 3u);
|
||||
EXPECT_EQ(got.depths, 3u);
|
||||
EXPECT_EQ(got.cameraInfos, 3u);
|
||||
EXPECT_EQ(got.frameId, "camera0_link");
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, ATwoDScanAloneGoesToTheLaserScanCallback)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_scan", true)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
|
||||
advertise<sensor_msgs::msg::LaserScan>("scan");
|
||||
|
||||
scan->publish(makeLaserScan("base_scan", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
const RecordingSubscriber::Record & got = sub_->back();
|
||||
EXPECT_EQ(got.kind, RecordingSubscriber::Record::kLaserScan);
|
||||
EXPECT_TRUE(got.hasScan2d);
|
||||
EXPECT_FALSE(got.hasScan3d);
|
||||
EXPECT_EQ(got.frameId, "base_scan");
|
||||
EXPECT_DOUBLE_EQ(got.stamp, 1000.0);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, AThreeDScanAloneGoesToTheLaserScanCallback)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
|
||||
advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
|
||||
|
||||
cloud->publish(makeScanCloud("lidar_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
const RecordingSubscriber::Record & got = sub_->back();
|
||||
EXPECT_EQ(got.kind, RecordingSubscriber::Record::kLaserScan);
|
||||
EXPECT_TRUE(got.hasScan3d);
|
||||
EXPECT_EQ(got.frameId, "lidar_link");
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, AScanWithOdometryIsSynchronizedWithIt)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
EXPECT_TRUE(sub_->isSubscribedToOdom());
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
|
||||
advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
|
||||
advertise<nav_msgs::msg::Odometry>("odom");
|
||||
|
||||
cloud->publish(makeScanCloud("lidar_link", 1000.0));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_TRUE(sub_->empty());
|
||||
|
||||
odom->publish(makeOdometry("odom", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
EXPECT_TRUE(sub_->back().hasOdom);
|
||||
EXPECT_TRUE(sub_->back().hasScan3d);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, ASensorDataGoesToItsOwnCallback)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_sensor_data", true),
|
||||
rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr data =
|
||||
advertise<rtabmap_msgs::msg::SensorData>("sensor_data");
|
||||
|
||||
data->publish(makeSensorData("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
const RecordingSubscriber::Record & got = sub_->back();
|
||||
EXPECT_EQ(got.kind, RecordingSubscriber::Record::kSensorData);
|
||||
EXPECT_EQ(got.cameraInfos, 1u);
|
||||
EXPECT_EQ(got.frameId, "camera_link");
|
||||
EXPECT_FALSE(got.hasOdom);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, ASensorDataCanBeSynchronizedWithOdometry)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_sensor_data", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr data =
|
||||
advertise<rtabmap_msgs::msg::SensorData>("sensor_data");
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
|
||||
advertise<nav_msgs::msg::Odometry>("odom");
|
||||
|
||||
data->publish(makeSensorData("camera_link", 1000.0));
|
||||
odom->publish(makeOdometry("odom", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
EXPECT_EQ(sub_->back().kind, RecordingSubscriber::Record::kSensorData);
|
||||
EXPECT_TRUE(sub_->back().hasOdom);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, OdometryAloneGoesToTheOdomCallback)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false)});
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
|
||||
advertise<nav_msgs::msg::Odometry>("odom");
|
||||
|
||||
odom->publish(makeOdometry("odom", 1000.0, 2.5));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
|
||||
const RecordingSubscriber::Record & got = sub_->back();
|
||||
EXPECT_EQ(got.kind, RecordingSubscriber::Record::kOdom);
|
||||
EXPECT_TRUE(got.hasOdom);
|
||||
EXPECT_FALSE(got.hasOdomInfo);
|
||||
EXPECT_EQ(got.frameId, "odom");
|
||||
EXPECT_DOUBLE_EQ(got.stamp, 1000.0);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, OdometryAndItsInfoAreSynchronizedTogether)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("subscribe_odom_info", true)});
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
|
||||
advertise<nav_msgs::msg::Odometry>("odom");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfo =
|
||||
advertise<rtabmap_msgs::msg::OdomInfo>("odom_info");
|
||||
|
||||
odom->publish(makeOdometry("odom", 1000.0));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_TRUE(sub_->empty()) << "the pair is incomplete until the info arrives";
|
||||
|
||||
odomInfo->publish(makeOdomInfo("odom", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
EXPECT_TRUE(sub_->back().hasOdom);
|
||||
EXPECT_TRUE(sub_->back().hasOdomInfo);
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, DeliversEveryFrameOfAStream)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
|
||||
for(int i=0; i<5; ++i)
|
||||
{
|
||||
const double stamp = 1000.0 + 0.1*double(i);
|
||||
rgb->publish(makeRgbImage("camera_link", stamp));
|
||||
depth->publish(makeDepthImage("camera_link", stamp));
|
||||
info->publish(makeCameraInfo("camera_link", stamp));
|
||||
ASSERT_TRUE(spinUntil([&, i]() { return sub_->size() == size_t(i+1); }))
|
||||
<< "frame " << i << " never arrived";
|
||||
}
|
||||
|
||||
ASSERT_EQ(sub_->size(), 5u);
|
||||
for(size_t i=1; i<sub_->size(); ++i)
|
||||
{
|
||||
EXPECT_GT(sub_->records()[i].stamp, sub_->records()[i-1].stamp);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, ExactSyncDropsAnIncompleteSet)
|
||||
{
|
||||
// With approx_sync off every input has to carry the same stamp, which is the whole
|
||||
// point of the setting -- and the most common reason a pipeline goes quiet.
|
||||
start({rclcpp::Parameter("subscribe_odom", false),
|
||||
rclcpp::Parameter("approx_sync", false)});
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.000));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.002));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.000));
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
EXPECT_TRUE(sub_->empty());
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1001.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1001.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1001.0));
|
||||
EXPECT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
|
||||
}
|
||||
|
||||
TEST_F(CommonDataSubscriberSyncTest, PublishesDiagnostics)
|
||||
{
|
||||
start({rclcpp::Parameter("subscribe_odom", false)});
|
||||
|
||||
std::shared_ptr<Collector<diagnostic_msgs::msg::DiagnosticArray>> diagnostics =
|
||||
collect<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics");
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
advertise<sensor_msgs::msg::Image>("rgb/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
advertise<sensor_msgs::msg::Image>("depth/image");
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
|
||||
rgb->publish(makeRgbImage("camera_link", 1000.0));
|
||||
depth->publish(makeDepthImage("camera_link", 1000.0));
|
||||
info->publish(makeCameraInfo("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !diagnostics->empty(); },
|
||||
std::chrono::milliseconds(10000)));
|
||||
|
||||
bool sawInput = false;
|
||||
bool sawOutput = false;
|
||||
for(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg :
|
||||
diagnostics->messages)
|
||||
{
|
||||
for(const diagnostic_msgs::msg::DiagnosticStatus & status : msg->status)
|
||||
{
|
||||
sawInput = sawInput || status.name.find("Input Status") != std::string::npos;
|
||||
sawOutput = sawOutput || status.name.find("Output Status") != std::string::npos;
|
||||
}
|
||||
}
|
||||
EXPECT_TRUE(sawInput);
|
||||
EXPECT_TRUE(sawOutput) << "tick() is what the subclass calls to report its own rate";
|
||||
}
|
||||
Reference in New Issue
Block a user