ROS2 various QOL updates (preparing new binary release) (#1225)

* Updating examples and README

* updated main readme

* odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning.

* update readme

* Added husky demo

* converted demo_robot_mapping.launch to ros2

* Converted stereo_outdoor demo from ros1 to ros2

* Converted multi-session demo to ros2

* Converted demo_find_object launch to ros2

* renamed files

* uniformized default qos, increased sync_queue_size default to 10

* Added vlp16 + imu example

* Added husky 3D lidar demos

* moved demos in subdir

* Added vlp16+zed example

* rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo.

* forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo

* pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces

* Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them.

* Added isaac sim demo

* Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty.

* Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo.

* rtabmap_demos: added README with examples

* Added TOC

* bump version to 0.21.9

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -109,6 +109,7 @@ private:
 
 protected:
 	rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
+	void tick(const rclcpp::Time & stamp);
 
 private:
 	rtabmap::Odometry * odometry_;
@@ -178,10 +179,14 @@ private:
 	bool compressionParallelized_;
 	int odomStrategy_;
 	bool waitIMUToinit_;
+	bool alwaysCheckImuTf_;
 	bool imuProcessed_;
-	std::map<double, rtabmap::IMU> imus_;
+	int processedMsgs_;
+	int droppedMsgs_;
+	std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr> imus_;
 	std::string configPath_;
 	rtabmap::Transform initialPose_;
+	rtabmap::Transform imuLocalTransform_;
 
 	rtabmap_util::ULogToRosout ulogToRosout_;
 
@@ -189,11 +194,13 @@ private:
 	{
 	public:
 		OdomStatusTask();
-		void setStatus(bool isLost);
+		void setStatus(bool isLost, int processedMsgs, int droppedMsgs);
 		void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
 	private:
 		bool lost_;
 		bool dataReceived_;
+		int processedMsgs_;
+		int droppedMsgs_;
 	};
 	OdomStatusTask statusDiagnostic_;
 	std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;

rtabmap_odom/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_odom</name>
-  <version>0.21.5</version>
+  <version>0.21.9</version>
   <description>RTAB-Map's odometry package.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_odom/src/OdometryROS.cpp

@@ -92,7 +92,10 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	compressionParallelized_(true),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
 	waitIMUToinit_(false),
+	alwaysCheckImuTf_(true),
 	imuProcessed_(false),
+	processedMsgs_(0),
+	droppedMsgs_(0),
 	configPath_(),
 	initialPose_(Transform::getIdentity()),
 	ulogToRosout_(this)
@@ -113,11 +116,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	odomSensorDataFeaturesPub_ = create_publisher<rtabmap_msgs::msg::SensorData>("odom_sensor_data/features", rclcpp::QoS(1).reliability(qos_));
 	odomSensorDataCompressedPub_ = create_publisher<rtabmap_msgs::msg::SensorData>("odom_sensor_data/compressed", rclcpp::QoS(1).reliability(qos_));
 
-	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
-	//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
-	//	this->get_node_base_interface(),
-	//	this->get_node_timers_interface());
-	//tfBuffer_->setCreateTimerInterface(timer_interface);
+	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
 	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
@@ -146,6 +145,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_);
 
 	waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_);
+	alwaysCheckImuTf_ = this->declare_parameter("always_check_imu_tf", alwaysCheckImuTf_);
+	
 
 	configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
 	if(configPath_.size() && configPath_.at(0) != '/')
@@ -200,6 +201,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate        = %f Hz", maxUpdateRate_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate        = %f Hz", minUpdateRate_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init       = %s", waitIMUToinit_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "Odometry: always_check_imu_tf    = %s", alwaysCheckImuTf_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
 }
@@ -418,28 +420,20 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 		double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp);
 		//RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds());
 
-		rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
-		if(this->frameId().compare(msg->header.frame_id) != 0)
-		{
-			localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
-		}
-		if(localTransform.isNull())
-		{
-			RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
-					msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
-			return;
-		}
-
-		IMU imu(cv::Vec4d(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w),
-				cv::Mat(3,3,CV_64FC1,(void*)msg->orientation_covariance.data()).clone(),
-				cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
-				cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
-				cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
-				cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
-				localTransform);
-
 		UScopeMutex m(imuMutex_);
-		imus_.insert(std::make_pair(stamp, imu));
+
+		if(!imuProcessed_ && imus_.empty())
+		{
+			rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
+			if(localTransform.isNull())
+			{
+				RCLCPP_WARN(this->get_logger(), "Dropping imu data! A valid TF between %s and %s is required to initialize IMU.",
+					this->frameId().c_str(), msg->header.frame_id.c_str());
+				return;
+			}
+		}
+
+		imus_.insert(std::make_pair(stamp, msg));
 
 		if(imus_.size() > 1000)
 		{
@@ -458,10 +452,12 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 		dataHeaderToProcess_ = header;
 		dataReady_.release();
 		dataMutex_.unlock();
+		++processedMsgs_;
 	}
 	else
 	{
-		RCLCPP_DEBUG(get_logger(), "Dropping image/scan data");
+		//RCLCPP_WARN(get_logger(), "Dropping image/scan data");
+		++droppedMsgs_;
 	}
 }
 
@@ -487,7 +483,7 @@ void OdometryROS::mainLoop()
 	SensorData & data = dataToProcess_;
 	std_msgs::msg::Header & header = dataHeaderToProcess_;
 
-	std::vector<std::pair<double, rtabmap::IMU> > imus;
+	std::vector<std::pair<double, sensor_msgs::msg::Imu::ConstSharedPtr> > imus;
 	{
 		UScopeMutex m(imuMutex_);
 	
@@ -504,21 +500,58 @@ void OdometryROS::mainLoop()
 			return;
 		}
 		// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
-		std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
+		std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
 		if(iterEnd!= imus_.end())
 		{
 			++iterEnd;
 		}
-		for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
+		for(std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iter=imus_.begin(); iter!=iterEnd;)
 		{
 			imus.push_back(*iter);
 			imus_.erase(iter++);
 		}
 	} // end imu lock
 
+	bool imuWarnShown = false;
 	for(size_t i=0; i<imus.size(); ++i)
 	{
-		SensorData dataIMU(imus[i].second, 0, imus[i].first);
+		if((alwaysCheckImuTf_ && !imuWarnShown) || imuLocalTransform_.isNull())
+		{
+			if(this->frameId().compare(imus[i].second->header.frame_id) != 0)
+			{
+				// We should not have to wait for IMU TF (imu delay <<< sensor data delay), so don't
+				rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), imus[i].second->header.frame_id, imus[i].second->header.stamp, *tfBuffer_, 0);
+				if(localTransform.isNull())
+				{
+					if(imuLocalTransform_.isNull()) {
+						RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. All IMU msgs up to sensor data time %f are skipped! If IMU TF is not static, make sure to publish it before the the imu topic is published.",
+								imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp), data.stamp());
+						break;
+					} else if(!imuWarnShown) {
+						imuWarnShown = true; // show only one time
+						RCLCPP_WARN(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. We will use latest known IMU local transform (if TF between camera/lidar and the IMU is static, you can safely ignore this warning and set always_check_imu_tf to false).",
+								imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp));
+					}
+				}
+				else {
+					imuLocalTransform_ = localTransform;
+				}
+			}
+			else if(imuLocalTransform_.isNull())
+			{
+				imuLocalTransform_.setIdentity();
+			}
+		}
+		
+		IMU imu(cv::Vec4d(imus[i].second->orientation.x, imus[i].second->orientation.y, imus[i].second->orientation.z, imus[i].second->orientation.w),
+				cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->orientation_covariance.data()).clone(),
+				cv::Vec3d(imus[i].second->angular_velocity.x, imus[i].second->angular_velocity.y, imus[i].second->angular_velocity.z),
+				cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->angular_velocity_covariance.data()).clone(),
+				cv::Vec3d(imus[i].second->linear_acceleration.x, imus[i].second->linear_acceleration.y, imus[i].second->linear_acceleration.z),
+				cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->linear_acceleration_covariance.data()).clone(),
+				imuLocalTransform_);
+
+		SensorData dataIMU(imu, 0, imus[i].first);
 		odometry_->process(dataIMU);
 		imuProcessed_ = true;
 	}
@@ -644,7 +677,7 @@ void OdometryROS::mainLoop()
 	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_;
 
 	// process data
-	rclcpp::Time timeStart = now();
+	rclcpp::Time timeStart = rclcpp::Clock().now();
 	rtabmap::OdometryInfo info;
 	if(!groundTruth.isNull())
 	{
@@ -1059,23 +1092,25 @@ void OdometryROS::mainLoop()
 	{
 		if(icpParams_)
 		{
-			RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds(), delay);
+			RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
 		}
 		else
 		{
-			RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds(), delay);
+			RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
 		}
 	}
 	else // if(icpParams_)
 	{
-		RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds(), delay);
+		RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
 	}
 
-	statusDiagnostic_.setStatus(pose.isNull());
+	statusDiagnostic_.setStatus(pose.isNull(), processedMsgs_, droppedMsgs_);
+	processedMsgs_ = 0;
+	droppedMsgs_ = 0;
 	if(syncDiagnostic_.get())
 	{
-		double curentRate = 1.0/(this->now()-timeStart).seconds();
-		syncDiagnostic_->tick(header.stamp,
+		double curentRate = 1.0/(rclcpp::Clock().now()-timeStart).seconds();
+		syncDiagnostic_->tickOutput(header.stamp,
 			maxUpdateRate_>0 ? maxUpdateRate_:
 			expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
 			previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
@@ -1117,6 +1152,7 @@ void OdometryROS::reset(const Transform & pose)
 	imuMutex_.lock();
 	imus_.clear();
 	imuMutex_.unlock();
+	imuLocalTransform_.setNull();
 	this->flushCallbacks();
 }
 
@@ -1188,13 +1224,17 @@ void OdometryROS::setLogError(
 OdometryROS::OdomStatusTask::OdomStatusTask() :
 		diagnostic_updater::DiagnosticTask("Odom status"),
 		lost_(false),
-		dataReceived_(false)
+		dataReceived_(false),
+		processedMsgs_(0),
+		droppedMsgs_(0)
 {}
 
-void OdometryROS::OdomStatusTask::setStatus(bool isLost)
+void OdometryROS::OdomStatusTask::setStatus(bool isLost, int processedMsgs, int droppedMsgs)
 {
 	dataReceived_ = true;
 	lost_ = isLost;
+	processedMsgs_ += processedMsgs;
+	droppedMsgs_ += droppedMsgs;
 }
 
 void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
@@ -1211,6 +1251,18 @@ void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrappe
 	{
 		stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Tracking.");
 	}
+	stat.add("Topics Processed", processedMsgs_);
+	stat.add("Topics Dropped", droppedMsgs_);
+	processedMsgs_ = 0;
+	droppedMsgs_ = 0;
+}
+
+void OdometryROS::tick(const rclcpp::Time & stamp)
+{
+	if(syncDiagnostic_.get())
+	{
+		syncDiagnostic_->tickInput(stamp);
+	}
 }
 
 }

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -280,6 +280,9 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 		scan_sub_.reset();
 		return;
 	}
+
+	tick(scanMsg->header.stamp);
+
 	scanReceived_ = true;
 	if(this->isPaused())
 	{
@@ -345,6 +348,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 
 		sensor_msgs::msg::PointCloud2 scanOutDeskewed;
 		rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
+		scanOutDeskewed.header.frame_id = scanMsg->header.frame_id;
 		scanOut = scanOutDeskewed;
 	}
 	else
@@ -523,6 +527,9 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 		cloud_sub_.reset();
 		return;
 	}
+
+	tick(pointCloudMsg->header.stamp);
+
 	cloudReceived_ = true;
 	if(this->isPaused())
 	{

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -64,7 +64,7 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
 		approxSync6_(0),
 		exactSync6_(0),
 		topicQueueSize_(10),
-		syncQueueSize_(2),
+		syncQueueSize_(5),
 		keepColor_(false)
 {
 	OdometryROS::init(false, true, false);
@@ -569,6 +569,8 @@ void RGBDOdometry::callback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depth,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -597,6 +599,8 @@ void RGBDOdometry::callback(
 void RGBDOdometry::callbackRGBDX(
 		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
 {
+	tick(images->header.stamp);
+
 	if(!this->isPaused())
 	{
 		if(images->rgbd_images.empty())
@@ -620,6 +624,8 @@ void RGBDOdometry::callbackRGBDX(
 void RGBDOdometry::callbackRGBD(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -636,6 +642,8 @@ void RGBDOdometry::callbackRGBD2(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
@@ -655,6 +663,8 @@ void RGBDOdometry::callbackRGBD3(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
@@ -677,6 +687,8 @@ void RGBDOdometry::callbackRGBD4(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
@@ -702,6 +714,8 @@ void RGBDOdometry::callbackRGBD5(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
@@ -730,6 +744,8 @@ void RGBDOdometry::callbackRGBD6(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -64,7 +64,7 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
 		approxSync6_(0),
 		exactSync6_(0),
 		topicQueueSize_(10),
-		syncQueueSize_(2),
+		syncQueueSize_(5),
 		keepColor_(false)
 {
 	OdometryROS::init(true, true, false);
@@ -714,6 +714,8 @@ void StereoOdometry::callback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
 {
+	tick(imageRectLeft->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -744,6 +746,8 @@ void StereoOdometry::callback(
 void StereoOdometry::callbackRGBD(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -761,6 +765,8 @@ void StereoOdometry::callbackRGBD(
 void StereoOdometry::callbackRGBDX(
 		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
 {
+	tick(images->header.stamp);
+
 	if(!this->isPaused())
 	{
 		if(images->rgbd_images.empty())
@@ -787,6 +793,8 @@ void StereoOdometry::callbackRGBD2(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
@@ -809,6 +817,8 @@ void StereoOdometry::callbackRGBD3(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
@@ -835,6 +845,8 @@ void StereoOdometry::callbackRGBD4(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
@@ -865,6 +877,8 @@ void StereoOdometry::callbackRGBD5(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(5);
@@ -899,6 +913,8 @@ void StereoOdometry::callbackRGBD6(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
 {
+	tick(image->header.stamp);
+
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(6);