Ported data_player node to ROS2. Fixed camera publisher namespace in rgbd_split.

rtabmap_util/CMakeLists.txt

@@ -76,6 +76,7 @@ SET(rtabmap_util_plugins_lib_src
    src/nodelets/point_cloud_aggregator.cpp
    src/nodelets/point_cloud_assembler.cpp
    src/nodelets/imu_to_tf.cpp
+   src/nodelets/db_player.cpp
    src/nodelets/lidar_deskewing.cpp
    src/nodelets/rgbd_relay.cpp
    src/nodelets/rgbd_split.cpp
@@ -134,6 +135,7 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDSplit")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DisparityToDepth")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ImuToTF")
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DbPlayer")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::LidarDeskewing")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZ")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZRGB")
@@ -188,10 +190,10 @@ ament_target_dependencies(rtabmap_point_cloud_xyzrgb ${Libraries})
 target_link_libraries(rtabmap_point_cloud_xyzrgb rtabmap_util_plugins)
 set_target_properties(rtabmap_point_cloud_xyzrgb PROPERTIES OUTPUT_NAME "point_cloud_xyzrgb")
 
-#add_executable(rtabmap_data_player src/DbPlayerNode.cpp)
-#ament_target_dependencies(rtabmap_data_player ${Libraries})
-#target_link_libraries(rtabmap_data_player rtabmap_util_plugins)
-#set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player")
+add_executable(rtabmap_data_player src/DbPlayerNode.cpp)
+ament_target_dependencies(rtabmap_data_player ${Libraries})
+target_link_libraries(rtabmap_data_player rtabmap_util_plugins)
+set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player")
 
 #add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp)
 #ament_target_dependencies(rtabmap_odom_msg_to_tf ${Libraries})
@@ -250,7 +252,7 @@ install(TARGETS
 )
 install(TARGETS
 #   rtabmap_map_optimizer
-#   rtabmap_data_player
+   rtabmap_data_player
 #   rtabmap_odom_msg_to_tf
    rtabmap_imu_to_tf
    rtabmap_disparity_to_depth

rtabmap_util/include/rtabmap_util/db_player.hpp

@@ -0,0 +1,105 @@
+/*
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_util/visibility.h>
+#include "rclcpp/rclcpp.hpp"
+
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <image_transport/image_transport.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <rosgraph_msgs/msg/clock.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <rtabmap/core/DBReader.h>
+
+namespace rtabmap_util
+{
+
+class DbPlayer : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit DbPlayer(const rclcpp::NodeOptions & options);
+	virtual ~DbPlayer();
+    bool publishNextFrame();
+    bool isPaused() const {return paused_;}
+    void setPaused(bool enabled) {paused_ = enabled;}
+
+private:
+    void pauseCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void resumeCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+
+private:
+    bool paused_;
+    std::shared_ptr<rtabmap::DBReader> reader_;
+    std::string frameId_;
+    std::string odomFrameId_;
+    std::string cameraFrameId_;
+    std::string scanFrameId_;
+    std::string gtFrameId_;
+    std::string gtBaseFrameId_;
+    int qos_;
+    double scanAngleMin_;
+    double scanAngleMax_;
+    double scanAngleIncrement_;
+    double scanRangeMin_;
+    double scanRangeMax_;
+
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr pauseSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resumeSrv_;
+    
+	image_transport::Publisher imagePub_;
+	image_transport::Publisher rgbPub_;
+	image_transport::Publisher depthPub_;
+	image_transport::Publisher leftPub_;
+	image_transport::Publisher rightPub_;
+    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
+    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
+    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
+    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_;
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometryPub_;
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scanPub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scanCloudPub_;
+	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPosePub_;
+	rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixPub_;
+	rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clockPub_;
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+};
+
+}

rtabmap_util/include/rtabmap_util/rgbd_split.hpp

@@ -50,8 +50,10 @@ public:
 private:
 	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
 
-	image_transport::CameraPublisher rgbPub_;
-	image_transport::CameraPublisher depthPub_;
+	image_transport::Publisher rgbPub_;
+	image_transport::Publisher depthPub_;
+  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
+  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
 };
 
 }

rtabmap_util/src/DbPlayerNode.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -25,36 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <ros/ros.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/image_encodings.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/LaserScan.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/NavSatFix.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <rosgraph_msgs/Clock.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <nav_msgs/Odometry.h>
-#ifdef PRE_ROS_IRON
-#include <cv_bridge/cv_bridge.h>
-#else
-#include <cv_bridge/cv_bridge.hpp>
-#endif
-#include <image_transport/image_transport.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <std_srvs/Empty.h>
-#include <rtabmap_conversions/MsgConversion.h>
-#include <rtabmap_msgs/SetGoal.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UThread.h>
-#include <rtabmap/utilite/UDirectory.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/DBReader.h>
-#include <rtabmap/core/OdometryEvent.h>
-#include <cmath>
+#include "rtabmap_util/db_player.hpp"
+#include "rtabmap/utilite/ULogger.h"
+#include "rclcpp/rclcpp.hpp"
 
 #ifndef _WIN32
 #include <sys/ioctl.h>
@@ -96,604 +69,58 @@ bool spacehit()
 }
 #endif
 
-bool paused = false;
-bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+int main(int argc, char **argv)
 {
-	if(paused)
-	{
-		ROS_WARN("Already paused!");
-	}
-	else
-	{
-		paused = true;
-		ROS_INFO("paused!");
-	}
-	return true;
-}
+	ULogger::setType(ULogger::kTypeConsole);
 
-bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-{
-	if(!paused)
-	{
-		ROS_WARN("Already running!");
-	}
-	else
-	{
-		paused = false;
-		ROS_INFO("resumed!");
-	}
-	return true;
-}
-
-int main(int argc, char** argv)
-{
-	ros::init(argc, argv, "data_player");
-
-	//ULogger::setType(ULogger::kTypeConsole);
-	//ULogger::setLevel(ULogger::kDebug);
-	//ULogger::setEventLevel(ULogger::kWarning);
-
-	bool publishClock = false;
+	std::vector<std::string> arguments;
 	for(int i=1;i<argc;++i)
 	{
-		if(strcmp(argv[i], "--clock") == 0)
-		{
-			publishClock = true;
-		}
+		arguments.push_back(argv[i]);
 	}
 
-	ros::NodeHandle nh;
-	ros::NodeHandle pnh("~");
+	rclcpp::init(argc, argv);
+	rclcpp::NodeOptions options;
+	options.arguments(arguments);
 
-	std::string frameId = "base_link";
-	std::string odomFrameId = "odom";
-	std::string cameraFrameId = "camera_optical_link";
-	std::string scanFrameId = "base_laser_link";
-	std::string gtFrameId = "world";
-	std::string gtBaseFrameId = "base_link_gt";
-	double rate = 1.0f;
-	std::string databasePath = "";
-	bool publishTf = true;
-	int startId = 0;
-	bool useDbStamps = true;
+	auto node = std::make_shared<rtabmap_util::DbPlayer>(options);
 
-	pnh.param("frame_id", frameId, frameId);
-	pnh.param("odom_frame_id", odomFrameId, odomFrameId);
-	pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
-	pnh.param("scan_frame_id", scanFrameId, scanFrameId);
-	pnh.param("ground_truth_frame_id", gtFrameId, gtFrameId);
-	pnh.param("ground_truth_base_frame_id", gtBaseFrameId, gtBaseFrameId);
-	pnh.param("rate", rate, rate); // Ratio of the database stamps
-	pnh.param("database", databasePath, databasePath);
-	pnh.param("publish_tf", publishTf, publishTf);
-	pnh.param("start_id", startId, startId);
+	rclcpp::Rate pauseRate(10);
 
-	// A general 360 lidar with 0.5 deg increment
-	double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
-	pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
-	pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
-	pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
-	pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
-	pnh.param<double>("scan_range_max", scanRangeMax, 60);
-
-	ROS_INFO("frame_id = %s", frameId.c_str());
-	ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
-	ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
-	ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
-	ROS_INFO("ground_truth_frame_id = %s", gtFrameId.c_str());
-	ROS_INFO("rate (factor) = %f", rate);
-	ROS_INFO("publish_tf = %s", publishTf?"true":"false");
-	ROS_INFO("start_id = %d", startId);
-	ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
-
-	if(databasePath.empty())
+	while(rclcpp::ok())
 	{
-		ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
-		return -1;
-	}
-	databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
-	if(databasePath.size() && databasePath.at(0) != '/')
-	{
-		databasePath = UDirectory::currentDir(true) + databasePath;
-	}
-	ROS_INFO("database = %s", databasePath.c_str());
-
-	rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId);
-	if(!reader.init())
-	{
-		ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
-		return -1;
-	}
-
-	ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
-	ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
-
-	image_transport::ImageTransport it(nh);
-	image_transport::Publisher imagePub;
-	image_transport::Publisher rgbPub;
-	image_transport::Publisher depthPub;
-	image_transport::Publisher leftPub;
-	image_transport::Publisher rightPub;
-	ros::Publisher rgbCamInfoPub;
-	ros::Publisher depthCamInfoPub;
-	ros::Publisher leftCamInfoPub;
-	ros::Publisher rightCamInfoPub;
-	ros::Publisher odometryPub;
-	ros::Publisher scanPub;
-	ros::Publisher scanCloudPub;
-	ros::Publisher globalPosePub;
-	ros::Publisher gpsFixPub;
-	ros::Publisher clockPub;
-	tf2_ros::TransformBroadcaster tfBroadcaster;
-
-	if(publishClock)
-	{
-		clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
-	}
-
-	UTimer timer;
-	rtabmap::SensorCaptureInfo cameraInfo;
-	rtabmap::SensorData data = reader.takeImage(&cameraInfo);
-	rtabmap::OdometryInfo odomInfo;
-	odomInfo.reg.covariance = cameraInfo.odomCovariance;
-	rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
-	double acquisitionTime = timer.ticks();
-	while(ros::ok() && odom.data().id())
-	{
-		ROS_INFO("Reading sensor data %d...", odom.data().id());
-
-		ros::Time time(odom.data().stamp());
-
-		if(publishClock)
-		{
-			rosgraph_msgs::Clock msg;
-			msg.clock = time;
-			clockPub.publish(msg);
+		if(!node->publishNextFrame()) {
+			// end of file, exit
+			RCLCPP_INFO(node->get_logger(), "Last frame published, exiting!");
+			break;
 		}
 
-		sensor_msgs::CameraInfo camInfoA; //rgb or left
-		sensor_msgs::CameraInfo camInfoB; //depth or right
-
-		camInfoA.K.assign(0);
-		camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
-		camInfoA.R.assign(0);
-		camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
-		camInfoA.P.assign(0);
-		camInfoA.P[10] = 1;
-
-		camInfoA.header.frame_id = cameraFrameId;
-		camInfoA.header.stamp = time;
-
-		camInfoB = camInfoA;
-
-		int type = -1;
-		if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
-		{
-			if(odom.data().cameraModels().size() > 1)
-			{
-				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
-			}
-			else
-			{
-				//depth
-				if(odom.data().cameraModels().size())
-				{
-					camInfoA.D.resize(5,0);
-
-					camInfoA.P[0] = odom.data().cameraModels()[0].fx();
-					camInfoA.K[0] = odom.data().cameraModels()[0].fx();
-					camInfoA.P[5] = odom.data().cameraModels()[0].fy();
-					camInfoA.K[4] = odom.data().cameraModels()[0].fy();
-					camInfoA.P[2] = odom.data().cameraModels()[0].cx();
-					camInfoA.K[2] = odom.data().cameraModels()[0].cx();
-					camInfoA.P[6] = odom.data().cameraModels()[0].cy();
-					camInfoA.K[5] = odom.data().cameraModels()[0].cy();
-
-					camInfoB = camInfoA;
-				}
-
-				type=0;
-
-				if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
-				if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
-				if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
-				if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
-			}
-		}
-		else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
-		{
-			if(odom.data().stereoCameraModels().size() > 1)
-			{
-				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
-			}
-			else
-			{
-				//stereo
-				if(odom.data().stereoCameraModels()[0].isValidForProjection())
-				{
-					camInfoA.D.resize(8,0);
-
-					camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
-					camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
-					camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
-					camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
-					camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
-					camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
-					camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
-					camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
-
-					camInfoB = camInfoA;
-					camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
-				}
-
-				type=1;
-
-				if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
-				if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
-				if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
-				if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
-			}
-
-		}
-		else
-		{
-			if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
-		}
-
-		camInfoA.height = odom.data().imageRaw().rows;
-		camInfoA.width = odom.data().imageRaw().cols;
-		camInfoB.height = odom.data().depthOrRightRaw().rows;
-		camInfoB.width = odom.data().depthOrRightRaw().cols;
-
-		if(!odom.data().laserScanRaw().isEmpty())
-		{
-			if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
-			{
-				scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
-				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
-				{
-					ROS_INFO("Scan will be published.");
-				}
-				else
-				{
-					ROS_INFO("Scan will be published with those parameters:");
-					ROS_INFO("  scan_angle_min=%f", scanAngleMin);
-					ROS_INFO("  scan_angle_max=%f", scanAngleMax);
-					ROS_INFO("  scan_angle_increment=%f", scanAngleIncrement);
-					ROS_INFO("  scan_range_min=%f", scanRangeMin);
-					ROS_INFO("  scan_range_max=%f", scanRangeMax);
-				}
-			}
-			else if(scanCloudPub.getTopic().empty())
-			{
-				scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
-				ROS_INFO("Scan cloud will be published.");
-			}
-		}
-
-		if(!odom.data().globalPose().isNull() &&
-			odom.data().globalPoseCovariance().cols==6 &&
-			odom.data().globalPoseCovariance().rows==6)
-		{
-			if(globalPosePub.getTopic().empty())
-			{
-				globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
-				ROS_INFO("Global pose will be published.");
-			}
-		}
-
-		if(odom.data().gps().stamp() > 0.0)
-		{
-			if(gpsFixPub.getTopic().empty())
-			{
-				gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
-				ROS_INFO("GPS will be published.");
-			}
-		}
-
-		// publish transforms first
-		if(publishTf)
-		{
-			rtabmap::Transform localTransform;
-			if(odom.data().cameraModels().size() == 1)
-			{
-				localTransform = odom.data().cameraModels()[0].localTransform();
-			}
-			else if(odom.data().stereoCameraModels().size() == 1)
-			{
-				localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
-			}
-			std::vector<geometry_msgs::TransformStamped> transforms;
-			if(!localTransform.isNull())
-			{
-				geometry_msgs::TransformStamped baseToCamera;
-				baseToCamera.child_frame_id = cameraFrameId;
-				baseToCamera.header.frame_id = frameId;
-				baseToCamera.header.stamp = time;
-				rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
-				transforms.push_back(baseToCamera);
-			}
-
-			if(!odom.pose().isNull())
-			{
-				geometry_msgs::TransformStamped odomToBase;
-				odomToBase.child_frame_id = frameId;
-				odomToBase.header.frame_id = odomFrameId;
-				odomToBase.header.stamp = time;
-				rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
-				transforms.push_back(odomToBase);
-			}
-
-			if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
-			{
-				geometry_msgs::TransformStamped baseToLaserScan;
-				baseToLaserScan.child_frame_id = scanFrameId;
-				baseToLaserScan.header.frame_id = frameId;
-				baseToLaserScan.header.stamp = time;
-				rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
-				transforms.push_back(baseToLaserScan);
-			}
-
-			if(!odom.data().groundTruth().isNull()) {
-				geometry_msgs::TransformStamped worldToBase;
-				worldToBase.child_frame_id = gtBaseFrameId;
-				worldToBase.header.frame_id = gtFrameId;
-				worldToBase.header.stamp = time;
-				rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
-				transforms.push_back(worldToBase);
-			}
-			tfBroadcaster.sendTransform(transforms);
-		}
-		if(!odom.pose().isNull())
-		{
-			if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
-
-			if(odometryPub.getNumSubscribers())
-			{
-				nav_msgs::Odometry odomMsg;
-				odomMsg.child_frame_id = frameId;
-				odomMsg.header.frame_id = odomFrameId;
-				odomMsg.header.stamp = time;
-				rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
-				UASSERT(odomMsg.pose.covariance.size() == 36 &&
-						odom.covariance().total() == 36 &&
-						odom.covariance().type() == CV_64FC1);
-				memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
-				odometryPub.publish(odomMsg);
-			}
-		}
-
-		// Publish async topics first (so that they can catched by rtabmap before the image topics)
-		if(globalPosePub.getNumSubscribers() > 0 &&
-			!odom.data().globalPose().isNull() &&
-			odom.data().globalPoseCovariance().cols==6 &&
-			odom.data().globalPoseCovariance().rows==6)
-		{
-			geometry_msgs::PoseWithCovarianceStamped msg;
-			rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
-			memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
-			msg.header.frame_id = frameId;
-			msg.header.stamp = time;
-			globalPosePub.publish(msg);
-		}
-
-		if(odom.data().gps().stamp() > 0.0)
-		{
-			sensor_msgs::NavSatFix msg;
-			msg.longitude = odom.data().gps().longitude();
-			msg.latitude = odom.data().gps().latitude();
-			msg.altitude = odom.data().gps().altitude();
-			msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
-			msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
-			msg.header.frame_id = frameId;
-			msg.header.stamp.fromSec(odom.data().gps().stamp());
-			gpsFixPub.publish(msg);
-		}
-
-		if(type >= 0)
-		{
-			if(rgbCamInfoPub.getNumSubscribers() && type == 0)
-			{
-				rgbCamInfoPub.publish(camInfoA);
-			}
-			if(leftCamInfoPub.getNumSubscribers() && type == 1)
-			{
-				leftCamInfoPub.publish(camInfoA);
-			}
-			if(depthCamInfoPub.getNumSubscribers() && type == 0)
-			{
-				depthCamInfoPub.publish(camInfoB);
-			}
-			if(rightCamInfoPub.getNumSubscribers() && type == 1)
-			{
-				rightCamInfoPub.publish(camInfoB);
-			}
-		}
-
-		if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
-		{
-			cv_bridge::CvImage img;
-			if(odom.data().imageRaw().channels() == 1)
-			{
-				img.encoding = sensor_msgs::image_encodings::MONO8;
-			}
-			else
-			{
-				img.encoding = sensor_msgs::image_encodings::BGR8;
-			}
-			img.image = odom.data().imageRaw();
-			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
-			imageRosMsg->header.frame_id = cameraFrameId;
-			imageRosMsg->header.stamp = time;
-
-			if(imagePub.getNumSubscribers())
-			{
-				imagePub.publish(imageRosMsg);
-			}
-			if(rgbPub.getNumSubscribers() && type == 0)
-			{
-				rgbPub.publish(imageRosMsg);
-			}
-			if(leftPub.getNumSubscribers() && type == 1)
-			{
-				leftPub.publish(imageRosMsg);
-			}
-		}
-
-		if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
-		{
-			cv_bridge::CvImage img;
-			if(odom.data().depthRaw().type() == CV_32FC1)
-			{
-				img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
-			}
-			else
-			{
-				img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
-			}
-			img.image = odom.data().depthRaw();
-			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
-			imageRosMsg->header.frame_id = cameraFrameId;
-			imageRosMsg->header.stamp = time;
-
-			depthPub.publish(imageRosMsg);
-		}
-
-		if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
-		{
-			cv_bridge::CvImage img;
-			img.encoding = sensor_msgs::image_encodings::MONO8;
-			img.image = odom.data().rightRaw();
-			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
-			imageRosMsg->header.frame_id = cameraFrameId;
-			imageRosMsg->header.stamp = time;
-
-			rightPub.publish(imageRosMsg);
-		}
-
-		if(!odom.data().laserScanRaw().isEmpty())
-		{
-			if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
-			{
-				//inspired from pointcloud_to_laserscan package
-				sensor_msgs::LaserScan msg;
-				msg.header.frame_id = scanFrameId;
-				msg.header.stamp = time;
-
-				msg.angle_min = scanAngleMin;
-				msg.angle_max = scanAngleMax;
-				msg.angle_increment = scanAngleIncrement;
-				msg.time_increment = 0.0;
-				msg.scan_time = 0;
-				msg.range_min = scanRangeMin;
-				msg.range_max = scanRangeMax;
-				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
-				{
-					msg.angle_min = odom.data().laserScanRaw().angleMin();
-					msg.angle_max = odom.data().laserScanRaw().angleMax();
-					msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
-					msg.range_min = odom.data().laserScanRaw().rangeMin();
-					msg.range_max = odom.data().laserScanRaw().rangeMax();
-				}
-
-				uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
-				msg.ranges.assign(rangesSize, 0.0);
-
-				const cv::Mat & scan = odom.data().laserScanRaw().data();
-				for (int i=0; i<scan.cols; ++i)
-				{
-					const float * ptr = scan.ptr<float>(0,i);
-					double range = hypot(ptr[0], ptr[1]);
-					if (range >= msg.range_min && range <=msg.range_max)
-					{
-						double angle = atan2(ptr[1], ptr[0]);
-						if (angle >= msg.angle_min && angle <= msg.angle_max)
-						{
-							int index = (angle - msg.angle_min) / msg.angle_increment;
-							if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
-							{
-								msg.ranges[index] = range;
-							}
-						}
-					}
-				}
-
-				scanPub.publish(msg);
-			}
-			else if(scanCloudPub.getNumSubscribers())
-			{
-				sensor_msgs::PointCloud2 msg;
-				pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
-				msg.header.frame_id = scanFrameId;
-				msg.header.stamp = time;
-				scanCloudPub.publish(msg);
-			}
-		}
-
-		if(odom.data().userDataRaw().type() == CV_8SC1 &&
-		   odom.data().userDataRaw().cols >= 7 && // including null str ending
-		   odom.data().userDataRaw().rows == 1 &&
-		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
-		{
-			//GOAL format detected, remove it from the user data and send it as goal event
-			std::string goalStr = (const char *)odom.data().userDataRaw().data;
-			if(!goalStr.empty())
-			{
-				std::list<std::string> strs = uSplit(goalStr, ':');
-				if(strs.size() == 2)
-				{
-					int goalId = atoi(strs.rbegin()->c_str());
-
-					if(goalId > 0)
-					{
-						ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
-						rtabmap_msgs::SetGoal setGoalSrv;
-						setGoalSrv.request.node_id = goalId;
-						setGoalSrv.request.node_label = "";
-						if(!ros::service::call("set_goal", setGoalSrv))
-						{
-							ROS_ERROR("Can't call \"set_goal\" service");
-						}
-					}
-				}
-			}
-		}
-
-		ros::spinOnce();
-
-		while(ros::ok())
+		while(rclcpp::ok())
 		{
 #ifndef _WIN32
 			if (spacehit()) {
-				paused = !paused;
-				if(paused)
+				node->setPaused(!node->isPaused());
+				if(node->isPaused())
 				{
-					ROS_INFO("paused!");
+					RCLCPP_INFO(node->get_logger(), "paused!");
 				}
 				else
 				{
-					ROS_INFO("resumed!");
+					RCLCPP_INFO(node->get_logger(), "resumed!");
 				}
 			}
 #endif
 
-			if(!paused)
+			if(!node->isPaused())
 			{
 				break;
 			}
 
-			uSleep(100);
-			ros::spinOnce();
+			pauseRate.sleep();
+			rclcpp::spin_some(node);
 		}
-
-		timer.restart();
-		cameraInfo = rtabmap::CameraInfo();
-		data = reader.takeImage(&cameraInfo);
-		odomInfo.reg.covariance = cameraInfo.odomCovariance;
-		odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
-		acquisitionTime = timer.ticks();
 	}
 
-
+	rclcpp::shutdown();
 	return 0;
 }

rtabmap_util/src/nodelets/db_player.cpp

@@ -0,0 +1,592 @@
+/*
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_util/db_player.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <image_transport/image_transport.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/SensorData.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#ifdef PRE_ROS_IRON
+#include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
+
+namespace rtabmap_util
+{
+
+DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
+	rclcpp::Node("db_player", options),
+    paused_(false),
+    frameId_("base_link"),
+    odomFrameId_("odom"),
+    cameraFrameId_("camera_optical_link"),
+    scanFrameId_("base_laser_link"),
+    gtFrameId_("world"),
+    gtBaseFrameId_("base_link_gt"),
+    qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)
+{
+    //ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	//ULogger::setEventLevel(ULogger::kWarning);
+
+    //parse input arguments
+    bool publishClock = false;
+    publishClock = this->declare_parameter("publish_clock", publishClock);
+	std::vector<std::string> tmpList = get_node_options().arguments();
+	std::vector<std::string> argList;
+	for(unsigned int i=0; i<tmpList.size(); ++i)
+	{
+	   if(tmpList[i].compare("--clock") == 0)
+		{
+			publishClock = true;
+		}
+	}
+
+	double rate = 1.0f;
+	std::string databasePath = "";
+	bool publishTf = true;
+    bool ignoreOdom = false;
+	int startId = 0;
+    frameId_ =        this->declare_parameter("frame_id", frameId_);
+    odomFrameId_ =    this->declare_parameter("odom_frame_id", odomFrameId_);
+    cameraFrameId_ =  this->declare_parameter("camera_frame_id", cameraFrameId_);
+    scanFrameId_ =    this->declare_parameter("scan_frame_id", scanFrameId_);
+    gtFrameId_ =      this->declare_parameter("ground_truth_frame_id", gtFrameId_);
+    gtBaseFrameId_ =  this->declare_parameter("ground_truth_base_frame_id", gtBaseFrameId_);
+    rate =            this->declare_parameter("rate", rate); // Ratio of the database stamps
+    databasePath =    this->declare_parameter("database", databasePath);
+    publishTf =       this->declare_parameter("publish_tf", publishTf);
+    ignoreOdom =      this->declare_parameter("ignore_odom", ignoreOdom);
+    startId =         this->declare_parameter("start_id", startId);
+	qos_ =            this->declare_parameter("qos", 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);
+
+    RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str());
+	RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str());
+	RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.c_str());
+	RCLCPP_INFO(get_logger(), "scan_frame_id = %s", scanFrameId_.c_str());
+	RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str());
+	RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate);
+	RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false");
+	RCLCPP_INFO(get_logger(), "start_id = %d", startId);
+	RCLCPP_INFO(get_logger(), "Publish clock (--clock): %s", publishClock?"true":"false");
+    RCLCPP_INFO(get_logger(), "qos = %d", qos_);
+
+    if(databasePath.empty())
+	{
+		RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
+		exit(-1);
+	}
+
+    databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
+	if(databasePath.size() && databasePath.at(0) != '/')
+	{
+		databasePath = UDirectory::currentDir(true) + databasePath;
+	}
+	RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str());
+
+	reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId));
+	if(!reader_->init())
+	{
+		RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str());
+		exit(-1);
+	}
+
+    const std::string servicePrefix = get_name() + std::string("/");
+    pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&DbPlayer::pauseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+    if(publishTf) {
+        tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+    }
+
+    if(publishClock)
+	{
+		clockPub_ = this->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
+	}
+}
+
+DbPlayer::~DbPlayer(){}
+
+void DbPlayer::pauseCallback(
+    const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<std_srvs::srv::Empty::Request>,
+    std::shared_ptr<std_srvs::srv::Empty::Response>)
+{
+    if(paused_)
+	{
+		RCLCPP_WARN(get_logger(), "Already paused!");
+	}
+	else
+	{
+		paused_ = true;
+		RCLCPP_INFO(get_logger(), "paused!");
+	}
+}
+void DbPlayer::resumeCallback(
+    const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<std_srvs::srv::Empty::Request>,
+    std::shared_ptr<std_srvs::srv::Empty::Response>)
+{
+    if(!paused_)
+	{
+		RCLCPP_WARN(get_logger(), "Already running!");
+	}
+	else
+	{
+		paused_ = false;
+		RCLCPP_INFO(get_logger(), "resumed!");
+	}
+}
+
+bool DbPlayer::publishNextFrame()
+{
+	rtabmap::SensorCaptureInfo cameraInfo;
+	rtabmap::SensorData data = reader_->takeImage(&cameraInfo);
+	rtabmap::OdometryInfo odomInfo;
+	odomInfo.reg.covariance = cameraInfo.odomCovariance;
+	rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
+    if(!odom.data().id())
+    {
+        return false;
+    }
+
+    RCLCPP_INFO(get_logger(), "Reading sensor data %d...", odom.data().id());
+
+    rclcpp::Time time = rtabmap_conversions::timestampToROS(odom.data().stamp());
+
+    if(clockPub_.get())
+    {
+        rosgraph_msgs::msg::Clock msg;
+        msg.clock = time;
+        clockPub_->publish(msg);
+    }
+
+    sensor_msgs::msg::CameraInfo camInfoA; //rgb or left
+    sensor_msgs::msg::CameraInfo camInfoB; //depth or right
+
+    camInfoA.k.fill(0);
+    camInfoA.k[0] = camInfoA.k[4] = camInfoA.k[8] = 1;
+    camInfoA.r.fill(0);
+    camInfoA.r[0] = camInfoA.r[4] = camInfoA.r[8] = 1;
+    camInfoA.p.fill(0);
+    camInfoA.p[10] = 1;
+
+    camInfoA.header.frame_id = cameraFrameId_;
+    camInfoA.header.stamp = time;
+
+    camInfoB = camInfoA;
+
+    if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
+    {
+        if(odom.data().cameraModels().size() > 1)
+        {
+            RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet...");
+        }
+        else
+        {
+            //depth
+            if(odom.data().cameraModels().size())
+            {
+                camInfoA.d.resize(5,0);
+
+                camInfoA.p[0] = odom.data().cameraModels()[0].fx();
+                camInfoA.k[0] = odom.data().cameraModels()[0].fx();
+                camInfoA.p[5] = odom.data().cameraModels()[0].fy();
+                camInfoA.k[4] = odom.data().cameraModels()[0].fy();
+                camInfoA.p[2] = odom.data().cameraModels()[0].cx();
+                camInfoA.k[2] = odom.data().cameraModels()[0].cx();
+                camInfoA.p[6] = odom.data().cameraModels()[0].cy();
+                camInfoA.k[5] = odom.data().cameraModels()[0].cy();
+
+                camInfoB = camInfoA;
+            }
+
+            if(rgbPub_.getTopic().empty()) rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
+            if(depthPub_.getTopic().empty()) depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
+            if(!rgbInfoPub_.get()) rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 1);
+            if(!depthInfoPub_.get()) depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", 1);
+        }
+    }
+    else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
+    {
+        if(odom.data().stereoCameraModels().size() > 1)
+        {
+            RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet...");
+        }
+        else
+        {
+            //stereo
+            if(odom.data().stereoCameraModels()[0].isValidForProjection())
+            {
+                camInfoA.d.resize(8,0);
+
+                camInfoA.p[0] = odom.data().stereoCameraModels()[0].left().fx();
+                camInfoA.k[0] = odom.data().stereoCameraModels()[0].left().fx();
+                camInfoA.p[5] = odom.data().stereoCameraModels()[0].left().fy();
+                camInfoA.k[4] = odom.data().stereoCameraModels()[0].left().fy();
+                camInfoA.p[2] = odom.data().stereoCameraModels()[0].left().cx();
+                camInfoA.k[2] = odom.data().stereoCameraModels()[0].left().cx();
+                camInfoA.p[6] = odom.data().stereoCameraModels()[0].left().cy();
+                camInfoA.k[5] = odom.data().stereoCameraModels()[0].left().cy();
+
+                camInfoB = camInfoA;
+                camInfoB.p[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
+            }
+
+            if(leftPub_.getTopic().empty()) leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
+            if(rightPub_.getTopic().empty()) rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
+            if(!leftInfoPub_.get()) leftInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 1);
+            if(!rightInfoPub_.get()) rightInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", 1);
+        }
+
+    }
+    else
+    {
+        if(imagePub_.getTopic().empty()) imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
+    }
+
+    camInfoA.height = odom.data().imageRaw().rows;
+    camInfoA.width = odom.data().imageRaw().cols;
+    camInfoB.height = odom.data().depthOrRightRaw().rows;
+    camInfoB.width = odom.data().depthOrRightRaw().cols;
+
+    if(!odom.data().laserScanRaw().isEmpty())
+    {
+        if(!scanPub_.get() && odom.data().laserScanRaw().is2d())
+        {
+            scanPub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 1);
+            if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+            {
+                RCLCPP_INFO(get_logger(), "Scan will be published.");
+            }
+            else
+            {
+                RCLCPP_INFO(get_logger(), "Scan will be published with those parameters:");
+                RCLCPP_INFO(get_logger(), "  scan_angle_min=%f", scanAngleMin_);
+                RCLCPP_INFO(get_logger(), "  scan_angle_max=%f", scanAngleMax_);
+                RCLCPP_INFO(get_logger(), "  scan_angle_increment=%f", scanAngleIncrement_);
+                RCLCPP_INFO(get_logger(), "  scan_range_min=%f", scanRangeMin_);
+                RCLCPP_INFO(get_logger(), "  scan_range_max=%f", scanRangeMax_);
+            }
+        }
+        else if(!scanCloudPub_.get())
+        {
+            scanCloudPub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 1);
+            RCLCPP_INFO(get_logger(), "Scan cloud will be published.");
+        }
+    }
+
+    if(!odom.data().globalPose().isNull() &&
+        odom.data().globalPoseCovariance().cols==6 &&
+        odom.data().globalPoseCovariance().rows==6)
+    {
+        if(!globalPosePub_.get())
+        {
+            globalPosePub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1);
+            RCLCPP_INFO(get_logger(), "Global pose will be published.");
+        }
+    }
+
+    if(odom.data().gps().stamp() > 0.0)
+    {
+        if(!gpsFixPub_.get())
+        {
+            gpsFixPub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("gps/fix", 1);
+            RCLCPP_INFO(get_logger(), "GPS will be published.");
+        }
+    }
+
+    // publish transforms first
+    if(tfBroadcaster_.get())
+    {
+        rtabmap::Transform localTransform;
+        if(odom.data().cameraModels().size() == 1)
+        {
+            localTransform = odom.data().cameraModels()[0].localTransform();
+        }
+        else if(odom.data().stereoCameraModels().size() == 1)
+        {
+            localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
+        }
+        std::vector<geometry_msgs::msg::TransformStamped> transforms;
+        if(!localTransform.isNull())
+        {
+            geometry_msgs::msg::TransformStamped baseToCamera;
+            baseToCamera.child_frame_id = cameraFrameId_;
+            baseToCamera.header.frame_id = frameId_;
+            baseToCamera.header.stamp = time;
+            rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
+            transforms.push_back(baseToCamera);
+        }
+
+        if(!odom.pose().isNull())
+        {
+            geometry_msgs::msg::TransformStamped odomToBase;
+            odomToBase.child_frame_id = frameId_;
+            odomToBase.header.frame_id = odomFrameId_;
+            odomToBase.header.stamp = time;
+            rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
+            transforms.push_back(odomToBase);
+        }
+
+        if(scanPub_.get() || scanCloudPub_.get())
+        {
+            geometry_msgs::msg::TransformStamped baseToLaserScan;
+            baseToLaserScan.child_frame_id = scanFrameId_;
+            baseToLaserScan.header.frame_id = frameId_;
+            baseToLaserScan.header.stamp = time;
+            rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
+            transforms.push_back(baseToLaserScan);
+        }
+
+        if(!odom.data().groundTruth().isNull()) {
+            geometry_msgs::msg::TransformStamped worldToBase;
+            worldToBase.child_frame_id = gtBaseFrameId_;
+            worldToBase.header.frame_id = gtFrameId_;
+            worldToBase.header.stamp = time;
+            rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
+            transforms.push_back(worldToBase);
+        }
+        tfBroadcaster_->sendTransform(transforms);
+    }
+
+    if(!odom.pose().isNull())
+    {
+        if(!odometryPub_.get()) odometryPub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
+
+        if(odometryPub_->get_subscription_count())
+        {
+            nav_msgs::msg::Odometry odomMsg;
+            odomMsg.child_frame_id = frameId_;
+            odomMsg.header.frame_id = odomFrameId_;
+            odomMsg.header.stamp = time;
+            rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
+            UASSERT(odomMsg.pose.covariance.size() == 36 &&
+                    odom.covariance().total() == 36 &&
+                    odom.covariance().type() == CV_64FC1);
+            memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
+            odometryPub_->publish(odomMsg);
+        }
+    }
+
+    // Publish async topics first (so that they can catched by rtabmap before the image topics)
+    if( globalPosePub_.get() && 
+        globalPosePub_->get_subscription_count() > 0 &&
+        !odom.data().globalPose().isNull() &&
+        odom.data().globalPoseCovariance().cols==6 &&
+        odom.data().globalPoseCovariance().rows==6)
+    {
+        geometry_msgs::msg::PoseWithCovarianceStamped msg;
+        rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
+        memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
+        msg.header.frame_id = frameId_;
+        msg.header.stamp = time;
+        globalPosePub_->publish(msg);
+    }
+
+    if( gpsFixPub_.get() &&
+        gpsFixPub_->get_subscription_count() > 0 &&
+        odom.data().gps().stamp() > 0.0)
+    {
+        sensor_msgs::msg::NavSatFix msg;
+        msg.longitude = odom.data().gps().longitude();
+        msg.latitude = odom.data().gps().latitude();
+        msg.altitude = odom.data().gps().altitude();
+        msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+        msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
+        msg.header.frame_id = frameId_;
+        msg.header.stamp = rtabmap_conversions::timestampToROS(odom.data().gps().stamp());
+        gpsFixPub_->publish(msg);
+    }
+
+    if( (imagePub_.getNumSubscribers()) || 
+        (rgbPub_.getNumSubscribers()) || 
+        (leftPub_.getNumSubscribers()))
+    {
+        cv_bridge::CvImage img;
+        if(odom.data().imageRaw().channels() == 1)
+        {
+            img.encoding = sensor_msgs::image_encodings::MONO8;
+        }
+        else
+        {
+            img.encoding = sensor_msgs::image_encodings::BGR8;
+        }
+        img.image = odom.data().imageRaw();
+        sensor_msgs::msg::Image imageRosMsg;
+        img.toImageMsg(imageRosMsg);
+        imageRosMsg.header.frame_id = cameraFrameId_;
+        imageRosMsg.header.stamp = time;
+
+        if(imagePub_.getNumSubscribers())
+        {
+            imagePub_.publish(imageRosMsg);
+        }
+        if(rgbPub_.getNumSubscribers())
+        {
+            rgbPub_.publish(imageRosMsg);
+            rgbInfoPub_->publish(camInfoA);
+        }
+        if(leftPub_.getNumSubscribers())
+        {
+            leftPub_.publish(imageRosMsg);
+            leftInfoPub_->publish(camInfoA);
+        }
+    }
+
+    if(depthPub_.getNumSubscribers() && !odom.data().depthRaw().empty())
+    {
+        cv_bridge::CvImage img;
+        if(odom.data().depthRaw().type() == CV_32FC1)
+        {
+            img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+        }
+        else
+        {
+            img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+        }
+        img.image = odom.data().depthRaw();
+        sensor_msgs::msg::Image imageRosMsg;
+        img.toImageMsg(imageRosMsg);
+        imageRosMsg.header.frame_id = cameraFrameId_;
+        imageRosMsg.header.stamp = time;
+
+        depthPub_.publish(imageRosMsg);
+        depthInfoPub_->publish(camInfoB);
+    }
+
+    if(rightPub_.getNumSubscribers() && !odom.data().rightRaw().empty())
+    {
+        cv_bridge::CvImage img;
+        if(odom.data().imageRaw().channels() == 1)
+        {
+            img.encoding = sensor_msgs::image_encodings::MONO8;
+        }
+        else
+        {
+            img.encoding = sensor_msgs::image_encodings::BGR8;
+        }
+        img.image = odom.data().rightRaw();
+        sensor_msgs::msg::Image imageRosMsg;
+        img.toImageMsg(imageRosMsg);
+        imageRosMsg.header.frame_id = cameraFrameId_;
+        imageRosMsg.header.stamp = time;
+
+        rightPub_.publish(imageRosMsg);
+        rightInfoPub_->publish(camInfoB);
+    }
+
+    if(!odom.data().laserScanRaw().isEmpty())
+    {
+        if(scanPub_.get() && scanPub_->get_subscription_count() && odom.data().laserScanRaw().is2d())
+        {
+            //inspired from pointcloud_to_laserscan package
+            sensor_msgs::msg::LaserScan msg;
+            msg.header.frame_id = scanFrameId_;
+            msg.header.stamp = time;
+
+            msg.angle_min = scanAngleMin_;
+            msg.angle_max = scanAngleMax_;
+            msg.angle_increment = scanAngleIncrement_;
+            msg.time_increment = 0.0;
+            msg.scan_time = 0;
+            msg.range_min = scanRangeMin_;
+            msg.range_max = scanRangeMax_;
+            if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+            {
+                msg.angle_min = odom.data().laserScanRaw().angleMin();
+                msg.angle_max = odom.data().laserScanRaw().angleMax();
+                msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
+                msg.range_min = odom.data().laserScanRaw().rangeMin();
+                msg.range_max = odom.data().laserScanRaw().rangeMax();
+            }
+
+            int rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
+            msg.ranges.assign(rangesSize, 0.0);
+
+            const cv::Mat & scan = odom.data().laserScanRaw().data();
+            for (int i=0; i<scan.cols; ++i)
+            {
+                const float * ptr = scan.ptr<float>(0,i);
+                double range = hypot(ptr[0], ptr[1]);
+                if (range >= msg.range_min && range <=msg.range_max)
+                {
+                    double angle = atan2(ptr[1], ptr[0]);
+                    if (angle >= msg.angle_min && angle <= msg.angle_max)
+                    {
+                        int index = (angle - msg.angle_min) / msg.angle_increment;
+                        if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
+                        {
+                            msg.ranges[index] = range;
+                        }
+                    }
+                }
+            }
+
+            scanPub_->publish(msg);
+        }
+        else if(scanCloudPub_.get() && scanCloudPub_->get_subscription_count())
+        {
+            sensor_msgs::msg::PointCloud2 msg;
+            pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
+            msg.header.frame_id = scanFrameId_;
+            msg.header.stamp = time;
+            scanCloudPub_->publish(msg);
+        }
+    }
+    return true;
+}
+
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::DbPlayer)

rtabmap_util/src/nodelets/rgbd_split.cpp

@@ -46,8 +46,10 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
 
 	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));
 
-	rgbPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	depthPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	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());
+	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);
 }
 
 
@@ -73,7 +75,8 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 			cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
 #endif
 		}
-		rgbPub_.publish(outputImage, outputCameraInfo);
+		rgbPub_.publish(outputImage);
+		rgbInfoPub_->publish(outputCameraInfo);
 	}
 
 	if(depthPub_.getNumSubscribers())
@@ -96,7 +99,8 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 #endif
 		}
 		outputImage.header = outputCameraInfo.header = input->header;
-		depthPub_.publish(outputImage, outputCameraInfo);
+		depthPub_.publish(outputImage);
+		depthInfoPub_->publish(outputCameraInfo);
 	}
 }