rtabmap_demos tests and docs (#1462)

* rtabmap_demos tests and docs

* added bag testing

* added rtabmap_examples launch tests

* updating demo bag download paths

* added netherdrone demo

* Fixed rgb-only callback with lidar rejected.  Updated lidar params

* Added back OrbitOriented rviz view to ros2, with optional octomap wll clipping

* fixing ci

* lidar demo added intermediate_nodes option

* added netherdrone as demo test

* running rtabmap_demos tests on ci

* added rtabmap_launch tests, fixed ground_truth_base_frame_id usage

* ficing rolling

* updating demo test harnest

* densify golden trajectories to avoid tf missing

* fixing tf steps

* updated min icp ratio for netherdrone demo

* fixing image_transport arg->params

* export pose opt=0

* lets process all frames

* updated netherdrone golden

* fixing publishers queue size just for tests

* added playdback demo doc

* Adding more logs to debug ci

* Fixing QOS for CI to reliable, added find-object demo test

* name threads

* fixing camera info expected transient on lyrical/rolling. Fixing find_object not appearing idle

* 30 Hz polling backward comp

* fixing test tf sim lock

* fixing lyrical qos bag parsing

* faster replay

* lockstep

* fixing clock deadlock

* Added test on shutdown

* updated netherdrone golden poses

* Extended stereo outdoor test

* updated shutdown test

* updated test

* multi-thread flaky test

* adding backtrace when test fails

* increased closure slack for netherdrone

* g2o gauss newton on stereo

* adjusted maximum optimizer iterations

* updated default iterations

* Added netherdrone in list of demos

rtabmap_conversions/src/MsgConversion.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_conversions/MsgConversion.h"
 
+#include <algorithm>
 #include <cmath>
 #include <limits>
 
@@ -2148,6 +2149,36 @@ bool convertRGBDMsgs(
 			(cameraInfoMsgs.size() == depthMsgs.size() || depthMsgs.empty()) &&
 			(cameraInfoMsgs.size() == depthCameraInfoMsgs.size() || depthCameraInfoMsgs.empty()));
 
+	// An RGBDImage of a camera without depth (e.g., from rgb_sync) has an empty depth
+	// slot, which reaches here as empty depth images: convert the frame as color only,
+	// rather than as a stereo pair missing its right image.
+	if(!depthMsgs.empty() &&
+	   std::all_of(depthMsgs.begin(), depthMsgs.end(),
+			   [](const cv_bridge::CvImageConstPtr & msg) { return msg.get() == 0 || msg->image.empty(); }))
+	{
+		return convertRGBDMsgs(
+				imageMsgs,
+				std::vector<cv_bridge::CvImageConstPtr>(),
+				cameraInfoMsgs,
+				std::vector<sensor_msgs::msg::CameraInfo>(),
+				frameId,
+				odomFrameId,
+				odomStamp,
+				rgb,
+				depth,
+				cameraModels,
+				stereoCameraModels,
+				tfBuffer,
+				waitForTransform,
+				alreadRectifiedImages,
+				localKeyPointsMsgs,
+				localPoints3dMsgs,
+				localDescriptorsMsgs,
+				localKeyPoints,
+				localPoints3d,
+				localDescriptors);
+	}
+
 	int imageWidth = imageMsgs.size()?imageMsgs[0]->image.cols:cameraInfoMsgs[0].width;
 	int imageHeight = imageMsgs.size()?imageMsgs[0]->image.rows:cameraInfoMsgs[0].height;
 	int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0;

rtabmap_conversions/test/test_msg_conversion.cpp

@@ -3048,6 +3048,43 @@ TEST(MsgConversion, convertRGBDMsgsSingleCamera)
 	EXPECT_EQ(depth.at<unsigned short>(0, 0), 1500);
 }
 
+// A camera without depth, through an RGBDImage as rgb_sync publishes it: the depth slot
+// is left empty, and toCvShare() turns it into an empty depth image. The frame must
+// come out as color only, not be rejected as a stereo pair without a right image.
+TEST(MsgConversion, convertRGBDMsgsColorOnlyRGBDImage)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
+
+	const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	rtabmap_msgs::msg::RGBDImage rgbdImage;
+	rgbdImage.header = makeImage("camera_link", 1000.0, rgbImage, "bgr8")->header;
+	makeImage("camera_link", 1000.0, rgbImage, "bgr8")->toImageMsg(rgbdImage.rgb);
+	rgbdImage.rgb_camera_info = makeCameraInfo("camera_link", 1000.0, 8, 8);
+
+	cv_bridge::CvImageConstPtr image, depthImage;
+	toCvShare(rgbdImage, std::shared_ptr<void const>(), image, depthImage);
+	ASSERT_TRUE(depthImage.get() != 0);
+	ASSERT_TRUE(depthImage->image.empty()) << "an RGBDImage without depth gives an empty depth image";
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs({image}, {depthImage}, {rgbdImage.rgb_camera_info},
+			{rgbdImage.depth_camera_info}, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels,
+			*buffer, 0.0, /*alreadyRectifiedImages=*/true));
+
+	EXPECT_TRUE(stereoModels.empty());
+	ASSERT_EQ(models.size(), 1u);
+	EXPECT_NEAR(models[0].fx(), 100.0, 1e-9);
+	expectTransformNear(models[0].localTransform(), baseToCamera, 1e-4f);
+	ASSERT_EQ(rgb.cols, 8);
+	ASSERT_EQ(rgb.rows, 8);
+	EXPECT_TRUE(depth.empty());
+}
+
 TEST(MsgConversion, convertRGBDMsgsMultiCameraSideBySide)
 {
 	// Two cameras are concatenated horizontally into one wide image, one model each.