Supporting image compression formats passthrough (#1461)

* Supporting ros depth compression formats passthrough

* Added image_compression_format (support jpg,png). Use Compression.h constants.

* Bridging gap with #1457

* CoreWrapper: decode_images_on_demand

* rgbd_sync->rgbd_split encoding/decoding test with image_transport validation. rgbd_split color passthrough

* more tests

* fixing ci on kilted+

* fixing test

* fixing lyrical tests

* fixing rolling ci

* Improved test coverage

* fixing some tf waitForTransform deadlocks when sim time is used in tests

* CI: build rtabmap_slam on rolling

rtabmap_slam builds without nav2_msgs, which is not on resolute yet: skip its
key instead of leaving the package out. Only rtabmap_costmap_plugins (and the
rtabmap_ros metapackage pulling it in) still need nav2.

rtabmap_slam/doc/rtabmap.md

@@ -37,6 +37,7 @@ Loop closures are found two ways:
   - [User data and environment sensors](#user-data-and-environment-sensors)
   - [Intermediate odometry](#intermediate-odometry)
 - [Deriving missing data](#deriving-missing-data)
+- [Compressed images](#compressed-images)
 - [Localization](#localization)
 - [Planning](#planning)
 - [Diagnostics](#diagnostics)
@@ -426,6 +427,7 @@ The node's own ROS parameters, with their real types. The ones about frames are
 | `gen_depth_fill_iterations` | `int` | `1` | Hole-filling passes. |
 | `gen_depth_fill_holes_error` | `double` | `0.1` | Maximum depth difference, in meters, across a hole for it to be filled. |
 | `stereo_to_depth` | `bool` | `false` | Compute a depth image from the stereo pair (with the `StereoBM/*` parameters) and map it as RGB-D. |
+| `decode_images_on_demand` | `bool` | `true` | Leave compressed images of `rgbd_image` to RTAB-Map to decode, only if it needs them. See [Compressed images](#compressed-images). |
 | `scan_cloud_max_points` | `int` | `0` | Points in a full `scan_cloud` sweep, for an organized or fixed-size cloud; used by ICP as the reference for its correspondence ratio. `0` takes each cloud's own size. |
 | `scan_cloud_is_2d` | `bool` | `false` | `scan_cloud` is a 2D lidar published as a cloud. |
 | `odom_sensor_sync` | `bool` | `true` | Place each sensor where the robot was at that sensor's stamp, and deskew 2D scans ray by ray, using the odometry in TF. See [Sensors not stamped together](#sensors-not-stamped-together). |
@@ -563,6 +565,12 @@ With `subscribe_inter_odom_info`, `inter_odom` is synchronized by exact stamp wi
 
 **`stereo_to_depth`** computes a dense depth image from a stereo pair, so a stereo camera is mapped like an RGB-D one — denser grids and clouds, at the cost of the disparity computation.
 
+## Compressed images
+
+An `rgbd_image` (`subscribe_rgbd`, single camera) carrying only compressed images, as [rgbd_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_image/compressed` does, is not decoded by this node: the compressed images are given to RTAB-Map as they are, which decodes them only if it needs the pixels -- to extract features, build a grid from depth or rectify the images. With features already extracted by odometry (`Mem/UseOdomFeatures`), they are often never decoded at all. Either way, they are stored in the database as received, not compressed again.
+
+The images are decoded here instead when this node needs them (`gen_scan`, `gen_depth`, `stereo_to_depth`), when RTAB-Map could not use them as they are (e.g., 16 bits color images, a color right image, which are converted first), and for several cameras. `decode_images_on_demand: false` always decodes them here, as before 0.24: a fallback if something needs the decoded images that this node does not know of.
+
 ## Localization
 
 `localization_pose` is the robot's pose in the map frame — `map` → `odom` composed with the odometry — after each update, with RTAB-Map's covariance. While mapping, that is the odometry covariance accumulated along the graph, growing with distance until a loop closure brings it down. In localization mode before the first loop closure, it is `9999`: the robot is not localized yet.

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -178,6 +178,28 @@ private:
 				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
 				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
 				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
+	/// Compressed images are left to rtabmap to decode when it needs them, unless
+	/// something here needs the pixels (stereo_to_depth, gen_scan, gen_depth).
+	virtual bool imagesDecodedOnDemand() const override
+	{
+		return decodeImagesOnDemand_ && !stereoToDepth_ && !genScan_ && !genDepth_;
+	}
+	virtual void commonMultiCameraCallbackWithCompressed(
+				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+				const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+				const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
+				const sensor_msgs::msg::LaserScan & scanMsg,
+				const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
+				const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
+				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
+				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
+				const std::vector<cv::Mat> & localDescriptors,
+				const std::vector<cv::Mat> & compressedImages,
+				const std::vector<cv::Mat> & compressedDepths) override;
 	// Callback called from sync thread
 	void commonMultiCameraCallbackImpl(
 				const std::string & odomFrameId,
@@ -192,7 +214,9 @@ private:
 				const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
 				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
 				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
-				const std::vector<cv::Mat> & localDescriptors);
+				const std::vector<cv::Mat> & localDescriptors,
+				const std::vector<cv::Mat> & compressedImages = std::vector<cv::Mat>(),
+				const std::vector<cv::Mat> & compressedDepths = std::vector<cv::Mat>());
 	// Callback called from sync thread
 	virtual void commonLaserScanCallback(
 				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
@@ -371,6 +395,7 @@ private:
 	double genScanMaxDepth_;
 	double genScanMinDepth_;
 	bool genDepth_;
+	bool decodeImagesOnDemand_;
 	int genDepthDecimation_;
 	int genDepthFillHolesSize_;
 	int genDepthFillIterations_;

rtabmap_slam/src/CoreWrapper.cpp

@@ -128,6 +128,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		genScanMaxDepth_(4.0),
 		genScanMinDepth_(0.0),
 		genDepth_(false),
+		decodeImagesOnDemand_(true),
 		genDepthDecimation_(1),
 		genDepthFillHolesSize_(0),
 		genDepthFillIterations_(1),
@@ -223,6 +224,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	genScanMaxDepth_ = this->declare_parameter("gen_scan_max_depth", genScanMaxDepth_);
 	genScanMinDepth_ = this->declare_parameter("gen_scan_min_depth", genScanMinDepth_);
 	genDepth_ = this->declare_parameter("gen_depth", genDepth_);
+	decodeImagesOnDemand_ = this->declare_parameter("decode_images_on_demand", decodeImagesOnDemand_);
 	genDepthDecimation_ = this->declare_parameter("gen_depth_decimation", genDepthDecimation_);
 	genDepthFillHolesSize_ = this->declare_parameter("gen_depth_fill_holes_size", genDepthFillHolesSize_);
 	genDepthFillIterations_ = this->declare_parameter("gen_depth_fill_iterations", genDepthFillIterations_);
@@ -268,6 +270,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	}
 
 	RCLCPP_INFO(get_logger(), "rtabmap: gen_depth  = %s", genDepth_?"true":"false");
+	RCLCPP_INFO(get_logger(), "rtabmap: decode_images_on_demand = %s", decodeImagesOnDemand_?"true":"false");
 	if(genDepth_)
 	{
 		RCLCPP_INFO(get_logger(), "rtabmap: gen_depth_decimation        = %d", genDepthDecimation_);
@@ -1348,6 +1351,29 @@ void CoreWrapper::commonMultiCameraCallback(
 		const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
 		const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
 		const std::vector<cv::Mat> & localDescriptors)
+{
+	commonMultiCameraCallbackWithCompressed(odomMsg, userDataMsg, imageMsgs, depthMsgs,
+			cameraInfoMsgs, depthCameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors,
+			std::vector<cv::Mat>(), std::vector<cv::Mat>());
+}
+
+void CoreWrapper::commonMultiCameraCallbackWithCompressed(
+		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+		const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+		const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+		const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
+		const sensor_msgs::msg::LaserScan & scan2dMsg,
+		const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
+		const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
+		const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
+		const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
+		const std::vector<cv::Mat> & localDescriptors,
+		const std::vector<cv::Mat> & compressedImages,
+		const std::vector<cv::Mat> & compressedDepths)
 {
 	std::string odomFrameId;
 	if(odomMsg.get())
@@ -1418,7 +1444,9 @@ void CoreWrapper::commonMultiCameraCallback(
 				globalDescriptorMsgs,
 				localKeyPoints,
 				localPoints3d,
-				localDescriptors);
+				localDescriptors,
+				compressedImages,
+				compressedDepths);
 		
 		if(syncData_.valid) {
 			syncTimer_->reset();
@@ -1439,7 +1467,9 @@ void CoreWrapper::commonMultiCameraCallbackImpl(
 		const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
 		const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
 		const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
-		const std::vector<cv::Mat> & localDescriptorsMsgs)
+		const std::vector<cv::Mat> & localDescriptorsMsgs,
+		const std::vector<cv::Mat> & compressedImages,
+		const std::vector<cv::Mat> & compressedDepths)
 {
 	UTimer timerConversion;
 	cv::Mat rgb;
@@ -1696,6 +1726,43 @@ void CoreWrapper::commonMultiCameraCallbackImpl(
 				userData);
 	}
 
+	// Keep the compressed images the images come from, so that rtabmap stores them as is
+	// instead of compressing them again (see Memory). Only for a single camera, and only if
+	// the images were not changed on the way (color conversion, generated depth).
+	if(imageMsgs.empty() && depthMsgs.empty() && compressedImages.size() == 1 && compressedDepths.size() == 1)
+	{
+		// Not decoded (see imagesDecodedOnDemand()): rtabmap decodes them if it needs to
+		if(!stereoCameraModels.empty())
+		{
+			syncData_.data.setStereoImage(compressedImages[0], compressedDepths[0], stereoCameraModels, false);
+		}
+		else
+		{
+			syncData_.data.setRGBDImage(compressedImages[0], compressedDepths[0], cameraModels, false);
+		}
+	}
+	else if(imageMsgs.size() == 1 && compressedImages.size() == 1 && compressedDepths.size() == 1)
+	{
+		const bool sameImage = !compressedImages[0].empty() && imageMsgs[0].get() &&
+				rgb.type() == imageMsgs[0]->image.type() && rgb.size() == imageMsgs[0]->image.size();
+		const bool sameDepth = !compressedDepths[0].empty() && !genDepth_ &&
+				depthMsgs.size() == 1 && depthMsgs[0].get() &&
+				depth.type() == depthMsgs[0]->image.type() && depth.size() == depthMsgs[0]->image.size();
+		if(sameImage || sameDepth)
+		{
+			const cv::Mat compressedImage = sameImage ? compressedImages[0] : cv::Mat();
+			const cv::Mat compressedDepth = sameDepth ? compressedDepths[0] : cv::Mat();
+			if(!stereoCameraModels.empty())
+			{
+				syncData_.data.setStereoImage(compressedImage, compressedDepth, stereoCameraModels, false);
+			}
+			else
+			{
+				syncData_.data.setRGBDImage(compressedImage, compressedDepth, cameraModels, false);
+			}
+		}
+	}
+
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
 	{

rtabmap_slam/test/core_wrapper_fixture.hpp

@@ -223,9 +223,9 @@ protected:
 		if(!tfPub_)
 		{
 			tfPub_ = helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
-			// The node's listener and nothing else: publishing before it is matched loses
+			// The node's listener (and tfReceived_): publishing before it is matched loses
 			// the transform.
-			waitForSubscriber(tfPub_);
+			waitForSubscriber(tfPub_, tfReceived_ ? 2 : 1);
 		}
 		tf2_msgs::msg::TFMessage msg;
 		msg.transforms.push_back(tf);
@@ -244,9 +244,47 @@ protected:
 			double variance = 0.001)
 	{
 		publishTf(makeTransform("odom", "base_link", stamp, x, y, yaw));
+		if(tfReceived_)
+		{
+			EXPECT_TRUE(spinUntil([&]() { return hasTransform(*tfReceived_, "base_link", stamp); }))
+					<< "odom -> base_link at " << stamp << " was not received";
+		}
 		pub->publish(makeOdometry(stamp, x, y, yaw, variance));
 	}
 
+	/// Whether @p tf received the transform of @p child at @p stamp.
+	static bool hasTransform(const Collector<tf2_msgs::msg::TFMessage> & tf,
+			const std::string & child, double stamp)
+	{
+		for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf.messages)
+		{
+			for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+			{
+				if(t.child_frame_id == child && rclcpp::Time(t.header.stamp) == stampOf(stamp))
+				{
+					return true;
+				}
+			}
+		}
+		return false;
+	}
+
+	/**
+	 * @brief Makes sendOdom() wait for each odom -> base_link transform to be received
+	 *        before publishing the odometry.
+	 *
+	 * To use with wait_for_transform=0 when the node is on simulated time: since Lyrical,
+	 * tf2_ros waits for a transform on the node's clock, which does not advance while the
+	 * test, the only /clock publisher, is blocked in the node's callback waiting for it.
+	 */
+	void waitForTfBeforeOdom()
+	{
+		tfReceived_ = collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+	}
+
+	/// The test's own /tf subscription, see waitForTfBeforeOdom().
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tfReceived_;
+
 	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPublisher()
 	{
 		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub =

rtabmap_slam/test/test_core_wrapper_inputs.cpp

@@ -23,6 +23,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
 #include <rtabmap/core/Parameters.h>
 
 #include <rtabmap/core/Compression.h>
+#include <opencv2/imgproc.hpp>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap_conversions/MsgConversion.h>
 
@@ -570,6 +571,331 @@ TEST_F(CoreWrapperInputsTest, maps_sensor_data)
 	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
 }
 
+/**
+ * Compressed images of rtabmap_msgs/SensorData (what odom_sensor_data/compressed carries)
+ * are stored as received, not decompressed and re-compressed: the formats below differ
+ * from Mem/ImageCompressionFormat (".jpg") and Mem/DepthCompressionFormat (".rvl"), so
+ * re-compressed images would not be the same bytes.
+ */
+class CoreWrapperCompressedSensorDataTest :
+	public CoreWrapperInputsTest,
+	public ::testing::WithParamInterface<std::tuple<std::string, bool>>
+{
+protected:
+	static constexpr int kWidth = 64;
+	static constexpr int kHeight = 48;
+
+	/// A textured color image and a depth image of @p depthType, compressed by rtabmap.
+	static rtabmap::SensorData makeCompressedData(int depthType, const std::string & depthFormat, double stamp)
+	{
+		cv::Mat rgb(kHeight, kWidth, CV_8UC3);
+		cv::randu(rgb, 0, 255);
+		cv::Mat depth(kHeight, kWidth, depthType);
+		if(depthType == CV_16UC1)
+		{
+			cv::randu(depth, 1000, 3000);
+		}
+		else
+		{
+			cv::randu(depth, 1.0f, 3.0f);
+		}
+		const rtabmap::CameraModel model(50.0, 50.0, kWidth/2.0, kHeight/2.0,
+				rtabmap::CameraModel::opticalRotation(), 0.0, cv::Size(kWidth, kHeight));
+		return rtabmap::SensorData(
+				rtabmap::compressImage2(rgb, ".png"),
+				rtabmap::compressImage2(depth, depthFormat),
+				model, 0, stamp);
+	}
+
+	/// Publishes @p data (compressed only, or with its raw images too) and returns the
+	/// node it became.
+	rtabmap_msgs::msg::Node map(const rtabmap::SensorData & data, bool withRaw,
+			const std::vector<rclcpp::Parameter> & params = {})
+	{
+		std::vector<rclcpp::Parameter> all = {
+				rclcpp::Parameter("subscribe_sensor_data", true),
+				rclcpp::Parameter("Mem/ImageCompressionFormat", std::string(".jpg")),
+				rclcpp::Parameter("Mem/DepthCompressionFormat", std::string(".rvl"))};
+		all.insert(all.end(), params.begin(), params.end());
+		makeNode(all);
+		std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+		rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr pub =
+				helper()->create_publisher<rtabmap_msgs::msg::SensorData>("sensor_data", 10);
+		EXPECT_TRUE(waitForSubscriber(pub));
+
+		rtabmap::SensorData copy = data;
+		if(withRaw)
+		{
+			copy.uncompressData();
+			EXPECT_FALSE(copy.imageRaw().empty());
+			EXPECT_FALSE(copy.depthRaw().empty());
+		}
+		rtabmap_msgs::msg::SensorData msg;
+		rtabmap_conversions::sensorDataToROS(copy, msg, "base_link", withRaw);
+		msg.header.stamp = stampOf(data.stamp());
+		sendOdom(odom, data.stamp(), 0.0);
+		pub->publish(msg);
+		EXPECT_TRUE(spinUntil([&]() { return !info->empty(); }));
+		return getNode(1);
+	}
+
+	static std::vector<unsigned char> bytes(const cv::Mat & compressed)
+	{
+		return std::vector<unsigned char>(compressed.data, compressed.data + compressed.total());
+	}
+};
+
+TEST_P(CoreWrapperCompressedSensorDataTest, stores_compressed_images_as_received)
+{
+	const std::string depthFormat = std::get<0>(GetParam());
+	const bool withRaw = std::get<1>(GetParam());
+	const int depthType = depthFormat == ".png" ? CV_16UC1 : CV_32FC1;
+	const rtabmap::SensorData data = makeCompressedData(depthType, depthFormat, 1.0);
+
+	const rtabmap_msgs::msg::Node node = map(data, withRaw);
+	ASSERT_EQ(1, node.id);
+	EXPECT_EQ(node.data.left_compressed, bytes(data.imageCompressed())) << "color not re-compressed";
+	EXPECT_EQ(node.data.right_compressed, bytes(data.depthOrRightCompressed())) << "depth not re-compressed";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		Formats,
+		CoreWrapperCompressedSensorDataTest,
+		::testing::Combine(
+				// 16UC1 PNG, 32FC1 lossless PNG (4 channels), 32FC1 inverse depth
+				::testing::Values(std::string(".png"), std::string(".png:10:100")),
+				::testing::Bool()),   // compressed only, or raw images too
+		[](const ::testing::TestParamInfo<std::tuple<std::string, bool>> & info) {
+			const std::string & f = std::get<0>(info.param);
+			return std::string(f == ".png" ? "png16" : "inverse_depth") +
+					(std::get<1>(info.param) ? "_with_raw" : "_compressed_only");
+		});
+
+/// The legacy lossless 32FC1 format is stored as received too.
+TEST_F(CoreWrapperCompressedSensorDataTest, stores_legacy_float_depth_as_received)
+{
+	const rtabmap::SensorData data = makeCompressedData(CV_32FC1, ".png", 1.0);
+	ASSERT_EQ(rtabmap::compressedDepthFormat(data.depthOrRightCompressed()), ".png");
+	const rtabmap_msgs::msg::Node node = map(data, false);
+	ASSERT_EQ(1, node.id);
+	EXPECT_EQ(node.data.right_compressed, bytes(data.depthOrRightCompressed()));
+}
+
+/// When Memory changes an image (here the color image, by Mem/ImagePostDecimation), it
+/// compresses it itself, with its own format; the images it leaves as is (here depth,
+/// decimated only by Mem/ImagePreDecimation) are still stored as received.
+TEST_F(CoreWrapperCompressedSensorDataTest, recompresses_only_the_images_it_changes)
+{
+	const rtabmap::SensorData data = makeCompressedData(CV_16UC1, ".png", 1.0);
+	const rtabmap_msgs::msg::Node node = map(data, false,
+			{rclcpp::Parameter("Mem/ImagePostDecimation", std::string("2"))});
+	ASSERT_EQ(1, node.id);
+	const cv::Mat rgb = rtabmap::uncompressImage(node.data.left_compressed);
+	EXPECT_EQ(rgb.cols, kWidth/2);
+	ASSERT_GE(node.data.left_compressed.size(), 2u);
+	EXPECT_EQ(node.data.left_compressed[0], 0xFF) << "JPEG, Mem/ImageCompressionFormat";
+	EXPECT_EQ(node.data.right_compressed, bytes(data.depthOrRightCompressed()));
+}
+
+/**
+ * The same for rtabmap_msgs/RGBDImage: images received compressed only are stored as
+ * received (depth in compressed_depth_image_transport's format only converted to rtabmap's
+ * format, without decompression), unless they had to be changed on the way.
+ */
+class CoreWrapperCompressedRGBDTest : public CoreWrapperInputsTest
+{
+protected:
+	static constexpr int kWidth = 64;
+	static constexpr int kHeight = 48;
+
+	/// @p msg with only its images and camera infos left to set.
+	static rtabmap_msgs::msg::RGBDImage makeMsg()
+	{
+		rtabmap_msgs::msg::RGBDImage msg;
+		msg.header.frame_id = "camera";
+		msg.header.stamp = stampOf(1.0);
+		msg.rgb_camera_info = makeCameraInfo("camera", 1.0, kWidth, kHeight);
+		msg.depth_camera_info = msg.rgb_camera_info;
+		return msg;
+	}
+
+	static cv::Mat colorImage()
+	{
+		return cv_bridge::toCvCopy(makeTexturedImage("camera", 1.0, kWidth, kHeight))->image;
+	}
+
+	static cv::Mat floatDepth()
+	{
+		cv::Mat depth(kHeight, kWidth, CV_32FC1);
+		cv::randu(depth, 1.0f, 3.0f);
+		return depth;
+	}
+
+	static sensor_msgs::msg::CompressedImage compressedColor(const cv::Mat & image, const std::string & encoding)
+	{
+		sensor_msgs::msg::CompressedImage msg;
+		EXPECT_TRUE(rtabmap_conversions::toCompressedImageMsg(
+				cv_bridge::CvImage(makeMsg().header, encoding, image), "png", msg));
+		return msg;
+	}
+
+	static sensor_msgs::msg::CompressedImage compressedDepth(const cv::Mat & depth, const std::string & format)
+	{
+		sensor_msgs::msg::CompressedImage msg;
+		msg.header = makeMsg().header;
+		EXPECT_TRUE(rtabmap_conversions::compressDepthImage(depth, format, msg));
+		return msg;
+	}
+
+	/// Maps @p msg (Mem/ImageCompressionFormat=".jpg", Mem/DepthCompressionFormat=".rvl",
+	/// so that re-compressed images are not the same bytes) and returns its node.
+	rtabmap_msgs::msg::Node map(const rtabmap_msgs::msg::RGBDImage & msg,
+			const std::vector<rclcpp::Parameter> & params = {})
+	{
+		std::vector<rclcpp::Parameter> all = {
+				rclcpp::Parameter("subscribe_rgbd", true),
+				rclcpp::Parameter("Mem/ImageCompressionFormat", std::string(".jpg")),
+				rclcpp::Parameter("Mem/DepthCompressionFormat", std::string(".rvl"))};
+		all.insert(all.end(), params.begin(), params.end());
+		publishStaticTf("camera");
+		makeNode(all);
+		std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+		rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+				helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+		EXPECT_TRUE(waitForSubscriber(rgbd));
+		sendOdom(odom, 1.0, 0.0);
+		rgbd->publish(msg);
+		EXPECT_TRUE(spinUntil([&]() { return !info->empty(); }));
+		return getNode(1);
+	}
+
+	/// Maps a color PNG and @p depth, both compressed only, and checks they are stored as is.
+	void checkStoredAsReceived(const sensor_msgs::msg::CompressedImage & depth, const std::string & storedFormat)
+	{
+		rtabmap_msgs::msg::RGBDImage msg = makeMsg();
+		msg.rgb_compressed = compressedColor(colorImage(), "bgr8");
+		msg.depth_compressed = depth;
+
+		const rtabmap_msgs::msg::Node node = map(msg);
+		ASSERT_EQ(1, node.id);
+		EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data) << "color not re-compressed";
+		const cv::Mat expected = depth.format.find("compressedDepth") != std::string::npos ?
+				rtabmap_conversions::compressedDepthTransportToRtabmap(depth) :
+				rtabmap_conversions::compressedMatFromBytes(depth.data);
+		EXPECT_EQ(node.data.right_compressed, std::vector<unsigned char>(expected.data, expected.data + expected.total()))
+			<< "depth not re-compressed";
+		EXPECT_EQ(rtabmap::compressedDepthFormat(node.data.right_compressed), storedFormat);
+	}
+
+	/// Maps a compressed stereo pair and checks it is stored as is.
+	void checkStereoStoredAsReceived(bool decodeOnDemand)
+	{
+		rtabmap_msgs::msg::RGBDImage msg = makeMsg();
+		msg.depth_camera_info.p[3] = -msg.depth_camera_info.p[0] * 0.1; // 10 cm baseline
+		cv::Mat left, right;
+		cv::cvtColor(colorImage(), left, cv::COLOR_BGR2GRAY);
+		cv::flip(left, right, 1);
+		msg.rgb_compressed = compressedColor(left, "mono8");
+		msg.depth_compressed = compressedColor(right, "mono8");
+		const rtabmap_msgs::msg::Node node = map(msg, {rclcpp::Parameter("decode_images_on_demand", decodeOnDemand)});
+		ASSERT_EQ(1, node.id);
+		ASSERT_EQ(node.data.right_camera_info.size(), 1u) << "stored as a stereo pair";
+		EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data);
+		EXPECT_EQ(node.data.right_compressed, msg.depth_compressed.data);
+	}
+
+	static bool isJpeg(const std::vector<unsigned char> & bytes)
+	{
+		return bytes.size() >= 2 && bytes[0] == 0xFF && bytes[1] == 0xD8;
+	}
+};
+
+TEST_F(CoreWrapperCompressedRGBDTest, stores_16bits_compressed_depth_as_received)
+{
+	cv::Mat depth16U;
+	floatDepth().convertTo(depth16U, CV_16UC1, 1000.0);
+	checkStoredAsReceived(compressedDepth(depth16U, ".png"), ".png");
+}
+
+TEST_F(CoreWrapperCompressedRGBDTest, stores_inverse_depth_as_received)
+{
+	checkStoredAsReceived(compressedDepth(floatDepth(), ".png:10:100"), ".png:10:100");
+}
+
+TEST_F(CoreWrapperCompressedRGBDTest, stores_legacy_float_depth_as_received)
+{
+	checkStoredAsReceived(compressedDepth(floatDepth(), "legacy"), ".png");
+}
+
+/// Decoding on demand can be disabled: images are then decoded here, and still stored as
+/// received.
+TEST_F(CoreWrapperCompressedRGBDTest, stores_compressed_images_as_received_when_decoded_here)
+{
+	rtabmap_msgs::msg::RGBDImage msg = makeMsg();
+	msg.rgb_compressed = compressedColor(colorImage(), "bgr8");
+	msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100");
+	const rtabmap_msgs::msg::Node node = map(msg, {rclcpp::Parameter("decode_images_on_demand", false)});
+	ASSERT_EQ(1, node.id);
+	EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data);
+	EXPECT_EQ(rtabmap::compressedDepthFormat(node.data.right_compressed), ".png:10:100");
+}
+
+/// gen_scan needs the depth pixels: the images are decoded here, and the scan generated.
+TEST_F(CoreWrapperCompressedRGBDTest, decodes_images_to_generate_a_scan)
+{
+	rtabmap_msgs::msg::RGBDImage msg = makeMsg();
+	msg.rgb_compressed = compressedColor(colorImage(), "bgr8");
+	msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100");
+	const rtabmap_msgs::msg::Node node = map(msg, {rclcpp::Parameter("gen_scan", true)});
+	ASSERT_EQ(1, node.id);
+	EXPECT_FALSE(node.data.laser_scan_compressed.empty()) << "scan generated from the depth image";
+	EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data) << "still stored as received";
+}
+
+/// A compressed stereo pair (gray right image) is stored as received too, decoded on
+/// demand or here.
+TEST_F(CoreWrapperCompressedRGBDTest, stores_a_compressed_stereo_pair_as_received)
+{
+	checkStereoStoredAsReceived(true);
+}
+
+TEST_F(CoreWrapperCompressedRGBDTest, stores_a_compressed_stereo_pair_as_received_when_decoded_here)
+{
+	checkStereoStoredAsReceived(false);
+}
+
+/// A raw image is compressed by rtabmap; the compressed one next to it is still stored as is.
+TEST_F(CoreWrapperCompressedRGBDTest, stores_compressed_depth_with_raw_color)
+{
+	rtabmap_msgs::msg::RGBDImage msg = makeMsg();
+	msg.rgb = makeTexturedImage("camera", 1.0, kWidth, kHeight);
+	msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100");
+	const rtabmap_msgs::msg::Node node = map(msg);
+	ASSERT_EQ(1, node.id);
+	EXPECT_TRUE(isJpeg(node.data.left_compressed)) << "Mem/ImageCompressionFormat";
+	const cv::Mat expected = rtabmap_conversions::compressedDepthTransportToRtabmap(msg.depth_compressed);
+	EXPECT_EQ(node.data.right_compressed, std::vector<unsigned char>(expected.data, expected.data + expected.total()));
+}
+
+/// A color image that has to be converted (here mono16 to mono8) is not the image of its
+/// compressed bytes anymore: it is compressed again by rtabmap.
+TEST_F(CoreWrapperCompressedRGBDTest, recompresses_converted_images)
+{
+	rtabmap_msgs::msg::RGBDImage msg = makeMsg();
+	cv::Mat gray16(kHeight, kWidth, CV_16UC1);
+	cv::randu(gray16, 0, 65535);
+	msg.rgb_compressed = compressedColor(gray16, "mono16");
+	ASSERT_EQ(msg.rgb_compressed.format, "mono16; png compressed mono16");
+	msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100");
+	const rtabmap_msgs::msg::Node node = map(msg);
+	ASSERT_EQ(1, node.id);
+	EXPECT_TRUE(isJpeg(node.data.left_compressed)) << "converted to mono8, then Mem/ImageCompressionFormat";
+	EXPECT_EQ(rtabmap::uncompressImage(node.data.left_compressed).type(), CV_8UC1);
+	EXPECT_EQ(rtabmap::compressedDepthFormat(node.data.right_compressed), ".png:10:100") << "depth still as received";
+}
+
 //==========================================================================================
 // Synchronization of sensors that are not stamped together
 //==========================================================================================

rtabmap_slam/test/test_core_wrapper_planning.cpp

@@ -50,14 +50,11 @@ protected:
 		ASSERT_TRUE(waitForPublisher(goalReached_->subscription));
 		ASSERT_TRUE(waitForPublisher(globalPath_->subscription));
 		ASSERT_TRUE(waitForPublisher(globalPathNodes_->subscription));
-		// Nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0.
-		for(int i=0; i<5; ++i)
+		if(simulatedTime())
 		{
-			const size_t before = info_->size();
-			sendUpdate(1.0 + i, 0.5 * i);
-			ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }))
-					<< "update " << i << " was not processed";
+			waitForTfBeforeOdom();
 		}
+		driveStraight(odom_, info_, 5);   // nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0
 		nextStamp_ = 6.0;
 	}
 
@@ -71,18 +68,13 @@ protected:
 	}
 
 	virtual std::vector<rclcpp::Parameter> nodeParameters() { return {}; }
-
-	/// Sends one odometry update stamped @p stamp, the robot at (@p x, 0, @p yaw).
-	virtual void sendUpdate(double stamp, double x, double yaw = 0.0)
-	{
-		sendOdom(odom_, stamp, x, 0.0, yaw);
-	}
+	virtual bool simulatedTime() const { return false; }
 
 	/// Moves the robot to @p x and waits for the update to be processed.
 	bool moveTo(double x)
 	{
 		const size_t before = info_->size();
-		sendUpdate(nextStamp_, x);
+		sendOdom(odom_, nextStamp_, x);
 		nextStamp_ += 1.0;
 		return spinUntil([&]() { return info_->size() > before; });
 	}
@@ -257,29 +249,22 @@ TEST_F(CoreWrapperPlanningTest, plans_through_the_graph_beyond_the_local_radius)
 /**
  * The same corridor, with the node on the tests' own clock (use_sim_time): map -> odom is
  * then stamped in the odometry's time base, and with tf_tolerance at 0, exactly at the
- * clock's time. The clock is set to each update's stamp before it is sent. A TF lookup
- * waits on that clock (tf2_ros sleeps on it since Rolling/Lyrical): frozen, a lookup that
- * has to wait never returns, even once its transform is there. Each update's transform is
- * then sent first, the clock and some time for the node's listener after it, the odometry
- * last, so that its lookup never has to wait. Only for tests whose lookups find their
- * transform: one for a transform that is not there -- a goal in an unknown frame, say --
- * would wait forever.
+ * clock's time. Only for tests whose TF lookups never have to wait: with a clock that only
+ * moves when told to, a lookup waiting for a transform that is not there -- a goal in an
+ * unknown frame, say -- would wait forever.
  */
 class CoreWrapperPlanningSimTimeTest : public CoreWrapperPlanningTest
 {
 protected:
 	std::vector<rclcpp::Parameter> nodeParameters() override
 	{
+		// wait_for_transform=0: the clock is advanced by the test only, a wait for a
+		// transform on it would never end (see waitForTfBeforeOdom()).
 		return {rclcpp::Parameter("use_sim_time", true),
-				rclcpp::Parameter("tf_tolerance", 0.0)};
-	}
-
-	void sendUpdate(double stamp, double x, double yaw = 0.0) override
-	{
-		publishTf(makeTransform("odom", "base_link", stamp, x, 0.0, yaw));
-		setClock(stamp);
-		odom_->publish(makeOdometry(stamp, x, 0.0, yaw));
+				rclcpp::Parameter("tf_tolerance", 0.0),
+				rclcpp::Parameter("wait_for_transform", 0.0)};
 	}
+	bool simulatedTime() const override { return true; }
 
 	/// Sets the node's clock to @p seconds.
 	void setClock(double seconds)
@@ -302,7 +287,7 @@ TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_t
 {
 	// Turn the robot to face +y where it stands, at x = 2.
 	const size_t before = info_->size();
-	sendUpdate(nextStamp_, 2.0, M_PI/2.0);
+	sendOdom(odom_, nextStamp_, 2.0, 0.0, M_PI/2.0);
 	ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }));
 
 	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
@@ -312,21 +297,8 @@ TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_t
 	// The goal is looked up through map -> odom -> base_link at its stamp: bring the clock
 	// to the turn's stamp, and wait for map -> odom to be published at it.
 	const double stamp = nextStamp_;
-	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
-			collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
 	setClock(stamp);
-	ASSERT_TRUE(spinUntil([&]() {
-		for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
-		{
-			for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
-			{
-				if(t.child_frame_id == "odom" && rclcpp::Time(t.header.stamp) == stampOf(stamp))
-				{
-					return true;
-				}
-			}
-		}
-		return false; }));
+	ASSERT_TRUE(spinUntil([&]() { return hasTransform(*tfReceived_, "odom", stamp); }));
 
 	// 1 m straight ahead of the robot, facing where it faces.
 	geometry_msgs::msg::PoseStamped pose;