rtabmap_util tests and doc (#1450)

* Initial tests

* more tests

* More in-depth deskew() testing

* slightly less verbose clamping corruption warning

* added tf buffer related tests

* added remaining tests

* Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp

* cleanup doc

* fixing ci

* rtabmap_util tests and doc

* Added db_player tests

* Added MapsManager tests

* Added map_assembler tests

* Documenting node first draft

* relative links

* Fixed british->usa english style. Reviewed all md files.

* added link to install ros1

* updated badges

* added Iron

* added ubuntu

* added codecov

* updated coverage ci

* fixing rosdep

* updated ci cov job

* ci bump

* fixing cov ci

* small doc cleanup

rtabmap_util/src/DbPlayerNode.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rclcpp/rclcpp.hpp"
 
+#include <stdexcept>
+
 #ifndef _WIN32
 #include <sys/ioctl.h>
 #include <termios.h>
@@ -83,7 +85,19 @@ int main(int argc, char **argv)
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);
 
-	auto node = std::make_shared<rtabmap_util::DbPlayer>(options);
+	std::shared_ptr<rtabmap_util::DbPlayer> node;
+	try
+	{
+		node = std::make_shared<rtabmap_util::DbPlayer>(options);
+	}
+	catch(const std::exception & e)
+	{
+		// The node reports what went wrong before throwing; keep the process exit clean
+		// rather than letting an uncaught exception abort.
+		UERROR("%s", e.what());
+		rclcpp::shutdown();
+		return -1;
+	}
 
 	rclcpp::Rate pauseRate(10);
 

rtabmap_util/src/MapsManager.cpp

@@ -273,6 +273,12 @@ void MapsManager::set2DMap(
 		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Memory * memory)
 {
+	if(!map.empty() && poses.empty())
+	{
+		UWARN("Ignoring the 2D map (%dx%d): no poses were given. Pass the poses of the "
+			  "nodes the map was assembled from.", map.cols, map.rows);
+		return;
+	}
 	occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
 	//update cache in case the map should be updated
 	if(memory && 
@@ -1420,6 +1426,7 @@ void MapsManager::publishMaps(
 		msg->header.frame_id = mapFrameId;
 		msg->header.stamp = stamp;
 		elevationMapPub_->publish(std::move(msg));
+		latched_.at(&elevationMapPub_) = true;
 	}
 	if(elevationMapPub_->get_subscription_count() == 0)
 	{

rtabmap_util/src/nodelets/db_player.cpp

@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_util/db_player.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 
+#include <stdexcept>
+
 #include <image_transport/image_transport.hpp>
 
 #include <rtabmap_conversions/MsgConversion.h>
@@ -106,13 +108,14 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
     qosGlobalPose_ =  this->declare_parameter("qos_global_pose", qos_);
     qosGps_ =         this->declare_parameter("qos_gps", qos_);
     qosImu_ =         this->declare_parameter("qos_imu", qos_);
+    qosEnvSensor_ =   this->declare_parameter("qos_env_sensor", qos_);
 
     // A general 360 lidar with 0.5 deg increment
     scanAngleMin_ =         this->declare_parameter("scan_angle_min", -M_PI);
     scanAngleMax_ =         this->declare_parameter("scan_angle_max", M_PI);
     scanAngleIncrement_ =   this->declare_parameter("scan_angle_increment", M_PI / 720.0);
     scanRangeMin_ =         this->declare_parameter("scan_range_min", 0.0);
-    scanRangeMax_ =         this->declare_parameter("scan_range_max", 60);
+    scanRangeMax_ =         this->declare_parameter("scan_range_max", 60.0);
 
     RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str());
     RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str());
@@ -136,8 +139,11 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
 
     if(databasePath.empty())
     {
+        // Throwing rather than exiting: this node can be loaded in a component container
+        // next to others, and taking the whole process down with it would be rude.
         RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
-        exit(-1);
+        throw std::invalid_argument(
+            "db_player: parameter \"database\" must be set (path to a RTAB-Map database).");
     }
 
     databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
@@ -151,7 +157,8 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
     if(!reader_->init())
     {
         RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str());
-        exit(-1);
+        throw std::runtime_error(
+            uFormat("db_player: cannot open database \"%s\".", databasePath.c_str()));
     }
 
     const std::string servicePrefix = get_name() + std::string("/");
@@ -300,8 +307,12 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
 
     if(!odom.data().laserScanRaw().isEmpty())
     {
-        if(!scanPub_.get() && odom.data().laserScanRaw().is2d())
+        // The publisher has to match the scan being replayed, not just whichever one has
+        // not been created yet: a 2D database must never advertise "scan_cloud".
+        if(odom.data().laserScanRaw().is2d())
         {
+          if(!scanPub_.get())
+          {
             scanPub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScan_));
             if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
             {
@@ -316,6 +327,7 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
                 RCLCPP_INFO(get_logger(), "  scan_range_min=%f", scanRangeMin_);
                 RCLCPP_INFO(get_logger(), "  scan_range_max=%f", scanRangeMax_);
             }
+          }
         }
         else if(!scanCloudPub_.get())
         {
@@ -569,7 +581,6 @@ bool DbPlayer::publishNextFrame()
         envSensorPub_->get_subscription_count() > 0 &&
         !odom.data().envSensors().empty())
     {
-        rtabmap_msgs::msg::EnvSensor msg;
         for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter)
         {
             rtabmap_msgs::msg::EnvSensor msg;

rtabmap_util/src/nodelets/disparity_to_depth.cpp

@@ -28,6 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_util/disparity_to_depth.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+
 #include <image_transport/image_transport.hpp>
 
 #ifdef PRE_ROS_IRON
@@ -44,16 +47,27 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
 {
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
+	// Each side can be set independently so the node can bridge a producer and a
+	// consumer that don't agree on reliability. Both default to qos.
+	int qosSub = this->declare_parameter("qos_sub", qos);
+	int qosPub = this->declare_parameter("qos_pub", qos);
+	int queueSub = this->declare_parameter("queue_sub", 1);
+	int queuePub = this->declare_parameter("queue_pub", 1);
+
+	UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
+			uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
+
+	const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub);
 
 #ifdef PRE_ROS_LYRICAL
-	pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());	
+	pub32f_ = image_transport::create_publisher(this, "depth", pubQos.get_rmw_qos_profile());
+	pub16u_ = image_transport::create_publisher(this, "depth_raw", pubQos.get_rmw_qos_profile());
 #else
-	pub32f_ = image_transport::create_publisher(*this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
-	pub16u_ = image_transport::create_publisher(*this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	pub32f_ = image_transport::create_publisher(*this, "depth", pubQos);
+	pub16u_ = image_transport::create_publisher(*this, "depth_raw", pubQos);
 #endif
 
-	sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
+	sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
 }
 
 DisparityToDepth::~DisparityToDepth(){}

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -61,7 +61,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
 			msg->header.frame_id,
 			fixedFrameId_,
 			msg->header.stamp,
-			rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment),
+			rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds((msg->ranges.empty()?0:msg->ranges.size()-1)*msg->time_increment),
 			*tfBuffer_,
 			waitForTransformDuration_);
 	if(tmpT.isNull())

rtabmap_util/src/nodelets/map_assembler.cpp

@@ -54,12 +54,18 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
 		Node("map_assembler", options),
 		lastNodeAdded_(-1),
 		rtabmapNodeName_("rtabmap"),
-		localGridsRegenerated_(false)
+		localGridsRegenerated_(false),
+		initializeFromRtabmapTimeout_(5.0)
 {
 	std::string configPath;
 	configPath = this->declare_parameter("config_path", configPath);
 	localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_);
 	rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_);
+	// Seconds to wait for rtabmap's get_map_data service on start-up, which is how
+	// map_assembler catches up on a map that already exists. Set it to 0 to skip the call
+	// entirely: the subscription to "mapData" is then created right away instead of after
+	// the wait, which is what you want when map_assembler starts before rtabmap.
+	initializeFromRtabmapTimeout_ = this->declare_parameter("initialize_from_rtabmap_timeout", initializeFromRtabmapTimeout_);
 
 	//parameters
 	rtabmap::ParametersMap parameters;
@@ -175,6 +181,7 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
 	}
 
 	RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids          = %s", this->get_name(), localGridsRegenerated_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "%s: initialize_from_rtabmap_timeout = %fs (0=don't ask rtabmap for the map)", this->get_name(), initializeFromRtabmapTimeout_);
 	mapsManager_.init(*this, this->get_name(), true);
 	mapsManager_.backwardCompatibilityParameters(*this, parameters);
 	mapsManager_.setParameters(parameters);
@@ -189,13 +196,29 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
 #endif
 #endif
 
-	std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
+	if(initializeFromRtabmapTimeout_ > 0.0)
+	{
+		std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
 
-	// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
-	serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-	timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-	client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
-	timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
+		// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
+		serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+		timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+		client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
+		timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
+	}
+	else
+	{
+		subscribeToMapData();
+	}
+}
+
+void MapAssembler::subscribeToMapData()
+{
+	rclcpp::SubscriptionOptions options;
+	// Null unless we came through the timer, in which case the node's default group is used.
+	options.callback_group = timerCbGroup_;
+	mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1),
+		std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
 }
 
 MapAssembler::~MapAssembler() {}
@@ -213,7 +236,8 @@ void MapAssembler::timerCallback()
 	std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
 	RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str());
 	
-	if(client_->wait_for_service(5s))
+	if(client_->wait_for_service(
+			std::chrono::duration<double>(initializeFromRtabmapTimeout_)))
 	{
 		auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
 		request->global_map = false;
@@ -237,18 +261,16 @@ void MapAssembler::timerCallback()
 	}
 	else
 	{
-		RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, "
+		RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for %f seconds, "
 				"may not be a problem if rtabmap is started afterwards. If rtabmap "
 				"is started after in localization mode, call %s/publish_maps "
 				"service with graph_only=false to make sure map_assembler has all the data.",
 					getMapSrv.c_str(),
+					initializeFromRtabmapTimeout_,
 					rtabmapNodeName_.c_str());
 	}
 
-	rclcpp::SubscriptionOptions options;
-	options.callback_group =  timerCbGroup_;
-	mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1), 
-		std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
+	subscribeToMapData();
 }
 
 void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg)

rtabmap_util/src/nodelets/point_cloud_aggregator.cpp

@@ -41,8 +41,6 @@ namespace rtabmap_util
 
 PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) :
 	Node("point_cloud_aggregator", options),
-	warningThread_(0),
-	callbackCalled_(false),
 	exactSync4_(0),
 	approxSync4_(0),
 	exactSync3_(0),
@@ -162,23 +160,16 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 	}
 
 
-	warningThread_ = new std::thread([&](){
-		rclcpp::Rate r(1.0/5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						this->get_name(),
-						approx?"":"Parameter \"approx_sync\" is false, which means that input "
-							"topics should have all the exact timestamp for the callback to be called.",
-						subscribedTopicsMsg.c_str());
-			}
-		}
-	});
+	syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
+	syncDiagnostic_->init(cloudSub_1_.getSubscriber()->get_topic_name(),
+		uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+				"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
+				"header are set. %s%s",
+				this->get_name(),
+				approx?"":"Parameter \"approx_sync\" is false, which means that input "
+					"topics should have all the exact timestamp for the callback to be called.",
+				subscribedTopicsMsg.c_str()));
+
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 }
 
@@ -190,13 +181,6 @@ PointCloudAggregator::~PointCloudAggregator()
 	delete approxSync3_;
 	delete exactSync2_;
 	delete approxSync2_;
-
-	if(warningThread_)
-	{
-		callbackCalled_=true;
-		warningThread_->join();
-		delete warningThread_;
-	}
 }
 
 void PointCloudAggregator::clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
@@ -234,8 +218,8 @@ void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2:
 }
 void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs)
 {
-	callbackCalled_ = true;
 	UASSERT(cloudMsgs.size() > 1);
+	syncDiagnostic_->tickInput(cloudMsgs[0]->header.stamp);
 	if(cloudPub_->get_subscription_count())
 	{
 		pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
@@ -420,6 +404,7 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
 		rosCloud->header.frame_id = frameId;
 		cloudPub_->publish(std::move(rosCloud));
 	}
+	syncDiagnostic_->tickOutput(cloudMsgs[0]->header.stamp);
 }
 }
 

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -45,8 +45,6 @@ namespace rtabmap_util
 
 PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 	Node("point_cloud_assembler", options),
-	warningThread_(0),
-	callbackCalled_(false),
 	exactSync_(0),
 	exactInfoSync_(0),
 	maxClouds_(0),
@@ -170,22 +168,14 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 							syncOdomSub_.getSubscriber()->get_topic_name());
 	}
 
-	warningThread_ = new std::thread([&](){
-		rclcpp::Rate r(1.0/5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				RCLCPP_WARN(this->get_logger(),
-						"%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
-						"header are set. %s",
-						get_name(),
-						subscribedTopicsMsg_.c_str());
-			}
-		}
-	});
+	syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
+	syncDiagnostic_->init(
+		cloudSub_?cloudSub_->get_topic_name():syncCloudSub_.getSubscriber()->get_topic_name(),
+		uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+				"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
+				"header are set. %s",
+				get_name(),
+				subscribedTopicsMsg_.c_str()));
 
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
 }
@@ -194,20 +184,12 @@ PointCloudAssembler::~PointCloudAssembler()
 {
 	delete exactSync_;
 	delete exactInfoSync_;
-
-	if(warningThread_)
-	{
-		callbackCalled_=true;
-		warningThread_->join();
-		delete warningThread_;
-	}
 }
 
 void PointCloudAssembler::callbackCloudOdom(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
 {
-	callbackCalled_ = true;
 	rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
 	if(!odom.isNull())
 	{
@@ -270,7 +252,6 @@ void PointCloudAssembler::callbackCloudOdomInfo(
 			const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	callbackCalled_ = true;
 	rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
 	if(!odom.isNull())
 	{
@@ -293,7 +274,7 @@ void PointCloudAssembler::callbackCloudOdomInfo(
 
 void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
 {
-	callbackCalled_ = true;
+	syncDiagnostic_->tickInput(cloudMsg->header.stamp);
 	if(cloudPub_->get_subscription_count())
 	{
 		UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
@@ -487,6 +468,7 @@ void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::Con
 					rosCloud.header.frame_id = frameId_;
 				}
 				cloudPub_->publish(rosCloud);
+				syncDiagnostic_->tickOutput(cloudMsg->header.stamp);
 				if(circularBuffer_)
 				{
 					if(!isMoving)

rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp

@@ -164,9 +164,10 @@ void PointCloudToDepthImage::callback(
 
 		if(cloudDisplacement.isNull())
 		{
-			RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!",
-				pointCloud2Msg->header.frame_id.c_str(), 
-				cameraInfoMsg->header.frame_id.c_str(),
+			RCLCPP_ERROR(this->get_logger(), "Could not find how %s moved between the cloud (%f) and the camera info (%f) stamps, accordingly to %s, aborting!",
+				pointCloud2Msg->header.frame_id.c_str(),
+				cloudStamp,
+				infoStamp,
 				fixedFrameId_.c_str());
 			return;
 		}

rtabmap_util/src/nodelets/rgbd_relay.cpp

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
 
 namespace rtabmap_util
 {
@@ -54,11 +55,20 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
 {
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
+	// The two sides can be set independently so the relay can bridge a publisher
+	// and a subscriber that don't agree on reliability. Both default to qos.
+	int qosSub = this->declare_parameter("qos_sub", qos);
+	int qosPub = this->declare_parameter("qos_pub", qos);
+	int queueSub = this->declare_parameter("queue_sub", 5);
+	int queuePub = this->declare_parameter("queue_pub", 1);
 	compress_ = this->declare_parameter("compress", compress_);
 	uncompress_ = this->declare_parameter("uncompress", uncompress_);
 
-	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
-	rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
+			uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
+
+	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
+	rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub));
 }
 
 void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
@@ -125,7 +135,7 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 				// already raw, just copy pointer
 				output->rgb = input->rgb;
 			}
-			if(!input->rgb_compressed.data.empty())
+			else if(!input->rgb_compressed.data.empty())
 			{
 				cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb);
 			}
@@ -135,20 +145,37 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 				// already raw, just copy pointer
 				output->depth = input->depth;
 			}
-			else if(input->depth_compressed.format.compare("jpg")==0)
+			else if(!input->depth_compressed.data.empty())
 			{
-				// right stereo image
-				cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth);
-			}
-			else
-			{
-				// dpeth image
+				// 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);
-				UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1);
-				cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
-				cvImg->toImageMsg(output->depth);
+				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);
+					}
+				}
 			}
 		}
 

rtabmap_util/src/nodelets/rgbd_split.cpp

@@ -26,6 +26,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap_util/rgbd_split.hpp>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <sensor_msgs/image_encodings.hpp>
 
 #ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
@@ -37,24 +41,55 @@ namespace rtabmap_util
 {
 
 RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
-	Node("rgbd_split", options)
+	Node("rgbd_split", options),
+	stereo_(false)
 {
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
+	// Each side can be set independently so the node can bridge a producer and a
+	// consumer that don't agree on reliability. Both default to qos.
+	int qosSub = this->declare_parameter("qos_sub", qos);
+	int qosPub = this->declare_parameter("qos_pub", qos);
+	int queueSub = this->declare_parameter("queue_sub", 5);
+	int queuePub = this->declare_parameter("queue_pub", 1);
+	// 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);
 
 	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");
 
-	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
+	UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
+			uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
+
+	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
+
+	const std::string base = rgbdImageSub_->get_topic_name();
+	const std::string firstName = stereo_?"/left":"/rgb";
+	const std::string secondName = stereo_?"/right":"/depth";
+	const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub);
 
 #ifdef PRE_ROS_LYRICAL
-	rgbPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	depthPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());	
+	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());
 #else
-	rgbPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
-	depthPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	rgbPub_ = image_transport::create_publisher(*this, base + firstName + "/image", pubQos);
+	depthPub_ = image_transport::create_publisher(*this, base + secondName + "/image", pubQos);
 #endif
-	rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/rgb/camera_info", 1);
-	depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/depth/camera_info", 1);
+	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);
+
+	// Resolved names: the outputs are derived from the input topic, so a remapping of
+	// "rgbd_image" moves all four with it. Print them so it is clear what to subscribe to.
+	RCLCPP_INFO(this->get_logger(), "%s: subscribed to:\n   %s", get_name(), rgbdImageSub_->get_topic_name());
+	RCLCPP_INFO(this->get_logger(), "%s: publishing:\n   %s,\n   %s,\n   %s,\n   %s",
+			get_name(),
+			rgbPub_.getTopic().c_str(),
+			rgbInfoPub_->get_topic_name(),
+			depthPub_.getTopic().c_str(),
+			depthInfoPub_->get_topic_name());
 }
 
 
@@ -100,7 +135,36 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 #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->depth_compressed)->toImageMsg(outputImage);
+			// 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())
+			{
+				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(outputImage);
+			}
 #endif
 		}
 		if(outputCameraInfo.header.frame_id.empty()) {
@@ -119,6 +183,37 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 				outputImage.header = outputCameraInfo.header;
 			}
 		}
+		// The "depth" slot of an RGBDImage holds either a depth image or the right image
+		// of a stereo pair, and "stereo" decides which name it goes out under. Warn when
+		// the two disagree: the topic name would be lying to every consumer downstream.
+		// Both directions only warn and keep forwarding -- publishing a right image on
+		// the depth topic is what this node has always done, and setups rely on it.
+		if(!outputImage.data.empty())
+		{
+			const bool isDepth =
+					outputImage.encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
+					outputImage.encoding == sensor_msgs::image_encodings::TYPE_32FC1 ||
+					outputImage.encoding == sensor_msgs::image_encodings::MONO16;
+			if(stereo_ && isDepth)
+			{
+				RCLCPP_WARN_ONCE(this->get_logger(),
+						"Parameter \"stereo\" is true, so the second half is published as \"%s\", "
+						"but the received image is a depth image (encoding=\"%s\"), not the right "
+						"image of a stereo pair. Set \"stereo\" to false to publish it as depth. "
+						"(This warning is printed only once)",
+						depthPub_.getTopic().c_str(), outputImage.encoding.c_str());
+			}
+			else if(!stereo_ && !isDepth)
+			{
+				RCLCPP_WARN_ONCE(this->get_logger(),
+						"Parameter \"stereo\" is false, so the second half is published as \"%s\", "
+						"but the received image is not a depth image (encoding=\"%s\"): it looks "
+						"like the right image of a stereo pair. Set \"stereo\" to true to publish "
+						"it under a name that says so. (This warning is printed only once)",
+						depthPub_.getTopic().c_str(), outputImage.encoding.c_str());
+			}
+		}
+
 		depthPub_.publish(outputImage);
 		depthInfoPub_->publish(outputCameraInfo);
 	}