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_odom/README.md

@@ -19,6 +19,7 @@ SLAM needs a pose for every measurement it maps. These nodes produce one by regi
 - [Services](#services)
 - [Published topics](#published-topics)
   - [Outputting filtered scans and features](#outputting-filtered-scans-and-features)
+  - [Compressed sensor data](#compressed-sensor-data)
 - [Diagnostics](#diagnostics)
 - [License](#license)
 
@@ -254,7 +255,7 @@ Common to all three nodes. **Every one of them, `odom` included, is published on
 | `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The scan map, the ICP path's equivalent of `odom_local_map`. |
 | `odom_last_frame` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The current frame's **features**, in the odom frame — not its scan or its pixels. Visual paths only, for the same reason as `odom_local_map`; for the filtered scan see `odom_sensor_data/*`. |
 | `odom_rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The frame **as odometry processed it**, not the input as it arrived. See [Outputting filtered scans and features](#outputting-filtered-scans-and-features). |
-| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries JPEG/PNG images instead of raw. |
+| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries compressed images instead of raw, see [Compressed sensor data](#compressed-sensor-data). |
 
 ### Outputting filtered scans and features
 
@@ -264,6 +265,18 @@ Common to all three nodes. **Every one of them, `odom` included, is published on
 - **The scan is the filtered one.** `icp_odometry` builds the frame after deskewing, voxelization, range filtering and normal estimation, so what comes out here is the decimated cloud ICP saw — not the raw sweep the lidar published. Subscribe to the driver's topic if you want the original.
 - **Images are converted.** `rgbd_odometry` hands over grayscale unless `keep_color` is set, so that is what these carry too.
 
+### Compressed sensor data
+
+`odom_sensor_data/compressed` carries the frame with its images and scan compressed, for a slow link. It is only built when something subscribes to it.
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `sensor_data_compression_format` | `string` | `".jpg"` | Format of the color image, and of the right image of a stereo pair: `".jpg"` or `".png"`. |
+| `sensor_data_depth_compression_format` | `string` | `".rvl"` | Format of the depth image: `".rvl"` (lossless, faster) or `".png"` (lossless, smaller), optionally followed by `":<maxDepth>[:<quantization>]"` (e.g. `".rvl:10:100"`) to compress 32FC1 depth as 16 bits inverse depth, as `Mem/DepthCompressionFormat`. That is lossy -- depth over `maxDepth` is lost and the error grows with the square of the depth (~0.05 mm at 1 m and 5 mm at 10 m with a quantization of 100) -- but much smaller than the lossless 32FC1 PNG. 16UC1 depth is always lossless. An invalid value falls back to `".rvl"` with an error. |
+| `sensor_data_parallel_compression` | `bool` | `true` | Compress the images and the scan in parallel threads. |
+
+The depth is in rtabmap's format (`rtabmap::uncompressImage()` reads it), not in `compressed_depth_image_transport`'s; `rtabmap_conversions::rtabmapToCompressedDepthTransport()` converts it without decompressing it.
+
 ## Diagnostics
 
 All three publish to `/diagnostics`: the input rate, the output rate, and how many frames were processed versus dropped. A healthy input rate with a low output rate means frames are arriving but not registering — check `odom_info` before touching anything else.

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -270,6 +270,7 @@ private:
 	double minUpdateRate_;
 	bool alwaysProcessMostRecentFrame_;
 	std::string compressionImgFormat_;
+	std::string compressionDepthFormat_;
 	bool compressionParallelized_;
 	int odomStrategy_;
 	bool waitIMUToinit_;

rtabmap_odom/src/OdometryROS.cpp

@@ -138,6 +138,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	minUpdateRate_(0.0),
 	alwaysProcessMostRecentFrame_(true),
 	compressionImgFormat_(".jpg"),
+	compressionDepthFormat_(".rvl"),
 	compressionParallelized_(true),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
 	waitIMUToinit_(false),
@@ -194,6 +195,18 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	alwaysProcessMostRecentFrame_ = this->declare_parameter("always_process_most_recent_frame", alwaysProcessMostRecentFrame_);
 
 	compressionImgFormat_ = this->declare_parameter("sensor_data_compression_format", compressionImgFormat_);
+	compressionDepthFormat_ = this->declare_parameter("sensor_data_depth_compression_format", compressionDepthFormat_);
+	{
+		std::string codec;
+		float maxDepth, quantization;
+		if(!rtabmap::parseImageCompressionFormat(compressionDepthFormat_, codec, maxDepth, quantization) ||
+		   (codec != ".png" && codec != ".rvl"))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Invalid sensor_data_depth_compression_format \"%s\" (should be \".png\" or \".rvl\", "
+					"optionally followed by \":maxDepth[:quantization]\"), using \".rvl\".", compressionDepthFormat_.c_str());
+			compressionDepthFormat_ = ".rvl";
+		}
+	}
 	compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_);
 
 	waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_);
@@ -268,6 +281,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init       = %s", waitIMUToinit_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "Odometry: always_check_imu_tf    = %s", alwaysCheckImuTf_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str());
+	RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_depth_compression_format = %s", compressionDepthFormat_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
 }
 
@@ -1300,7 +1314,11 @@ void OdometryROS::processData()
 	
 	postProcessData(data, header);
 
-	if(!data.imageRaw().empty() && odomRgbdImagePub_->get_subscription_count()>0)
+	// Any frame with a camera, also with features only or compressed images only (lidar
+	// odometry sets an invalid CameraModel() as placeholder)
+	if(((!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection()) ||
+	    (!data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection())) &&
+	   odomRgbdImagePub_->get_subscription_count()>0)
 	{
 		if(!header.frame_id.empty())
 		{
@@ -1344,7 +1362,7 @@ void OdometryROS::processData()
 		if(compressionParallelized_)
 		{
 			rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_);
-			rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
+			rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?compressionDepthFormat_:compressionImgFormat_);
 			rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data());
 			if(!data.imageRaw().empty())
 			{
@@ -1369,7 +1387,7 @@ void OdometryROS::processData()
 		else
 		{
 			compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
-			compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
+			compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?compressionDepthFormat_:compressionImgFormat_);
 			compressedScan = compressData2(data.laserScanRaw().data());
 		}
 		if(!compressedImage.empty() && !data.stereoCameraModels().empty())

rtabmap_odom/test/test_icp_odometry.cpp

@@ -4,6 +4,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
 */
 
 #include <gtest/gtest.h>
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
 
 #include <tf2_ros/static_transform_broadcaster.hpp>
 
@@ -584,6 +585,28 @@ TEST_F(IcpOdometryTest, publishes_an_identity_pose_for_the_first_scan_cloud)
 	EXPECT_NEAR(0.0, msg.pose.pose.position.z, 1e-6);
 }
 
+/// Without a camera there is no RGBDImage to republish: odom_rgbd_image stays silent,
+/// while the odometry itself is published.
+TEST_F(IcpOdometryTest, does_not_publish_an_rgbd_image_without_a_camera)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
+			collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(frames->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(frames->empty());
+}
+
 /// A 2D lidar goes in on `scan` instead of `scan_cloud`, and reaches the same odometry.
 TEST_F(IcpOdometryTest, accepts_a_laser_scan)
 {

rtabmap_odom/test/test_odometry_ros.cpp

@@ -987,6 +987,26 @@ TEST_F(OdometryRosTest, publishes_compressed_sensor_data_when_asked)
 			<< "the raw scan should have been replaced by the compressed one";
 }
 
+/// Same, compressing in the odometry thread instead of one thread per sensor.
+TEST_F(OdometryRosTest, publishes_compressed_sensor_data_without_parallel_compression)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> compressed =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/compressed");
+	makeNode({rclcpp::Parameter("publish_compressed_sensor_data", true),
+	          rclcpp::Parameter("sensor_data_parallel_compression", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+	ASSERT_TRUE(spinUntil([&]() { return !compressed->empty(); }))
+			<< "nothing was published on odom_sensor_data/compressed";
+	EXPECT_GT(compressed->back().laser_scan_compressed.size(), 0u);
+	EXPECT_EQ(0u, compressed->back().laser_scan.data.size());
+}
+
 
 /**
  * The whole recovery story, as a wheeled robot would live it: a guess frame it trusts,