mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-12 20:49:50 +08:00
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.
This commit is contained in:
matlabbe
@@ -305,6 +305,8 @@ install(DIRECTORY include/
|
||||
#############
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
find_package(ament_index_cpp REQUIRED)
|
||||
find_package(class_loader REQUIRED)
|
||||
|
||||
# Each node gets its own test binary: a crash or a stuck executor in one node cannot
|
||||
# take the others down, and every binary starts with a clean DDS graph.
|
||||
@@ -334,6 +336,12 @@ if(BUILD_TESTING)
|
||||
rtabmap_util_add_node_test(test_disparity_to_depth)
|
||||
rtabmap_util_add_node_test(test_rgbd_relay)
|
||||
rtabmap_util_add_node_test(test_rgbd_split)
|
||||
rtabmap_util_add_node_test(test_rgbd_pipeline)
|
||||
# Loads rtabmap_sync's components by hand: since Kilted, rclcpp_components no longer
|
||||
# exports these to its dependents.
|
||||
if(TARGET test_rgbd_pipeline)
|
||||
target_link_libraries(test_rgbd_pipeline ament_index_cpp::ament_index_cpp class_loader::class_loader)
|
||||
endif()
|
||||
rtabmap_util_add_node_test(test_lidar_deskewing)
|
||||
rtabmap_util_add_node_test(test_obstacles_detection)
|
||||
rtabmap_util_add_node_test(test_point_cloud_aggregator)
|
||||
|
||||
@@ -76,8 +76,10 @@ flowchart LR
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `compress` | `bool` | `false` | Fill the compressed fields of the output. Color becomes JPEG; depth becomes PNG, or JPEG when the message carries a stereo pair rather than depth. Fields already compressed on input are passed through as-is. |
|
||||
| `uncompress` | `bool` | `false` | Fill the raw fields of the output by decoding the compressed ones. Fields already raw on input are passed through as-is. |
|
||||
| `compress` | `bool` | `false` | Fill the compressed fields of the output. Color becomes JPEG; depth becomes PNG, or JPEG when the message carries a stereo pair rather than depth (see `image_compression_format` and `depth_compression_format`). Fields already compressed on input are passed through as-is. |
|
||||
| `image_compression_format` | `string` | `".jpg"` | With `compress`, format of the color image, and of the right image of a stereo pair: `".jpg"` (lossy, smaller) or `".png"` (lossless, larger). An invalid value falls back to `".jpg"` with an error. |
|
||||
| `uncompress` | `bool` | `false` | Fill the raw fields of the output by decoding the compressed ones. Fields already raw on input are passed through as-is. Depth compressed by rtabmap or by `compressed_depth_image_transport` is accepted. |
|
||||
| `depth_compression_format` | `string` | `".png"` | With `compress`, format of the compressed depth, see [rgbd_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s parameter of the same name: `".png"` or `".rvl"` (32FC1 converted to millimeters), optionally followed by `":<maxDepth>[:<quantization>]"` (32FC1 as inverse depth), optionally prefixed by `"legacy:"` (rtabmap's own format, 32FC1 lossless without depth parameters), or `"legacy"` (`"legacy:.png"`). |
|
||||
| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. |
|
||||
| `qos_pub` | `int` | value of `qos` | Reliability of the `rgbd_image_relay` publisher alone. |
|
||||
@@ -118,4 +120,4 @@ Setting neither `compress` nor `uncompress` forwards the message unchanged and s
|
||||
|
||||
Setting both is allowed and produces a message carrying each image twice, raw and compressed. That is rarely what you want.
|
||||
|
||||
Depth is compressed as **PNG**, a stereo right image as **JPEG**.
|
||||
Depth is compressed as **PNG** by default (see `depth_compression_format`), in `compressed_depth_image_transport`'s format, color and stereo right images as **JPEG** by default (see `image_compression_format`), in `compressed_image_transport`'s format.
|
||||
|
||||
@@ -13,6 +13,7 @@ It is the inverse of [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync
|
||||
- [Published Topics](#published-topics)
|
||||
- [Parameters](#parameters)
|
||||
- [Stereo messages](#stereo-messages)
|
||||
- [Compressed images](#compressed-images)
|
||||
- [Notes](#notes)
|
||||
|
||||
## Usage
|
||||
@@ -57,6 +58,8 @@ The output topics are named after the **resolved** input topic, so remapping `rg
|
||||
| `<rgbd_image>/rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
|
||||
| `<rgbd_image>/depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The depth image, or the right image of a stereo pair. |
|
||||
| `<rgbd_image>/depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | For a stereo pair this is the right camera, and its `P(0,3)` carries the baseline. |
|
||||
| `<rgbd_image>/rgb/image/compressed` | [`sensor_msgs/msg/CompressedImage`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CompressedImage.html) | The color image in [`compressed_image_transport`](https://github.com/ros-perception/image_transport_plugins)'s format, published by the node itself unless `compressed_passthrough` is false. Same for `<rgbd_image>/right/image/compressed` with `stereo: true`. See [Compressed images](#compressed-images). |
|
||||
| `<rgbd_image>/depth/image/compressedDepth` | [`sensor_msgs/msg/CompressedImage`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CompressedImage.html) | The depth image in [`compressed_depth_image_transport`](https://github.com/ros-perception/image_transport_plugins)'s format, published by the node itself unless `compressed_depth_passthrough` is false. See [Compressed images](#compressed-images). |
|
||||
|
||||
Each half is only unpacked if something is subscribed to it, so subscribing to color alone does not pay for depth decompression.
|
||||
|
||||
@@ -70,6 +73,12 @@ Each half is only unpacked if something is subscribed to it, so subscribing to c
|
||||
| `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. |
|
||||
| `queue_pub` | `int` | `1` | Queue depth of every publisher. Must be at least 1. |
|
||||
| `stereo` | `bool` | `false` | Name the outputs `left`/`right` instead of `rgb`/`depth`. See [Stereo messages](#stereo-messages). |
|
||||
| `compressed_passthrough` | `bool` | `true` | Publish the `compressed` topics (color, left and right images) from the compressed images of the input without decompressing them, in place of the `compressed` image_transport plugin. See [Compressed images](#compressed-images). |
|
||||
| `compressed_depth_passthrough` | `bool` | `compressed_passthrough` | Same for the `compressedDepth` topic (depth), in place of the `compressedDepth` image_transport plugin. |
|
||||
| `<color topic>.compressed.format` | `string` | `"jpeg"` | `jpeg` or `png`. With `compressed_passthrough`, used only when an image (color, left or right) has to be compressed. `<color topic>` is the color (or left) topic with `.` instead of `/`, e.g. `rgbd_image.rgb.image`. |
|
||||
| `<depth topic>.compressedDepth.format` | `string` | `"png"` | `png` or `rvl` (Jazzy and later). With `compressed_depth_passthrough`, used only when the depth has to be compressed. `<depth topic>` is the depth topic with `.` instead of `/`, e.g. `rgbd_image.depth.image`. |
|
||||
| `<depth topic>.compressedDepth.depth_max` | `double` | `10.0` | Same, maximum depth (m) of a 32FC1 depth image. |
|
||||
| `<depth topic>.compressedDepth.depth_quantization` | `double` | `100.0` | Same, depth quantization of a 32FC1 depth image. |
|
||||
|
||||
## Stereo messages
|
||||
|
||||
@@ -92,6 +101,27 @@ Only the names change — the message contents and the order of the two halves a
|
||||
|
||||
The node checks the setting against what actually arrives, going by the encoding of the second half: `16UC1`, `32FC1` and `mono16` are depth, anything else is an image. If the two disagree it logs a warning **once** and keeps forwarding — a mismatch makes the topic name misleading, not the data wrong, so it is never worth dropping a frame over.
|
||||
|
||||
## Compressed images
|
||||
|
||||
`rgb_compressed` of an `RGBDImage` holds a JPEG or PNG image, and so does `depth_compressed` for the right image of a stereo pair. With `compressed_passthrough` (the default), the node republishes them as they are on `<rgbd_image>/rgb/image/compressed` (and `<rgbd_image>/right/image/compressed`), readable with the `compressed` transport. Their format is set from the image header, with the encoding (e.g. `"bgr8; jpeg compressed bgr8"`), which producers before 0.24 did not set. Raw images, or images whose header cannot be read, are compressed with `<color topic>.compressed.format`.
|
||||
|
||||
`depth_compressed` of an `RGBDImage` holds depth in `compressed_depth_image_transport`'s format, or, from rtabmap_ros before 0.24 or with `depth_compression_format: "legacy"`, in rtabmap's own format (`png`, `rvl`, `png:<max>:<q>`, `rvl:<max>:<q>`), which image_transport plugins cannot read. The node republishes both on `<rgbd_image>/depth/image/compressedDepth`, readable with the `compressedDepth` transport:
|
||||
|
||||
```bash
|
||||
ros2 run image_transport republish compressedDepth raw --ros-args \
|
||||
-r in/compressedDepth:=/camera/rgbd_image/depth/image/compressedDepth -r out:=/depth
|
||||
```
|
||||
|
||||
With `compressed_depth_passthrough` (the default), the depth is not decompressed when that can be avoided:
|
||||
|
||||
| Input depth | `compressedDepth` output |
|
||||
|---|---|
|
||||
| `compressed_depth_image_transport` format | Republished as is (RVL re-compressed as PNG before Jazzy). |
|
||||
| rtabmap's legacy `png` (16UC1), `rvl`, `png:<max>:<q>`, `rvl:<max>:<q>` | Only the header is converted, the compressed payload is copied. Before Jazzy, `compressed_depth_image_transport` cannot decode RVL, so RVL payloads are re-compressed as PNG, losslessly. |
|
||||
| Raw, or rtabmap's legacy `png` of 32FC1 depth (4 channels) | Compressed with the `<depth topic>.compressedDepth.*` parameters. |
|
||||
|
||||
The passthrough does not support the plugins' `jpeg_quality` and `png_level` parameters: to set them, set `compressed_passthrough: false` (`compressed` plugin) or `compressed_depth_passthrough: false` (`compressedDepth` plugin), and the plugin decompresses and re-compresses every frame instead. `compressed_depth_passthrough` follows `compressed_passthrough` unless set, so e.g. `compressed_passthrough: false` with `compressed_depth_passthrough: true` sets `jpeg_quality` of the color image while still passing the depth through.
|
||||
|
||||
## Notes
|
||||
|
||||
If a message has no `frame_id` on one of its sub-messages, the node fills it in from the other one so the output is always usable by TF.
|
||||
|
||||
@@ -45,6 +45,9 @@ private:
|
||||
private:
|
||||
bool compress_;
|
||||
bool uncompress_;
|
||||
std::string depthCompressionFormat_;
|
||||
/// Format of the compressed color (and right) images: ".jpg" or ".png".
|
||||
std::string imageCompressionFormat_ = ".jpg";
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
|
||||
};
|
||||
|
||||
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <image_transport/image_transport.hpp>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
@@ -55,6 +56,32 @@ private:
|
||||
|
||||
image_transport::Publisher rgbPub_;
|
||||
image_transport::Publisher depthPub_;
|
||||
/// Replace the "compressed" image_transport plugin of rgbPub_, and of depthPub_ with
|
||||
/// "stereo" (right image), so that images already compressed in the RGBDImage are
|
||||
/// republished without being decompressed. Null if "compressed_passthrough" is false.
|
||||
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressedRgbPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressedRightPub_;
|
||||
/// Format used when an image has to be compressed: "jpeg" or "png".
|
||||
std::string compressedImageFormat_;
|
||||
/// Same for the "compressedDepth" plugin of depthPub_, without "stereo". Null if
|
||||
/// "compressed_depth_passthrough" is false.
|
||||
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressedDepthPub_;
|
||||
/// Format used when depth has to be compressed (raw input, or compressed in a format
|
||||
/// that compressed_depth_image_transport cannot read), same parameters as the plugin.
|
||||
std::string compressedDepthFormat_;
|
||||
double compressedDepthMax_;
|
||||
double compressedDepthQuantization_;
|
||||
|
||||
/// Publishes the image (color, left or right) of @p raw or @p compressed on @p rawPub
|
||||
/// and on @p compressedPub, without decompressing it when possible.
|
||||
void publishImage(
|
||||
const sensor_msgs::msg::Image & raw,
|
||||
const sensor_msgs::msg::CompressedImage & compressed,
|
||||
const std_msgs::msg::Header & defaultHeader,
|
||||
const rtabmap_msgs::msg::RGBDImage::SharedPtr & input,
|
||||
const image_transport::Publisher & rawPub,
|
||||
const rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr & compressedPub,
|
||||
sensor_msgs::msg::Image & outputImage) const;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
|
||||
};
|
||||
|
||||
@@ -37,6 +37,11 @@
|
||||
<depend>grid_map_ros</depend>
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_index_cpp</test_depend>
|
||||
<test_depend>class_loader</test_depend>
|
||||
<!-- image_transport plugins the tests read rgbd_split's outputs with -->
|
||||
<test_depend>compressed_image_transport</test_depend>
|
||||
<test_depend>compressed_depth_image_transport</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
|
||||
@@ -51,7 +51,8 @@ namespace rtabmap_util
|
||||
RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_relay", options),
|
||||
compress_(false),
|
||||
uncompress_(false)
|
||||
uncompress_(false),
|
||||
depthCompressionFormat_(".png")
|
||||
{
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
@@ -63,6 +64,19 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
|
||||
int queuePub = this->declare_parameter("queue_pub", 1);
|
||||
compress_ = this->declare_parameter("compress", compress_);
|
||||
uncompress_ = this->declare_parameter("uncompress", uncompress_);
|
||||
imageCompressionFormat_ = this->declare_parameter("image_compression_format", imageCompressionFormat_);
|
||||
if(!rtabmap_conversions::isValidImageCompressionFormat(imageCompressionFormat_))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid image_compression_format \"%s\" (should be \".jpg\" or \".png\"), using \".jpg\".", imageCompressionFormat_.c_str());
|
||||
imageCompressionFormat_ = ".jpg";
|
||||
}
|
||||
depthCompressionFormat_ = this->declare_parameter("depth_compression_format", depthCompressionFormat_);
|
||||
if(!rtabmap_conversions::isValidDepthCompressionFormat(depthCompressionFormat_))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid depth_compression_format \"%s\" (should be \".png\" or \".rvl\", "
|
||||
"optionally followed by \":maxDepth[:quantization]\", optionally prefixed by \"legacy:\", or \"legacy\"), using \".png\".", depthCompressionFormat_.c_str());
|
||||
depthCompressionFormat_ = ".png";
|
||||
}
|
||||
|
||||
UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
|
||||
uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
|
||||
@@ -103,7 +117,7 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
else if(!input->rgb.data.empty())
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
|
||||
rgb->toCompressedImageMsg(output->rgb_compressed, cv_bridge::JPG);
|
||||
rtabmap_conversions::toCompressedImageMsg(*rgb, imageCompressionFormat_, output->rgb_compressed);
|
||||
}
|
||||
|
||||
if(!input->depth_compressed.data.empty())
|
||||
@@ -117,14 +131,14 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
{
|
||||
// right stereo image
|
||||
cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
|
||||
imageRightPtr->toCompressedImageMsg(output->depth_compressed, cv_bridge::JPG);
|
||||
rtabmap_conversions::toCompressedImageMsg(*imageRightPtr, imageCompressionFormat_, output->depth_compressed);
|
||||
}
|
||||
else
|
||||
{
|
||||
// depth image
|
||||
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
|
||||
output->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
|
||||
output->depth_compressed.format = "png";
|
||||
output->depth_compressed.header = input->depth.header;
|
||||
rtabmap_conversions::compressDepthImage(imageDepthPtr->image, depthCompressionFormat_, output->depth_compressed);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -147,34 +161,10 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
}
|
||||
else if(!input->depth_compressed.data.empty())
|
||||
{
|
||||
// Decode first, then pick the encoding from what actually came out.
|
||||
// Branching on the "jpg"/"png" format string instead would abort on a
|
||||
// right image compressed as PNG, which nothing forbids.
|
||||
auto cvImg = std::make_unique<cv_bridge::CvImage>();
|
||||
cvImg->header = input->depth_compressed.header;
|
||||
cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data);
|
||||
if(cvImg->image.empty())
|
||||
cv_bridge::CvImagePtr cvImg = rtabmap_conversions::uncompressDepthImage(input->depth_compressed);
|
||||
if(!cvImg->image.empty())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").",
|
||||
rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(cvImg->image.type())
|
||||
{
|
||||
case CV_32FC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; break;
|
||||
case CV_16UC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_16UC1; break;
|
||||
case CV_8UC1: cvImg->encoding = sensor_msgs::image_encodings::MONO8; break;
|
||||
case CV_8UC3: cvImg->encoding = sensor_msgs::image_encodings::BGR8; break;
|
||||
default:
|
||||
RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg->image.type());
|
||||
cvImg->image = cv::Mat();
|
||||
break;
|
||||
}
|
||||
if(!cvImg->image.empty())
|
||||
{
|
||||
cvImg->toImageMsg(output->depth);
|
||||
}
|
||||
cvImg->toImageMsg(output->depth);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -27,9 +27,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include <rtabmap_util/rgbd_split.hpp>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
@@ -42,7 +45,11 @@ namespace rtabmap_util
|
||||
|
||||
RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_split", options),
|
||||
stereo_(false)
|
||||
stereo_(false),
|
||||
compressedImageFormat_("jpeg"),
|
||||
compressedDepthFormat_("png"),
|
||||
compressedDepthMax_(10.0),
|
||||
compressedDepthQuantization_(100.0)
|
||||
{
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
@@ -55,11 +62,18 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
|
||||
// A stereo RGBDImage carries the right image in the depth slot, so name the outputs
|
||||
// left/right instead of rgb/depth to say what they really are.
|
||||
stereo_ = this->declare_parameter("stereo", false);
|
||||
// Republish compressed images on the compressed and compressedDepth topics without
|
||||
// decompressing them. Depth can be set apart, e.g., to use the jpeg_quality parameter
|
||||
// of the compressed plugin while still passing the depth through.
|
||||
bool compressedPassthrough = this->declare_parameter("compressed_passthrough", true);
|
||||
bool compressedDepthPassthrough = this->declare_parameter("compressed_depth_passthrough", compressedPassthrough);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: queue_sub = %d", get_name(), queueSub);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: queue_pub = %d", get_name(), queuePub);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: stereo = %s", get_name(), stereo_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "%s: compressed_passthrough = %s", get_name(), compressedPassthrough?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "%s: compressed_depth_passthrough = %s", get_name(), compressedDepthPassthrough?"true":"false");
|
||||
|
||||
UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
|
||||
uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
|
||||
@@ -71,12 +85,107 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
|
||||
const std::string secondName = stereo_?"/right":"/depth";
|
||||
const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub);
|
||||
|
||||
const std::string rgbTopic = base + firstName + "/image";
|
||||
const std::string depthTopic = base + secondName + "/image";
|
||||
|
||||
// Parameter namespace image_transport gives to a topic (see
|
||||
// image_transport::Publisher), e.g., "rgbd_image.depth.image".
|
||||
const auto paramBaseOf = [this](const std::string & topic) {
|
||||
std::string paramBase = topic.substr(std::min(topic.size(), this->get_effective_namespace().size()));
|
||||
std::replace(paramBase.begin(), paramBase.end(), '/', '.');
|
||||
if(!paramBase.empty() && paramBase.front() == '.')
|
||||
{
|
||||
paramBase = paramBase.substr(1);
|
||||
}
|
||||
return paramBase;
|
||||
};
|
||||
// Disables the image_transport @p transport plugin of @p topic, as we publish on its
|
||||
// topic instead. image_transport reads the list of plugins from this parameter when
|
||||
// the publisher is created: the plugin is removed from it even if set by the user,
|
||||
// otherwise both would publish on the same topic.
|
||||
const auto removePlugin = [this, ¶mBaseOf](const std::string & topic, const std::string & transport, const std::string & passthroughParam) {
|
||||
const std::string name = paramBaseOf(topic) + ".enable_pub_plugins";
|
||||
std::vector<std::string> plugins;
|
||||
for(const std::string & loadable : image_transport::getLoadableTransports())
|
||||
{
|
||||
if(loadable != transport)
|
||||
{
|
||||
plugins.push_back(loadable);
|
||||
}
|
||||
}
|
||||
plugins = this->declare_parameter(name, plugins);
|
||||
const auto plugin = std::find(plugins.begin(), plugins.end(), transport);
|
||||
if(plugin != plugins.end())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "%s: %s is removed from %s, as %s is true: the node "
|
||||
"publishes that topic itself. Set %s to false to use the plugin.",
|
||||
get_name(), transport.c_str(), name.c_str(), passthroughParam.c_str(), passthroughParam.c_str());
|
||||
plugins.erase(plugin);
|
||||
this->set_parameter(rclcpp::Parameter(name, plugins));
|
||||
}
|
||||
};
|
||||
|
||||
if(compressedPassthrough)
|
||||
{
|
||||
const std::string kCompressed = "image_transport/compressed";
|
||||
// Same parameter as compressed_image_transport's publisher ("jpeg_quality" and
|
||||
// "png_level" are not supported), read from the color topic.
|
||||
const std::string cBase = paramBaseOf(rgbTopic) + ".compressed.";
|
||||
compressedImageFormat_ = this->declare_parameter(cBase + "format", compressedImageFormat_);
|
||||
if(compressedImageFormat_ != "jpeg" && compressedImageFormat_ != "png")
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "%sformat should be \"jpeg\" or \"png\" (\"%s\"), using \"jpeg\".",
|
||||
cBase.c_str(), compressedImageFormat_.c_str());
|
||||
compressedImageFormat_ = "jpeg";
|
||||
}
|
||||
removePlugin(rgbTopic, kCompressed, "compressed_passthrough");
|
||||
compressedRgbPub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(rgbTopic + "/compressed", pubQos);
|
||||
if(stereo_)
|
||||
{
|
||||
removePlugin(depthTopic, kCompressed, "compressed_passthrough");
|
||||
compressedRightPub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(depthTopic + "/compressed", pubQos);
|
||||
}
|
||||
}
|
||||
|
||||
if(!stereo_ && compressedDepthPassthrough)
|
||||
{
|
||||
removePlugin(depthTopic, "image_transport/compressedDepth", "compressed_depth_passthrough");
|
||||
// Same parameters as compressed_depth_image_transport's publisher
|
||||
// ("png_level" is not supported).
|
||||
const std::string cdBase = paramBaseOf(depthTopic) + ".compressedDepth.";
|
||||
compressedDepthFormat_ = this->declare_parameter(cdBase + "format", compressedDepthFormat_);
|
||||
compressedDepthMax_ = this->declare_parameter(cdBase + "depth_max", compressedDepthMax_);
|
||||
compressedDepthQuantization_ = this->declare_parameter(cdBase + "depth_quantization", compressedDepthQuantization_);
|
||||
#ifdef PRE_ROS_JAZZY
|
||||
if(compressedDepthFormat_ == "rvl")
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "%sformat \"rvl\" cannot be decoded by compressed_depth_image_transport "
|
||||
"before ROS Jazzy, using \"png\".", cdBase.c_str());
|
||||
compressedDepthFormat_ = "png";
|
||||
}
|
||||
#endif
|
||||
if(compressedDepthFormat_ != "png" && compressedDepthFormat_ != "rvl")
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "%sformat should be \"png\" or \"rvl\" (\"%s\"), using \"png\".",
|
||||
cdBase.c_str(), compressedDepthFormat_.c_str());
|
||||
compressedDepthFormat_ = "png";
|
||||
}
|
||||
if(compressedDepthMax_ <= 0.0 || compressedDepthQuantization_ <= 0.0)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "%sdepth_max (%f) and %sdepth_quantization (%f) should be positive, using 10 and 100.",
|
||||
cdBase.c_str(), compressedDepthMax_, cdBase.c_str(), compressedDepthQuantization_);
|
||||
compressedDepthMax_ = 10.0;
|
||||
compressedDepthQuantization_ = 100.0;
|
||||
}
|
||||
compressedDepthPub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(depthTopic + "/compressedDepth", pubQos);
|
||||
}
|
||||
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
rgbPub_ = image_transport::create_publisher(this, base + firstName + "/image", pubQos.get_rmw_qos_profile());
|
||||
depthPub_ = image_transport::create_publisher(this, base + secondName + "/image", pubQos.get_rmw_qos_profile());
|
||||
rgbPub_ = image_transport::create_publisher(this, rgbTopic, pubQos.get_rmw_qos_profile());
|
||||
depthPub_ = image_transport::create_publisher(this, depthTopic, pubQos.get_rmw_qos_profile());
|
||||
#else
|
||||
rgbPub_ = image_transport::create_publisher(*this, base + firstName + "/image", pubQos);
|
||||
depthPub_ = image_transport::create_publisher(*this, base + secondName + "/image", pubQos);
|
||||
rgbPub_ = image_transport::create_publisher(*this, rgbTopic, pubQos);
|
||||
depthPub_ = image_transport::create_publisher(*this, depthTopic, pubQos);
|
||||
#endif
|
||||
rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(base + firstName + "/camera_info", pubQos);
|
||||
depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(base + secondName + "/camera_info", pubQos);
|
||||
@@ -93,79 +202,173 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
|
||||
}
|
||||
|
||||
|
||||
void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
|
||||
void RGBDSplit::publishImage(
|
||||
const sensor_msgs::msg::Image & raw,
|
||||
const sensor_msgs::msg::CompressedImage & compressed,
|
||||
const std_msgs::msg::Header & defaultHeader,
|
||||
const rtabmap_msgs::msg::RGBDImage::SharedPtr & input,
|
||||
const image_transport::Publisher & rawPub,
|
||||
const rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr & compressedPub,
|
||||
sensor_msgs::msg::Image & outputImage) const
|
||||
{
|
||||
if(rgbPub_.getNumSubscribers())
|
||||
{
|
||||
sensor_msgs::msg::Image outputImage;
|
||||
sensor_msgs::msg::CameraInfo outputCameraInfo;
|
||||
outputImage.header = outputCameraInfo.header = input->header;
|
||||
outputCameraInfo = input->rgb_camera_info;
|
||||
const bool rawSubscribed = rawPub.getNumSubscribers() > 0;
|
||||
const bool compressedSubscribed = compressedPub && compressedPub->get_subscription_count() > 0;
|
||||
|
||||
if(!input->rgb.data.empty())
|
||||
// compressed without decompression, when compressed_image_transport can read it
|
||||
sensor_msgs::msg::CompressedImage outputCompressed;
|
||||
bool compressedReady = false;
|
||||
if(compressedSubscribed && raw.data.empty() && !compressed.data.empty())
|
||||
{
|
||||
const std::string format = rtabmap_conversions::compressedImageTransportFormat(compressed.data);
|
||||
if(!format.empty())
|
||||
{
|
||||
outputCompressed = compressed;
|
||||
outputCompressed.format = format; // with the encoding, which older producers did not set
|
||||
compressedReady = true;
|
||||
}
|
||||
}
|
||||
|
||||
outputImage.header = defaultHeader;
|
||||
cv_bridge::CvImageConstPtr image;
|
||||
if(!raw.data.empty())
|
||||
{
|
||||
if(rawSubscribed)
|
||||
{
|
||||
// already raw, just copy pointer
|
||||
outputImage = input->rgb;
|
||||
outputImage = raw;
|
||||
}
|
||||
else if(!input->rgb_compressed.data.empty())
|
||||
else
|
||||
{
|
||||
#ifdef CV_BRIDGE_HYDRO
|
||||
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
|
||||
#else
|
||||
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
|
||||
#endif
|
||||
outputImage.header = raw.header;
|
||||
}
|
||||
rgbPub_.publish(outputImage);
|
||||
if(compressedSubscribed)
|
||||
{
|
||||
image = cv_bridge::toCvShare(raw, input);
|
||||
}
|
||||
}
|
||||
else if(!compressed.data.empty() && (rawSubscribed || !compressedReady))
|
||||
{
|
||||
cv_bridge::CvImagePtr decoded = cv_bridge::toCvCopy(compressed);
|
||||
decoded->toImageMsg(outputImage);
|
||||
image = decoded;
|
||||
}
|
||||
else if(!compressed.data.empty())
|
||||
{
|
||||
outputImage.header = compressed.header;
|
||||
}
|
||||
if(outputImage.header.frame_id.empty())
|
||||
{
|
||||
outputImage.header = defaultHeader;
|
||||
}
|
||||
|
||||
if(rawSubscribed)
|
||||
{
|
||||
rawPub.publish(outputImage);
|
||||
}
|
||||
if(compressedSubscribed)
|
||||
{
|
||||
if(!compressedReady && image)
|
||||
{
|
||||
compressedReady = rtabmap_conversions::toCompressedImageMsg(*image, compressedImageFormat_, outputCompressed);
|
||||
}
|
||||
if(compressedReady)
|
||||
{
|
||||
outputCompressed.header = outputImage.header;
|
||||
compressedPub->publish(outputCompressed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
|
||||
{
|
||||
if(rgbPub_.getNumSubscribers() || (compressedRgbPub_ && compressedRgbPub_->get_subscription_count()))
|
||||
{
|
||||
sensor_msgs::msg::Image outputImage;
|
||||
const sensor_msgs::msg::CameraInfo outputCameraInfo = input->rgb_camera_info;
|
||||
publishImage(input->rgb, input->rgb_compressed, input->header, input, rgbPub_, compressedRgbPub_, outputImage);
|
||||
rgbInfoPub_->publish(outputCameraInfo);
|
||||
}
|
||||
|
||||
if(depthPub_.getNumSubscribers())
|
||||
const bool rawDepthSubscribed = depthPub_.getNumSubscribers() > 0;
|
||||
const bool compressedDepthSubscribed = compressedDepthPub_ && compressedDepthPub_->get_subscription_count() > 0;
|
||||
const bool compressedRightSubscribed = compressedRightPub_ && compressedRightPub_->get_subscription_count() > 0;
|
||||
if(rawDepthSubscribed || compressedDepthSubscribed || compressedRightSubscribed)
|
||||
{
|
||||
sensor_msgs::msg::Image outputImage;
|
||||
sensor_msgs::msg::CameraInfo outputCameraInfo;
|
||||
outputCameraInfo = input->depth_camera_info;
|
||||
|
||||
if(!input->depth.data.empty())
|
||||
cv::Mat depth;
|
||||
sensor_msgs::msg::CompressedImage outputCompressedDepth;
|
||||
bool compressedDepthReady = false;
|
||||
if(compressedRightPub_)
|
||||
{
|
||||
// already raw, just copy pointer
|
||||
outputImage = input->depth;
|
||||
// Right image of a stereo pair
|
||||
publishImage(input->depth, input->depth_compressed, input->header, input, depthPub_, compressedRightPub_, outputImage);
|
||||
}
|
||||
else if(!input->depth_compressed.data.empty())
|
||||
else
|
||||
{
|
||||
#ifdef CV_BRIDGE_HYDRO
|
||||
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
|
||||
#else
|
||||
// Decode first, then pick the encoding from what actually came out. Going
|
||||
// by the "jpg"/"png" format string instead would mislabel a depth PNG as
|
||||
// mono8 (cv_bridge cannot infer 16-bit from it), and would abort outright on
|
||||
// a right image compressed as PNG, which nothing forbids.
|
||||
cv_bridge::CvImage cvImg;
|
||||
cvImg.header = input->depth_compressed.header;
|
||||
cvImg.image = rtabmap::uncompressImage(input->depth_compressed.data);
|
||||
if(cvImg.image.empty())
|
||||
// compressedDepth without decompression, when the depth is already compressed
|
||||
// in a format compressed_depth_image_transport can read.
|
||||
if(compressedDepthSubscribed && input->depth.data.empty() && !input->depth_compressed.data.empty())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").",
|
||||
rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str());
|
||||
const bool rosFormat = input->depth_compressed.format.find("compressedDepth") != std::string::npos;
|
||||
#ifdef PRE_ROS_JAZZY
|
||||
const bool rvlSupported = false;
|
||||
#else
|
||||
const bool rvlSupported = true;
|
||||
#endif
|
||||
if(rosFormat && (rvlSupported || input->depth_compressed.format.find(" rvl") == std::string::npos))
|
||||
{
|
||||
outputCompressedDepth = input->depth_compressed;
|
||||
compressedDepthReady = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// RVL is re-compressed as PNG before Jazzy. Legacy 32FC1 format (4 channels
|
||||
// PNG) and right images have no compressedDepth equivalent, they are
|
||||
// re-compressed below.
|
||||
const cv::Mat bytes = rosFormat ?
|
||||
rtabmap_conversions::compressedDepthTransportToRtabmap(input->depth_compressed) :
|
||||
rtabmap_conversions::compressedMatFromBytes(input->depth_compressed.data, false);
|
||||
outputCompressedDepth.header = input->depth_compressed.header;
|
||||
compressedDepthReady = rtabmap_conversions::rtabmapToCompressedDepthTransport(bytes, outputCompressedDepth, false);
|
||||
}
|
||||
}
|
||||
|
||||
if(!input->depth.data.empty())
|
||||
{
|
||||
if(rawDepthSubscribed)
|
||||
{
|
||||
// already raw, just copy pointer
|
||||
outputImage = input->depth;
|
||||
}
|
||||
else
|
||||
{
|
||||
outputImage.header = input->depth.header;
|
||||
}
|
||||
if(compressedDepthSubscribed)
|
||||
{
|
||||
depth = cv_bridge::toCvShare(input->depth, input)->image;
|
||||
}
|
||||
}
|
||||
else if(!input->depth_compressed.data.empty() && (rawDepthSubscribed || !compressedDepthReady))
|
||||
{
|
||||
#ifdef CV_BRIDGE_HYDRO
|
||||
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
|
||||
#else
|
||||
cv_bridge::CvImagePtr cvImg = rtabmap_conversions::uncompressDepthImage(input->depth_compressed);
|
||||
if(!cvImg->image.empty())
|
||||
{
|
||||
cvImg->toImageMsg(outputImage);
|
||||
depth = cvImg->image;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(cvImg.image.type())
|
||||
{
|
||||
case CV_32FC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; break;
|
||||
case CV_16UC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_16UC1; break;
|
||||
case CV_8UC1: cvImg.encoding = sensor_msgs::image_encodings::MONO8; break;
|
||||
case CV_8UC3: cvImg.encoding = sensor_msgs::image_encodings::BGR8; break;
|
||||
default:
|
||||
RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg.image.type());
|
||||
cvImg.image = cv::Mat();
|
||||
break;
|
||||
}
|
||||
outputImage.header = input->depth_compressed.header;
|
||||
}
|
||||
if(!cvImg.image.empty())
|
||||
{
|
||||
cvImg.toImageMsg(outputImage);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
if(outputCameraInfo.header.frame_id.empty()) {
|
||||
if(outputImage.header.frame_id.empty()) {
|
||||
@@ -214,7 +417,35 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
}
|
||||
}
|
||||
|
||||
depthPub_.publish(outputImage);
|
||||
if(!compressedRightPub_) // else already published
|
||||
{
|
||||
if(rawDepthSubscribed)
|
||||
{
|
||||
depthPub_.publish(outputImage);
|
||||
}
|
||||
if(compressedDepthSubscribed)
|
||||
{
|
||||
if(!compressedDepthReady && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1))
|
||||
{
|
||||
const std::string format = depth.type() == CV_32FC1 ?
|
||||
uFormat(".%s:%g:%g", compressedDepthFormat_.c_str(), compressedDepthMax_, compressedDepthQuantization_) :
|
||||
"." + compressedDepthFormat_;
|
||||
// Same as compressed_depth_image_transport: inverse depth for 32FC1
|
||||
compressedDepthReady = rtabmap_conversions::compressDepthImage(depth, format, outputCompressedDepth);
|
||||
}
|
||||
else if(!compressedDepthReady && !depth.empty())
|
||||
{
|
||||
RCLCPP_WARN_ONCE(this->get_logger(), "Cannot publish \"%s\" as compressedDepth, it is "
|
||||
"not a depth image (type=%d). (This warning is printed only once)",
|
||||
compressedDepthPub_->get_topic_name(), depth.type());
|
||||
}
|
||||
if(compressedDepthReady)
|
||||
{
|
||||
outputCompressedDepth.header = outputImage.header;
|
||||
compressedDepthPub_->publish(outputCompressedDepth);
|
||||
}
|
||||
}
|
||||
}
|
||||
depthInfoPub_->publish(outputCameraInfo);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,417 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
/**
|
||||
* Round trip of raw images through rgbd_sync (compressed output) and rgbd_split, read
|
||||
* back through image_transport (raw, and the "compressed" and "compressedDepth" plugins),
|
||||
* compared to the original images, for each image and depth compression format. Same
|
||||
* for a stereo pair through stereo_sync and rgbd_split ("stereo").
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/rgbd_split.hpp>
|
||||
|
||||
#include <ament_index_cpp/get_resource.hpp>
|
||||
#include <class_loader/class_loader.hpp>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <rclcpp_components/node_factory.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
constexpr int kWidth = 64;
|
||||
constexpr int kHeight = 48;
|
||||
|
||||
struct PipelineCase
|
||||
{
|
||||
std::string name;
|
||||
int depthType; ///< CV_16UC1 (mm) or CV_32FC1 (m)
|
||||
std::string imageFormat; ///< rgbd_sync's image_compression_format
|
||||
std::string depthFormat; ///< rgbd_sync's depth_compression_format
|
||||
};
|
||||
|
||||
/**
|
||||
* Creates a node registered as a component (RCLCPP_COMPONENTS_REGISTER_NODE) by another
|
||||
* package, the way component_container does: rtabmap_sync does not install the headers
|
||||
* of its nodes.
|
||||
*/
|
||||
rclcpp::Node::SharedPtr loadComponent(
|
||||
const std::string & package, const std::string & className, const rclcpp::NodeOptions & options)
|
||||
{
|
||||
std::string content, basePath;
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
if(!ament_index_cpp::get_resource("rclcpp_components", package, content, &basePath))
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
#else
|
||||
// The overload above is deprecated in Lyrical and removed in Rolling
|
||||
const ament_index_cpp::PathWithResource resource = ament_index_cpp::get_resource("rclcpp_components", package);
|
||||
if(!resource.resourcePath)
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
content = resource.contents;
|
||||
basePath = resource.resourcePath->string();
|
||||
#endif
|
||||
std::istringstream lines(content);
|
||||
std::string line;
|
||||
while(std::getline(lines, line))
|
||||
{
|
||||
const size_t split = line.find(';');
|
||||
if(split == std::string::npos || line.substr(0, split) != className)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
// Never unloaded: the node may outlive any owner of the loader in the test.
|
||||
static std::vector<class_loader::ClassLoader *> loaders;
|
||||
loaders.push_back(new class_loader::ClassLoader(basePath + "/" + line.substr(split + 1)));
|
||||
std::shared_ptr<rclcpp_components::NodeFactory> factory =
|
||||
loaders.back()->createInstance<rclcpp_components::NodeFactory>(
|
||||
"rclcpp_components::NodeFactoryTemplate<" + className + ">");
|
||||
rclcpp_components::NodeInstanceWrapper wrapper = factory->create_node_instance(options);
|
||||
// The component class derives from rclcpp::Node only, at the start of the object.
|
||||
return std::static_pointer_cast<rclcpp::Node>(wrapper.get_node_instance());
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/// A smooth color gradient, so that JPEG stays close to it.
|
||||
cv::Mat colorImage()
|
||||
{
|
||||
cv::Mat image(kHeight, kWidth, CV_8UC3);
|
||||
for(int r = 0; r < kHeight; ++r)
|
||||
{
|
||||
for(int c = 0; c < kWidth; ++c)
|
||||
{
|
||||
image.at<cv::Vec3b>(r, c) = cv::Vec3b(uchar(4*c), uchar(5*r), uchar(2*(r+c)));
|
||||
}
|
||||
}
|
||||
return image;
|
||||
}
|
||||
|
||||
/// A depth ramp from 0.5 to 8 m, with invalid (0) pixels on the first row.
|
||||
cv::Mat depthImage(int type)
|
||||
{
|
||||
cv::Mat depth(kHeight, kWidth, type);
|
||||
for(int r = 0; r < kHeight; ++r)
|
||||
{
|
||||
for(int c = 0; c < kWidth; ++c)
|
||||
{
|
||||
const float meters = r == 0 && c < 8 ? 0.0f :
|
||||
0.5f + 7.5f * float(r * kWidth + c) / float(kWidth * kHeight);
|
||||
if(type == CV_16UC1)
|
||||
{
|
||||
depth.at<uint16_t>(r, c) = uint16_t(meters * 1000.0f + 0.5f);
|
||||
}
|
||||
else
|
||||
{
|
||||
depth.at<float>(r, c) = meters;
|
||||
}
|
||||
}
|
||||
}
|
||||
return depth;
|
||||
}
|
||||
|
||||
/// Depth in meters at (r, c), 0 when invalid (0 or NaN).
|
||||
float meters(const cv::Mat & depth, int r, int c)
|
||||
{
|
||||
const float d = depth.type() == CV_16UC1 ? depth.at<uint16_t>(r, c) * 0.001f : depth.at<float>(r, c);
|
||||
return std::isfinite(d) ? d : 0.0f;
|
||||
}
|
||||
|
||||
class RGBDPipelineTest : public NodeTest, public ::testing::WithParamInterface<PipelineCase>
|
||||
{
|
||||
protected:
|
||||
using ImagePtr = sensor_msgs::msg::Image::ConstSharedPtr;
|
||||
|
||||
image_transport::Subscriber subscribe(const std::string & topic, const std::string & transport, std::vector<ImagePtr> & out)
|
||||
{
|
||||
const auto callback = [&out](const ImagePtr & msg) { out.push_back(msg); };
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
return image_transport::create_subscription(helper().get(), topic, callback, transport);
|
||||
#else
|
||||
return image_transport::create_subscription(*helper(), topic, callback, transport, rclcpp::QoS(10), rclcpp::SubscriptionOptions());
|
||||
#endif
|
||||
}
|
||||
|
||||
/// What reaches the consumer of rgbd_split for the original images.
|
||||
void run(const PipelineCase & cs)
|
||||
{
|
||||
rclcpp::Node::SharedPtr sync = loadComponent("rtabmap_sync", "rtabmap_sync::RGBDSync",
|
||||
rclcpp::NodeOptions().parameter_overrides({
|
||||
rclcpp::Parameter("approx_sync", false),
|
||||
rclcpp::Parameter("image_compression_format", cs.imageFormat),
|
||||
rclcpp::Parameter("depth_compression_format", cs.depthFormat)}));
|
||||
ASSERT_TRUE(sync) << "rtabmap_sync::RGBDSync component not found";
|
||||
addNode(sync);
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()
|
||||
.arguments({"--ros-args", "-r", "rgbd_image:=rgbd_image/compressed"})));
|
||||
|
||||
const std::string rgbTopic = "rgbd_image/compressed/rgb/image";
|
||||
const std::string depthTopic = "rgbd_image/compressed/depth/image";
|
||||
image_transport::Subscriber subs[] = {
|
||||
subscribe(rgbTopic, "raw", rgbRaw_),
|
||||
subscribe(rgbTopic, "compressed", rgbCompressed_),
|
||||
subscribe(depthTopic, "raw", depthRaw_),
|
||||
subscribe(depthTopic, "compressedDepth", depthCompressed_)};
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(rgbPub));
|
||||
ASSERT_TRUE(waitForSubscriber(depthPub));
|
||||
ASSERT_TRUE(waitForSubscriber(infoPub));
|
||||
for(const image_transport::Subscriber & sub : subs)
|
||||
{
|
||||
ASSERT_TRUE(spinUntil([&]() { return sub.getNumPublishers() > 0; })) << sub.getTopic();
|
||||
}
|
||||
|
||||
color_ = colorImage();
|
||||
depth_ = depthImage(cs.depthType);
|
||||
rgbPub->publish(makeImage("camera_link", 1000.0, color_, sensor_msgs::image_encodings::BGR8));
|
||||
depthPub->publish(makeImage("camera_link", 1000.0, depth_,
|
||||
cs.depthType == CV_16UC1 ? sensor_msgs::image_encodings::TYPE_16UC1 : sensor_msgs::image_encodings::TYPE_32FC1));
|
||||
infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight));
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return !rgbRaw_.empty() && !rgbCompressed_.empty() && !depthRaw_.empty() && !depthCompressed_.empty();
|
||||
}));
|
||||
}
|
||||
|
||||
/// The color image read back is the original, exactly in PNG, closely in JPEG.
|
||||
void expectColor(const ImagePtr & msg, const std::string & imageFormat)
|
||||
{
|
||||
SCOPED_TRACE(msg->encoding);
|
||||
const cv::Mat image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
|
||||
ASSERT_EQ(image.size(), color_.size());
|
||||
if(imageFormat == ".png")
|
||||
{
|
||||
EXPECT_EQ(cv::norm(image, color_, cv::NORM_INF), 0.0) << "lossless";
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_LT(cv::norm(image, color_, cv::NORM_L1) / double(image.total() * image.channels()), 3.0)
|
||||
<< "mean error of JPEG";
|
||||
}
|
||||
}
|
||||
|
||||
/// The depth image read back is the original within @p tolerance(meters), with invalid
|
||||
/// pixels (0 or NaN) where the original is invalid, and in @p type.
|
||||
template<typename Tolerance>
|
||||
void expectDepth(const ImagePtr & msg, int type, Tolerance tolerance)
|
||||
{
|
||||
SCOPED_TRACE(msg->encoding);
|
||||
const cv::Mat depth = cv_bridge::toCvCopy(msg)->image;
|
||||
ASSERT_EQ(depth.type(), type);
|
||||
ASSERT_EQ(depth.size(), depth_.size());
|
||||
for(int r = 0; r < kHeight; ++r)
|
||||
{
|
||||
for(int c = 0; c < kWidth; ++c)
|
||||
{
|
||||
const float expected = meters(depth_, r, c);
|
||||
const float got = meters(depth, r, c);
|
||||
if(expected == 0.0f)
|
||||
{
|
||||
ASSERT_EQ(got, 0.0f) << "invalid at " << r << "," << c;
|
||||
}
|
||||
else
|
||||
{
|
||||
ASSERT_NEAR(got, expected, tolerance(expected)) << "at " << r << "," << c;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cv::Mat color_;
|
||||
cv::Mat depth_;
|
||||
std::vector<ImagePtr> rgbRaw_;
|
||||
std::vector<ImagePtr> rgbCompressed_;
|
||||
std::vector<ImagePtr> depthRaw_;
|
||||
std::vector<ImagePtr> depthCompressed_;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_P(RGBDPipelineTest, ImagesReadBackAsSent)
|
||||
{
|
||||
const PipelineCase & cs = GetParam();
|
||||
ASSERT_NO_FATAL_FAILURE(run(cs));
|
||||
|
||||
expectColor(rgbRaw_.back(), cs.imageFormat);
|
||||
expectColor(rgbCompressed_.back(), cs.imageFormat);
|
||||
|
||||
const auto exact = [](float) { return 1e-6f; };
|
||||
const auto millimeters = [](float) { return 0.0005f + 1e-5f; };
|
||||
// Inverse depth quantization: half a step, or a whole one when truncated (the
|
||||
// compressed_depth_image_transport plugin, used by rgbd_split to re-compress)
|
||||
const auto inverseDepth = [](float d) { return 1.02f * d * d / (100.0f * 101.0f) + 1e-6f; };
|
||||
|
||||
// "legacy:<format>": rtabmap's own format, with or without inverse depth parameters
|
||||
const bool legacy = cs.depthFormat.compare(0, 6, "legacy") == 0;
|
||||
const std::string format = !legacy ? cs.depthFormat : cs.depthFormat == "legacy" ? ".png" : cs.depthFormat.substr(7);
|
||||
const bool inverse = format.find(':') != std::string::npos;
|
||||
if(cs.depthType == CV_16UC1)
|
||||
{
|
||||
// Always lossless
|
||||
expectDepth(depthRaw_.back(), CV_16UC1, exact);
|
||||
expectDepth(depthCompressed_.back(), CV_16UC1, exact);
|
||||
}
|
||||
else if(inverse)
|
||||
{
|
||||
expectDepth(depthRaw_.back(), CV_32FC1, inverseDepth);
|
||||
expectDepth(depthCompressed_.back(), CV_32FC1, inverseDepth);
|
||||
}
|
||||
else if(legacy)
|
||||
{
|
||||
// Lossless in rtabmap's legacy format, which rgbd_split has to compress again for
|
||||
// compressedDepth, with its default inverse depth quantization (10 m, 100)
|
||||
expectDepth(depthRaw_.back(), CV_32FC1, exact);
|
||||
expectDepth(depthCompressed_.back(), CV_32FC1, inverseDepth);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 32FC1 compressed in millimeters
|
||||
expectDepth(depthRaw_.back(), CV_16UC1, millimeters);
|
||||
expectDepth(depthCompressed_.back(), CV_16UC1, millimeters);
|
||||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
Formats,
|
||||
RGBDPipelineTest,
|
||||
::testing::Values(
|
||||
PipelineCase{"defaults_16UC1", CV_16UC1, ".jpg", ".png"},
|
||||
PipelineCase{"defaults_32FC1", CV_32FC1, ".jpg", ".png"},
|
||||
PipelineCase{"png_color", CV_16UC1, ".png", ".png"},
|
||||
PipelineCase{"rvl_16UC1", CV_16UC1, ".png", ".rvl"},
|
||||
PipelineCase{"rvl_32FC1", CV_32FC1, ".png", ".rvl"},
|
||||
PipelineCase{"inverse_depth_png", CV_32FC1, ".png", ".png:10:100"},
|
||||
PipelineCase{"inverse_depth_rvl", CV_32FC1, ".png", ".rvl:10:100"},
|
||||
PipelineCase{"legacy_16UC1", CV_16UC1, ".png", "legacy"},
|
||||
PipelineCase{"legacy_32FC1", CV_32FC1, ".png", "legacy"},
|
||||
PipelineCase{"legacy_rvl_16UC1", CV_16UC1, ".png", "legacy:.rvl"},
|
||||
PipelineCase{"legacy_rvl_32FC1", CV_32FC1, ".png", "legacy:.rvl"},
|
||||
PipelineCase{"legacy_inverse_depth", CV_32FC1, ".png", "legacy:.png:10:100"}),
|
||||
[](const ::testing::TestParamInfo<PipelineCase> & info) { return info.param.name; });
|
||||
|
||||
namespace {
|
||||
|
||||
/// The same round trip for a stereo pair: stereo_sync -> rgbd_split ("stereo"), with a
|
||||
/// color left image and a gray right image.
|
||||
class StereoPipelineTest : public NodeTest, public ::testing::WithParamInterface<std::string>
|
||||
{
|
||||
protected:
|
||||
using ImagePtr = sensor_msgs::msg::Image::ConstSharedPtr;
|
||||
|
||||
image_transport::Subscriber subscribe(const std::string & topic, const std::string & transport, std::vector<ImagePtr> & out)
|
||||
{
|
||||
const auto callback = [&out](const ImagePtr & msg) { out.push_back(msg); };
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
return image_transport::create_subscription(helper().get(), topic, callback, transport);
|
||||
#else
|
||||
return image_transport::create_subscription(*helper(), topic, callback, transport, rclcpp::QoS(10), rclcpp::SubscriptionOptions());
|
||||
#endif
|
||||
}
|
||||
|
||||
/// @p msg is @p original, exactly in PNG, closely in JPEG.
|
||||
static void expectImage(const ImagePtr & msg, const cv::Mat & original, const std::string & imageFormat)
|
||||
{
|
||||
SCOPED_TRACE(msg->encoding);
|
||||
const cv::Mat image = cv_bridge::toCvCopy(msg)->image;
|
||||
ASSERT_EQ(image.type(), original.type());
|
||||
ASSERT_EQ(image.size(), original.size());
|
||||
if(imageFormat == ".png")
|
||||
{
|
||||
EXPECT_EQ(cv::norm(image, original, cv::NORM_INF), 0.0) << "lossless";
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_LT(cv::norm(image, original, cv::NORM_L1) / double(image.total() * image.channels()), 3.0)
|
||||
<< "mean error of JPEG";
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_P(StereoPipelineTest, ImagesReadBackAsSent)
|
||||
{
|
||||
const std::string imageFormat = GetParam();
|
||||
rclcpp::Node::SharedPtr sync = loadComponent("rtabmap_sync", "rtabmap_sync::StereoSync",
|
||||
rclcpp::NodeOptions().parameter_overrides({
|
||||
rclcpp::Parameter("approx_sync", false),
|
||||
rclcpp::Parameter("image_compression_format", imageFormat)}));
|
||||
ASSERT_TRUE(sync) << "rtabmap_sync::StereoSync component not found";
|
||||
addNode(sync);
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()
|
||||
.arguments({"--ros-args", "-r", "rgbd_image:=rgbd_image/compressed"})
|
||||
.parameter_overrides({rclcpp::Parameter("stereo", true)})));
|
||||
|
||||
std::vector<ImagePtr> leftRaw, leftCompressed, rightRaw, rightCompressed;
|
||||
const std::string leftTopic = "rgbd_image/compressed/left/image";
|
||||
const std::string rightTopic = "rgbd_image/compressed/right/image";
|
||||
image_transport::Subscriber subs[] = {
|
||||
subscribe(leftTopic, "raw", leftRaw),
|
||||
subscribe(leftTopic, "compressed", leftCompressed),
|
||||
subscribe(rightTopic, "raw", rightRaw),
|
||||
subscribe(rightTopic, "compressed", rightCompressed)};
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("left/image_rect", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("right/image_rect", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(leftPub));
|
||||
ASSERT_TRUE(waitForSubscriber(rightPub));
|
||||
ASSERT_TRUE(waitForSubscriber(leftInfoPub));
|
||||
ASSERT_TRUE(waitForSubscriber(rightInfoPub));
|
||||
for(const image_transport::Subscriber & sub : subs)
|
||||
{
|
||||
ASSERT_TRUE(spinUntil([&]() { return sub.getNumPublishers() > 0; })) << sub.getTopic();
|
||||
}
|
||||
|
||||
const cv::Mat left = colorImage();
|
||||
cv::Mat right;
|
||||
cv::cvtColor(left, right, cv::COLOR_BGR2GRAY);
|
||||
cv::flip(right, right, 1);
|
||||
leftPub->publish(makeImage("camera_link", 1000.0, left, sensor_msgs::image_encodings::BGR8));
|
||||
rightPub->publish(makeImage("camera_link", 1000.0, right, sensor_msgs::image_encodings::MONO8));
|
||||
leftInfoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight));
|
||||
rightInfoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, -10.0)); // 10 cm baseline
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return !leftRaw.empty() && !leftCompressed.empty() && !rightRaw.empty() && !rightCompressed.empty();
|
||||
}));
|
||||
expectImage(leftRaw.back(), left, imageFormat);
|
||||
expectImage(leftCompressed.back(), left, imageFormat);
|
||||
expectImage(rightRaw.back(), right, imageFormat);
|
||||
expectImage(rightCompressed.back(), right, imageFormat);
|
||||
EXPECT_EQ(rightCompressed.back()->encoding, "mono8");
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
Formats,
|
||||
StereoPipelineTest,
|
||||
::testing::Values(std::string(".jpg"), std::string(".png")),
|
||||
[](const ::testing::TestParamInfo<std::string> & info) { return info.param.substr(1); });
|
||||
@@ -9,6 +9,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
#include <rtabmap_util/rgbd_relay.hpp>
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
@@ -21,12 +22,15 @@ class RGBDRelayTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
/// Starts the node, wires up the input publisher and the output collector.
|
||||
void start(bool compress, bool uncompress)
|
||||
void start(bool compress, bool uncompress, const std::string & depthCompressionFormat = ".png",
|
||||
const std::string & imageCompressionFormat = ".jpg")
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("compress", compress),
|
||||
rclcpp::Parameter("uncompress", uncompress)})));
|
||||
rclcpp::Parameter("uncompress", uncompress),
|
||||
rclcpp::Parameter("depth_compression_format", depthCompressionFormat),
|
||||
rclcpp::Parameter("image_compression_format", imageCompressionFormat)})));
|
||||
|
||||
out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
|
||||
pub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
@@ -65,8 +69,9 @@ TEST_F(RGBDRelayTest, CompressesRawImagesWhenAsked)
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
EXPECT_FALSE(got.rgb_compressed.data.empty()) << "rgb must be compressed";
|
||||
EXPECT_FALSE(got.depth_compressed.data.empty()) << "depth must be compressed";
|
||||
// Depth is lossless png; color is jpg.
|
||||
EXPECT_EQ(got.depth_compressed.format, "png");
|
||||
// Depth is lossless png in compressed_depth_image_transport's format; color is jpeg.
|
||||
EXPECT_EQ(got.depth_compressed.format, "16UC1; compressedDepth png");
|
||||
EXPECT_EQ(got.rgb_compressed.format, "bgr8; jpeg compressed bgr8");
|
||||
EXPECT_TRUE(got.rgb.data.empty()) << "the raw image is not carried as well";
|
||||
}
|
||||
|
||||
@@ -81,13 +86,26 @@ TEST_F(RGBDRelayTest, CompressesAStereoPairAsJpeg)
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth_compressed.data.empty());
|
||||
EXPECT_NE(got.depth_compressed.format, "png")
|
||||
<< "a stereo right image must not take the depth PNG path";
|
||||
EXPECT_EQ(got.depth_compressed.format, "mono8; jpeg compressed mono8")
|
||||
<< "a stereo right image must not take the depth path";
|
||||
EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos)
|
||||
<< "expected a jpeg format, got \"" << got.depth_compressed.format << "\"";
|
||||
EXPECT_LT(got.depth_camera_info.p[3], 0.0) << "the baseline must survive the relay";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, ImageCompressionFormatAppliesToColorAndRightImages)
|
||||
{
|
||||
start(/*compress=*/true, /*uncompress=*/false, ".png", ".png");
|
||||
|
||||
pub_->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
EXPECT_EQ(out_->back().rgb_compressed.format, "bgr8; png compressed bgr8");
|
||||
|
||||
pub_->publish(makeStereoRGBDImage("camera_link", 1001.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return out_->size() > 1; }));
|
||||
EXPECT_EQ(out_->back().depth_compressed.format, "mono8; png compressed mono8") << "right image";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng)
|
||||
{
|
||||
// The same call with no baseline is treated as color + depth instead.
|
||||
@@ -98,7 +116,7 @@ TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng)
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth_compressed.data.empty());
|
||||
EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless";
|
||||
EXPECT_EQ(got.depth_compressed.format, "16UC1; compressedDepth png") << "depth must stay lossless";
|
||||
EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], 0.0) << "no baseline: not stereo";
|
||||
}
|
||||
|
||||
@@ -146,6 +164,76 @@ TEST_F(RGBDRelayTest, UncompressRestoresRawImages)
|
||||
EXPECT_EQ(got.depth.height, 8u);
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, CompressesFloatDepthAsInverseDepthWhenAsked)
|
||||
{
|
||||
start(/*compress=*/true, /*uncompress=*/false, ".rvl:10:100");
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat depth(8, 8, CV_32FC1, cv::Scalar(1.5f));
|
||||
cv_bridge::CvImage(in.header, sensor_msgs::image_encodings::TYPE_32FC1, depth).toImageMsg(in.depth);
|
||||
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth_compressed.data.empty());
|
||||
EXPECT_EQ(got.depth_compressed.format.rfind("32FC1; compressedDepth ", 0), 0u) << got.depth_compressed.format;
|
||||
EXPECT_EQ(got.depth_compressed.header.frame_id, in.depth.header.frame_id);
|
||||
const cv::Mat restored = rtabmap_conversions::uncompressDepthImage(got.depth_compressed)->image;
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.001);
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, InvalidDepthCompressionFormatFallsBackToPng)
|
||||
{
|
||||
start(/*compress=*/true, /*uncompress=*/false, ".jpg:10");
|
||||
|
||||
pub_->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
EXPECT_EQ(out_->back().depth_compressed.format, "16UC1; compressedDepth png");
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, LegacyDepthCompressionKeepsFloatDepthLossless)
|
||||
{
|
||||
start(/*compress=*/true, /*uncompress=*/false, "legacy");
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat depth(8, 8, CV_32FC1, cv::Scalar(1.2345f));
|
||||
cv_bridge::CvImage(in.header, sensor_msgs::image_encodings::TYPE_32FC1, depth).toImageMsg(in.depth);
|
||||
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
EXPECT_EQ(got.depth_compressed.format, "png");
|
||||
const cv::Mat restored = rtabmap::uncompressImage(got.depth_compressed.data);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
EXPECT_EQ(cv::norm(restored, depth, cv::NORM_INF), 0.0) << "lossless";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, UncompressRestoresRosCompressedDepth)
|
||||
{
|
||||
// depth_compressed filled by compressed_depth_image_transport ("32FC1; compressedDepth png")
|
||||
start(/*compress=*/false, /*uncompress=*/true);
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat depth(8, 8, CV_32FC1, cv::Scalar(2.5f));
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
in.depth_compressed.header = in.header;
|
||||
ASSERT_TRUE(rtabmap_conversions::rtabmapToCompressedDepthTransport(
|
||||
rtabmap::compressImage2(depth, ".png:10:100"), in.depth_compressed));
|
||||
ASSERT_EQ(in.depth_compressed.format, "32FC1; compressedDepth png");
|
||||
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth.data.empty()) << "depth must be decompressed";
|
||||
EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
const cv::Mat restored = cv_bridge::toCvCopy(got.depth)->image;
|
||||
EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.001);
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, UncompressPrefersTheRawImageOverTheCompressedOne)
|
||||
{
|
||||
// A message may carry both. The raw image is already usable, so decompressing the
|
||||
|
||||
@@ -8,7 +8,11 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
|
||||
#include <rtabmap_util/rgbd_split.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <cstring>
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
@@ -136,6 +140,436 @@ TEST_F(RGBDSplitTest, DecompressesDepthWithTheCorrectEncoding)
|
||||
<< "and the values must survive the round trip";
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitTest, DecompressesRosCompressedDepthAndInverseDepth)
|
||||
{
|
||||
// depth_compressed filled by compressed_depth_image_transport, or by rtabmap with an
|
||||
// inverse depth format (Mem/DepthCompressionFormat=".rvl:10:100").
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
const cv::Mat original(8, 8, CV_32FC1, cv::Scalar(3.0f));
|
||||
const cv::Mat compressed = rtabmap::compressImage2(original, ".rvl:10:100");
|
||||
for(bool ros : {true, false})
|
||||
{
|
||||
SCOPED_TRACE(ros ? "compressedDepth" : "rtabmap");
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
in.depth_compressed.header = in.header;
|
||||
if(ros)
|
||||
{
|
||||
ASSERT_TRUE(rtabmap_conversions::rtabmapToCompressedDepthTransport(compressed, in.depth_compressed));
|
||||
// RVL re-compressed as PNG before Jazzy
|
||||
ASSERT_EQ(in.depth_compressed.format.rfind("32FC1; compressedDepth ", 0), 0u);
|
||||
}
|
||||
else
|
||||
{
|
||||
in.depth_compressed.format = "rvl:10:100";
|
||||
in.depth_compressed.data.assign(compressed.data, compressed.data + compressed.total());
|
||||
}
|
||||
|
||||
const size_t received = depth->size();
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return depth->size() > received; }));
|
||||
|
||||
const sensor_msgs::msg::Image & got = depth->back();
|
||||
EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
EXPECT_EQ(got.width, 8u);
|
||||
EXPECT_EQ(got.height, 8u);
|
||||
ASSERT_EQ(got.step, 32u);
|
||||
EXPECT_NEAR(*reinterpret_cast<const float *>(&got.data[0]), 3.0f, 0.001f);
|
||||
}
|
||||
}
|
||||
|
||||
/// The compressedDepth topic of depth/image, published by rgbd_split itself (see
|
||||
/// "compressed_passthrough") and read here through the real
|
||||
/// compressed_depth_image_transport plugin, as any ROS consumer would.
|
||||
class RGBDSplitCompressedDepthTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::vector<rclcpp::Parameter> & params = {})
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(
|
||||
rclcpp::NodeOptions().parameter_overrides(params)));
|
||||
compressed_ = collect<sensor_msgs::msg::CompressedImage>("rgbd_image/depth/image/compressedDepth");
|
||||
const auto callback = [this](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { decoded_.push_back(msg); };
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
decodedSub_ = image_transport::create_subscription(helper().get(), "rgbd_image/depth/image", callback, "compressedDepth");
|
||||
#else
|
||||
decodedSub_ = image_transport::create_subscription(*helper(), "rgbd_image/depth/image", callback, "compressedDepth", rclcpp::QoS(10), rclcpp::SubscriptionOptions());
|
||||
#endif
|
||||
pub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub_));
|
||||
ASSERT_TRUE(waitForPublisher(compressed_->subscription));
|
||||
}
|
||||
|
||||
/// Publishes @p in and waits for both the raw compressedDepth message and its decoding.
|
||||
void publishAndWait(const rtabmap_msgs::msg::RGBDImage & in)
|
||||
{
|
||||
const size_t received = compressed_->size();
|
||||
const size_t decoded = decoded_.size();
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return compressed_->size() > received && decoded_.size() > decoded; }));
|
||||
}
|
||||
|
||||
static rtabmap_msgs::msg::RGBDImage withCompressedDepth(const cv::Mat & compressed, const std::string & format)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
in.depth_compressed.header = in.header;
|
||||
in.depth_compressed.format = format;
|
||||
in.depth_compressed.data.assign(compressed.data, compressed.data + compressed.total());
|
||||
return in;
|
||||
}
|
||||
|
||||
void expectDecodedDepth(float expected, float tolerance)
|
||||
{
|
||||
ASSERT_FALSE(decoded_.empty());
|
||||
const sensor_msgs::msg::Image & got = *decoded_.back();
|
||||
ASSERT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
ASSERT_EQ(got.width, 8u);
|
||||
ASSERT_EQ(got.height, 8u);
|
||||
EXPECT_NEAR(*reinterpret_cast<const float *>(&got.data[0]), expected, tolerance);
|
||||
EXPECT_EQ(got.header.frame_id, "camera_link");
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CompressedImage>> compressed_;
|
||||
std::vector<sensor_msgs::msg::Image::ConstSharedPtr> decoded_;
|
||||
image_transport::Subscriber decodedSub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub_;
|
||||
};
|
||||
|
||||
TEST_F(RGBDSplitCompressedDepthTest, RepublishesRosCompressedDepthAsIs)
|
||||
{
|
||||
start();
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
in.depth_compressed.header = in.header;
|
||||
ASSERT_TRUE(rtabmap_conversions::rtabmapToCompressedDepthTransport(
|
||||
rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(4.0f)), ".png:20:50"), in.depth_compressed));
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().format, in.depth_compressed.format);
|
||||
EXPECT_EQ(compressed_->back().data, in.depth_compressed.data) << "not decompressed and re-compressed";
|
||||
expectDecodedDepth(4.0f, 0.01f);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitCompressedDepthTest, ConvertsRtabmapInverseDepthWithoutDecompressingIt)
|
||||
{
|
||||
start();
|
||||
const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(3.0f)), ".png:10:100");
|
||||
const rtabmap_msgs::msg::RGBDImage in = withCompressedDepth(compressed, "png:10:100");
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png");
|
||||
// Same payload: only the header changed
|
||||
const std::vector<unsigned char> & data = compressed_->back().data;
|
||||
ASSERT_EQ(data.size(), compressed.total() - 16 + 12);
|
||||
EXPECT_EQ(memcmp(data.data() + 12, compressed.data + 16, compressed.total() - 16), 0);
|
||||
expectDecodedDepth(3.0f, 0.001f);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitCompressedDepthTest, Converts16BitsRvl)
|
||||
{
|
||||
start();
|
||||
const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_16UC1, cv::Scalar(1234)), ".rvl");
|
||||
publishAndWait(withCompressedDepth(compressed, "rvl"));
|
||||
#ifdef PRE_ROS_JAZZY
|
||||
// compressed_depth_image_transport cannot decode RVL before Jazzy: re-compressed as PNG
|
||||
EXPECT_EQ(compressed_->back().format, "16UC1; compressedDepth png");
|
||||
#else
|
||||
// without decompressing it
|
||||
EXPECT_EQ(compressed_->back().format, "16UC1; compressedDepth rvl");
|
||||
EXPECT_EQ(compressed_->back().data.size(), compressed.total() - 8 + 12);
|
||||
#endif
|
||||
const sensor_msgs::msg::Image & got = *decoded_.back();
|
||||
ASSERT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
EXPECT_EQ(*reinterpret_cast<const uint16_t *>(&got.data[0]), 1234);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitCompressedDepthTest, CompressesLegacyFloatDepthWithTheTransportParameters)
|
||||
{
|
||||
// The legacy 32FC1 format (4 channels PNG) has no compressedDepth equivalent.
|
||||
start({rclcpp::Parameter("rgbd_image.depth.image.compressedDepth.format", std::string("rvl")),
|
||||
rclcpp::Parameter("rgbd_image.depth.image.compressedDepth.depth_max", 5.0)});
|
||||
const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(2.0f)), ".png");
|
||||
ASSERT_EQ(rtabmap::compressedDepthFormat(compressed), ".png");
|
||||
|
||||
publishAndWait(withCompressedDepth(compressed, "png"));
|
||||
#ifdef PRE_ROS_JAZZY
|
||||
const std::string codec = "png"; // RVL not supported by compressed_depth_image_transport
|
||||
#else
|
||||
const std::string codec = "rvl";
|
||||
#endif
|
||||
EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth " + codec);
|
||||
EXPECT_EQ(rtabmap::compressedDepthFormat(rtabmap_conversions::compressedDepthTransportToRtabmap(compressed_->back())), "." + codec + ":5:100");
|
||||
expectDecodedDepth(2.0f, 0.001f);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitCompressedDepthTest, ConvertsRtabmapRvlInverseDepth)
|
||||
{
|
||||
start();
|
||||
const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(6.0f)), ".rvl:10:100");
|
||||
publishAndWait(withCompressedDepth(compressed, "rvl:10:100"));
|
||||
const cv::Mat bytes = rtabmap_conversions::compressedDepthTransportToRtabmap(compressed_->back());
|
||||
#ifdef PRE_ROS_JAZZY
|
||||
EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png");
|
||||
EXPECT_EQ(rtabmap::compressedDepthFormat(bytes), ".png:10:100");
|
||||
#else
|
||||
EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth rvl");
|
||||
EXPECT_EQ(rtabmap::compressedDepthFormat(bytes), ".rvl:10:100");
|
||||
#endif
|
||||
// Same quantized values either way
|
||||
const cv::Mat restored = rtabmap::uncompressImage(bytes);
|
||||
EXPECT_EQ(cv::countNonZero(restored != rtabmap::uncompressImage(compressed)), 0);
|
||||
expectDecodedDepth(6.0f, 0.002f);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitCompressedDepthTest, CompressesRawDepth)
|
||||
{
|
||||
start();
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
cv_bridge::CvImage(in.header, sensor_msgs::image_encodings::TYPE_32FC1,
|
||||
cv::Mat(8, 8, CV_32FC1, cv::Scalar(1.5f))).toImageMsg(in.depth);
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png");
|
||||
expectDecodedDepth(1.5f, 0.001f);
|
||||
}
|
||||
|
||||
/// What compressed_depth_image_transport publishes for a 16UC1 depth image in RVL (since
|
||||
/// Jazzy): its config header, then rtabmap's RVL payload without its signature.
|
||||
TEST_F(RGBDSplitCompressedDepthTest, RepublishesRosRvlDepthWhereItCanBeDecoded)
|
||||
{
|
||||
start();
|
||||
const cv::Mat rvl = rtabmap::compressImage2(cv::Mat(8, 8, CV_16UC1, cv::Scalar(2345)), ".rvl");
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
in.depth_compressed.header = in.header;
|
||||
in.depth_compressed.format = "16UC1; compressedDepth rvl";
|
||||
in.depth_compressed.data.assign(12, 0); // format 0 (INV_DEPTH), depthParam unused for 16UC1
|
||||
in.depth_compressed.data.insert(in.depth_compressed.data.end(), rvl.data + 8, rvl.data + rvl.total());
|
||||
|
||||
publishAndWait(in);
|
||||
#ifdef PRE_ROS_JAZZY
|
||||
// compressed_depth_image_transport cannot decode RVL before Jazzy: re-compressed as PNG
|
||||
EXPECT_EQ(compressed_->back().format, "16UC1; compressedDepth png");
|
||||
#else
|
||||
EXPECT_EQ(compressed_->back().format, in.depth_compressed.format);
|
||||
EXPECT_EQ(compressed_->back().data, in.depth_compressed.data) << "not decompressed and re-compressed";
|
||||
#endif
|
||||
const sensor_msgs::msg::Image & got = *decoded_.back();
|
||||
ASSERT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
EXPECT_EQ(*reinterpret_cast<const uint16_t *>(&got.data[0]), 2345);
|
||||
}
|
||||
|
||||
/// A raw depth image compressed here keeps its own header, not the RGBDImage's.
|
||||
TEST_F(RGBDSplitCompressedDepthTest, CompressesRawDepthWithItsOwnHeader)
|
||||
{
|
||||
start();
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
std_msgs::msg::Header depthHeader = in.header;
|
||||
depthHeader.frame_id = "depth_optical";
|
||||
cv_bridge::CvImage(depthHeader, sensor_msgs::image_encodings::TYPE_32FC1,
|
||||
cv::Mat(8, 8, CV_32FC1, cv::Scalar(1.5f))).toImageMsg(in.depth);
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().header.frame_id, "depth_optical");
|
||||
EXPECT_EQ(decoded_.back()->header.frame_id, "depth_optical");
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitCompressedDepthTest, PluginIsUsedWhenPassthroughIsDisabled)
|
||||
{
|
||||
start({rclcpp::Parameter("compressed_passthrough", false)});
|
||||
const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(3.0f)), ".png:10:100");
|
||||
|
||||
publishAndWait(withCompressedDepth(compressed, "png:10:100"));
|
||||
expectDecodedDepth(3.0f, 0.01f);
|
||||
// Re-compressed by the plugin ("32FC1; compressedDepth" before Jazzy, without the codec)
|
||||
EXPECT_EQ(compressed_->back().format.rfind("32FC1; compressedDepth", 0), 0u) << compressed_->back().format;
|
||||
EXPECT_EQ(rtabmap::compressedDepthFormat(rtabmap_conversions::compressedDepthTransportToRtabmap(compressed_->back())), ".png:10:100");
|
||||
}
|
||||
|
||||
/// compressed_depth_passthrough follows compressed_passthrough unless set: the depth can
|
||||
/// still be passed through with the color compressed by the plugin (e.g., for jpeg_quality).
|
||||
TEST_F(RGBDSplitCompressedDepthTest, DepthPassthroughCanBeKeptWithoutColorPassthrough)
|
||||
{
|
||||
start({rclcpp::Parameter("compressed_passthrough", false),
|
||||
rclcpp::Parameter("compressed_depth_passthrough", true)});
|
||||
const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(3.0f)), ".png:10:100");
|
||||
|
||||
publishAndWait(withCompressedDepth(compressed, "png:10:100"));
|
||||
EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png");
|
||||
// Same payload: only the header changed
|
||||
const std::vector<unsigned char> & data = compressed_->back().data;
|
||||
ASSERT_EQ(data.size(), compressed.total() - 16 + 12);
|
||||
EXPECT_EQ(memcmp(data.data() + 12, compressed.data + 16, compressed.total() - 16), 0);
|
||||
expectDecodedDepth(3.0f, 0.001f);
|
||||
}
|
||||
|
||||
/// The compressed topic of the color (or left/right) image, published by rgbd_split
|
||||
/// itself too, read through the real compressed_image_transport plugin.
|
||||
class RGBDSplitCompressedImageTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::string & topic, const std::vector<rclcpp::Parameter> & params = {})
|
||||
{
|
||||
split_ = addNode(std::make_shared<rtabmap_util::RGBDSplit>(
|
||||
rclcpp::NodeOptions().parameter_overrides(params)));
|
||||
compressed_ = collect<sensor_msgs::msg::CompressedImage>(topic + "/compressed");
|
||||
const auto callback = [this](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { decoded_.push_back(msg); };
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
decodedSub_ = image_transport::create_subscription(helper().get(), topic, callback, "compressed");
|
||||
#else
|
||||
decodedSub_ = image_transport::create_subscription(*helper(), topic, callback, "compressed", rclcpp::QoS(10), rclcpp::SubscriptionOptions());
|
||||
#endif
|
||||
pub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub_));
|
||||
ASSERT_TRUE(waitForPublisher(compressed_->subscription));
|
||||
}
|
||||
|
||||
void publishAndWait(const rtabmap_msgs::msg::RGBDImage & in)
|
||||
{
|
||||
const size_t received = compressed_->size();
|
||||
const size_t decoded = decoded_.size();
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return compressed_->size() > received && decoded_.size() > decoded; }));
|
||||
}
|
||||
|
||||
std::shared_ptr<rtabmap_util::RGBDSplit> split_;
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CompressedImage>> compressed_;
|
||||
std::vector<sensor_msgs::msg::Image::ConstSharedPtr> decoded_;
|
||||
image_transport::Subscriber decodedSub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub_;
|
||||
};
|
||||
|
||||
TEST_F(RGBDSplitCompressedImageTest, RepublishesCompressedColorAsIs)
|
||||
{
|
||||
start("rgbd_image/rgb/image");
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image;
|
||||
in.rgb = sensor_msgs::msg::Image();
|
||||
ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg(
|
||||
cv_bridge::CvImage(in.header, "bgr8", color), ".png", in.rgb_compressed));
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed";
|
||||
EXPECT_EQ(compressed_->back().format, "bgr8; png compressed bgr8");
|
||||
EXPECT_EQ(cv::norm(cv_bridge::toCvCopy(decoded_.back())->image, color, cv::NORM_INF), 0.0);
|
||||
}
|
||||
|
||||
/// Older producers set only "jpg" as format: the encoding is added from the image header.
|
||||
TEST_F(RGBDSplitCompressedImageTest, AddsTheEncodingToAnOldFormat)
|
||||
{
|
||||
start("rgbd_image/rgb/image");
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
cv_bridge::toCvCopy(in.rgb)->toCompressedImageMsg(in.rgb_compressed, cv_bridge::JPG);
|
||||
in.rgb = sensor_msgs::msg::Image();
|
||||
ASSERT_EQ(in.rgb_compressed.format, "jpg");
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data);
|
||||
EXPECT_EQ(compressed_->back().format, "bgr8; jpeg compressed bgr8");
|
||||
EXPECT_EQ(decoded_.back()->encoding, "bgr8");
|
||||
}
|
||||
|
||||
/// A raw color image is compressed with "<topic>.compressed.format".
|
||||
TEST_F(RGBDSplitCompressedImageTest, CompressesRawColorWithTheTransportParameter)
|
||||
{
|
||||
start("rgbd_image/rgb/image", {rclcpp::Parameter("rgbd_image.rgb.image.compressed.format", std::string("png"))});
|
||||
const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().format, "bgr8; png compressed bgr8");
|
||||
EXPECT_EQ(cv::norm(cv_bridge::toCvCopy(decoded_.back())->image, cv_bridge::toCvCopy(in.rgb)->image, cv::NORM_INF), 0.0);
|
||||
}
|
||||
|
||||
/// With compressed_passthrough, the compressed plugin is removed even if the user enabled
|
||||
/// it: only the node publishes on the topic, the image as is.
|
||||
TEST_F(RGBDSplitCompressedImageTest, RemovesTheCompressedPluginEnabledByTheUser)
|
||||
{
|
||||
const std::string name = "rgbd_image.rgb.image.enable_pub_plugins";
|
||||
start("rgbd_image/rgb/image", {rclcpp::Parameter(name,
|
||||
std::vector<std::string>{"image_transport/raw", "image_transport/compressed"})});
|
||||
const std::vector<std::string> plugins = split_->get_parameter(name).as_string_array();
|
||||
EXPECT_EQ(plugins, std::vector<std::string>{"image_transport/raw"});
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image;
|
||||
in.rgb = sensor_msgs::msg::Image();
|
||||
ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg(
|
||||
cv_bridge::CvImage(in.header, "bgr8", color), ".png", in.rgb_compressed));
|
||||
|
||||
publishAndWait(in);
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_EQ(compressed_->size(), 1u) << "published by the node only";
|
||||
EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed";
|
||||
}
|
||||
|
||||
/// compressed_depth_passthrough does not change the color image's passthrough.
|
||||
TEST_F(RGBDSplitCompressedImageTest, ColorPassthroughCanBeKeptWithoutDepthPassthrough)
|
||||
{
|
||||
start("rgbd_image/rgb/image", {rclcpp::Parameter("compressed_depth_passthrough", false)});
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image;
|
||||
in.rgb = sensor_msgs::msg::Image();
|
||||
ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg(
|
||||
cv_bridge::CvImage(in.header, "bgr8", color), ".png", in.rgb_compressed));
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed";
|
||||
}
|
||||
|
||||
/// A raw color image compressed here keeps its own header, not the RGBDImage's.
|
||||
TEST_F(RGBDSplitCompressedImageTest, CompressesRawColorWithItsOwnHeader)
|
||||
{
|
||||
start("rgbd_image/rgb/image");
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
in.rgb.header.frame_id = "rgb_optical";
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().header.frame_id, "rgb_optical");
|
||||
EXPECT_EQ(decoded_.back()->header.frame_id, "rgb_optical");
|
||||
}
|
||||
|
||||
/// A compressed color image without a frame takes the RGBDImage's header.
|
||||
TEST_F(RGBDSplitCompressedImageTest, RepublishesCompressedColorWithTheInputHeaderIfItHasNone)
|
||||
{
|
||||
start("rgbd_image/rgb/image");
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image;
|
||||
in.rgb = sensor_msgs::msg::Image();
|
||||
ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg(
|
||||
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", color), ".png", in.rgb_compressed));
|
||||
ASSERT_TRUE(in.rgb_compressed.header.frame_id.empty());
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed";
|
||||
EXPECT_EQ(compressed_->back().header.frame_id, "camera_link");
|
||||
EXPECT_EQ(compressed_->back().header.stamp, in.header.stamp);
|
||||
}
|
||||
|
||||
/// With "stereo", the right image is republished as is on right/image/compressed.
|
||||
TEST_F(RGBDSplitCompressedImageTest, RepublishesACompressedRightImageAsIs)
|
||||
{
|
||||
start("rgbd_image/right/image", {rclcpp::Parameter("stereo", true)});
|
||||
rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat right = cv_bridge::toCvCopy(in.depth)->image;
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg(
|
||||
cv_bridge::CvImage(in.header, "mono8", right), ".jpg", in.depth_compressed));
|
||||
|
||||
publishAndWait(in);
|
||||
EXPECT_EQ(compressed_->back().data, in.depth_compressed.data);
|
||||
EXPECT_EQ(compressed_->back().format, "mono8; jpeg compressed mono8");
|
||||
EXPECT_EQ(decoded_.back()->encoding, "mono8");
|
||||
}
|
||||
|
||||
/// Feeds a compressed right image in @p format and returns what lands on depth/image.
|
||||
class RGBDSplitRightImageTest : public NodeTest
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user