Backporting ros2 commit 0ad034680b

rtabmap_launch/launch/rtabmap.launch

@@ -127,6 +127,7 @@
   <arg name="odom_guess_min_rotation"    default="0"/>
   <arg name="odom_max_rate"            default="0"/>
   <arg name="odom_expected_rate"       default="0"/>
+  <arg name="odom_always_process_most_recent_frame" default="true"/>
   <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
   <arg name="wait_imu_to_init"         default="false"/>
   <arg name="use_odom_features"        default="false"/>
@@ -264,6 +265,7 @@
           <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
           <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
           <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
+          <param name="always_process_most_recent_frame" type="bool" value="$(arg odom_always_process_most_recent_frame)"/>
           <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
           <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
         </node>
@@ -295,6 +297,7 @@
           <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
           <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
           <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
+          <param name="always_process_most_recent_frame" type="bool" value="$(arg odom_always_process_most_recent_frame)"/>
           <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
           <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
         </node>
@@ -323,6 +326,7 @@
       <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
       <param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
       <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
+      <param name="always_process_most_recent_frame" type="bool" value="$(arg odom_always_process_most_recent_frame)"/>
       <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
       <param name="deskewing"                   type="bool"   value="$(arg scan_deskewing)"/>
       <param name="deskewing_slerp"             type="bool"   value="$(arg scan_deskewing_slerp)"/>

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -95,6 +95,7 @@ private:
 	virtual void onOdomInit() = 0;
 	virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
 
+	void processData();
 	virtual void mainLoop();
 	virtual void mainLoopKill();
 
@@ -160,9 +161,12 @@ private:
 	rtabmap::Transform guessPreviousPose_;
 	ros::Time previousStamp_;
 	ros::Time previousClockTime_;
+	double lastReceivedTopicClock_;
+	double lastReceivedTopicStamp_;
 	double expectedUpdateRate_;
 	double maxUpdateRate_;
 	double minUpdateRate_;
+	bool alwaysProcessMostRecentFrame_;
 	std::string compressionImgFormat_;
 	bool compressionParallelized_;
 	int odomStrategy_;

rtabmap_odom/src/OdometryROS.cpp

@@ -78,9 +78,12 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	stereoParams_(stereoParams),
 	visParams_(visParams),
 	icpParams_(icpParams),
+	lastReceivedTopicClock_(0.0),
+	lastReceivedTopicStamp_(0.0),
 	expectedUpdateRate_(0.0),
 	maxUpdateRate_(0.0),
 	minUpdateRate_(0.0),
+	alwaysProcessMostRecentFrame_(true),
 	compressionImgFormat_(".jpg"),
 	compressionParallelized_(true),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
@@ -148,6 +151,7 @@ void OdometryROS::onInit()
 	pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate
 	pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
 	pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);
+	pnh.param("always_process_most_recent_frame", alwaysProcessMostRecentFrame_, alwaysProcessMostRecentFrame_);
 
 	pnh.param("sensor_data_compression_format", compressionImgFormat_, compressionImgFormat_);
 	pnh.param("sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);
@@ -373,10 +377,11 @@ void OdometryROS::onInit()
 	Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
 	if(waitIMUToinit_)
 	{
-		int queueSize = 10;
+		int queueSize = 50;
-		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("imu_queue_size", queueSize, queueSize);
-		imuSub_ = nh.subscribe("imu", queueSize*5, &OdometryROS::callbackIMU, this);
+		imuSub_ = nh.subscribe("imu", queueSize, &OdometryROS::callbackIMU, this);
-		NODELET_INFO("odometry: Subscribing to IMU topic %s", imuSub_.getTopic().c_str());
+		NODELET_INFO("odometry: Subscribing to IMU topic %s (imu_queue_size=%d)",
+			imuSub_.getTopic().c_str(), queueSize);
 	}
 
 	this->start();
@@ -460,22 +465,43 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 void OdometryROS::processData(SensorData & data, const std_msgs::Header & header)
 {
 	//NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec());
+	double clockNow = ros::Time::now().toSec();
 	if(dataMutex_.lockTry() == 0)
 	{
 		if(bufferedDataToProcess_) {
-			NODELET_ERROR("We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!",
+			NODELET_ERROR("We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.",
 						dataHeaderToProcess_.stamp.toSec(), header.stamp.toSec());
 		}
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
 		bufferedDataToProcess_ = false;
-		dataReady_.release();
+		if(alwaysProcessMostRecentFrame_) {
+			dataReady_.release();
+		}
 		dataMutex_.unlock();
+		if(!alwaysProcessMostRecentFrame_) {
+			processData();
+		}
 	}
 	else
 	{
-		NODELET_DEBUG("Dropping image/scan data");
+		double estimatedPeriod = clockNow - lastReceivedTopicClock_;
+		double topicPeriod = header.stamp.toSec() - lastReceivedTopicStamp_;
+		if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) {
+			NODELET_WARN("Dropping image/scan data with stamp %f (delay=%f). Something is wrong "
+				"because the clock difference with the previous topic received (%fs) is much lower than the "
+				"expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing "
+				"a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" "
+				"to avoid aggressively dropping data.",
+				header.stamp.toSec(),
+				clockNow - header.stamp.toSec(),
+				estimatedPeriod,
+				topicPeriod,
+				lastReceivedTopicStamp_);
+		}
 	}
+	lastReceivedTopicStamp_ = header.stamp.toSec();
+	lastReceivedTopicClock_ = clockNow;
 }
 
 void OdometryROS::mainLoopKill()
@@ -493,7 +519,11 @@ void OdometryROS::mainLoop()
 		// thread killed
 		return;
 	}
+	processData();
+}
 
+void OdometryROS::processData()
+{
 	UScopeMutex lock(dataMutex_);
 
 	// aliases
@@ -512,21 +542,36 @@ void OdometryROS::mainLoop()
 
 		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec()))
 		{
-			NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.",
+			if(imus_.empty()) {
-					data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
+				// If empty, it is an error!
+				NODELET_ERROR("Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.",
+						data.stamp());
+			}
 			bufferedDataToProcess_ = true;
 			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(header.stamp.toSec());
+		std::map<double, rtabmap::IMU>::iterator iterLast = iterEnd;
 		if(iterEnd!= imus_.end())
 		{
 			++iterEnd;
 		}
-		for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
+		std::map<double, rtabmap::IMU>::iterator iterFirst = imus_.begin();
+		for(std::map<double, rtabmap::IMU>::iterator iter=iterFirst; iter!=iterEnd;)
 		{
-			imus.push_back(*iter);
+			// Because we always keep the last processed imu in the buffer, skip the first 
-			imus_.erase(iter++);
+			// one when processing again the buffer unless its time is lower/equal to image 
+			// current stamp (could happen on initialization).
+			if(iter!=iterFirst || iter->first <= header.stamp.toSec()) {
+				imus.push_back(*iter);
+			}
+			if(iter!=iterLast) {
+				imus_.erase(iter++);
+			}
+			else {
+				++iter;
+			}
 		}
 	}
 
@@ -1193,6 +1238,8 @@ void OdometryROS::reset(const Transform & pose)
 	guessPreviousPose_.setNull();
 	previousStamp_ = ros::Time();
 	previousClockTime_ = ros::Time();
+	lastReceivedTopicClock_ = 0.0;
+	lastReceivedTopicStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -91,7 +91,19 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		int queueSize = 1;
-		pnh.param("queue_size",  queueSize, queueSize);
+		pnh.param("topic_queue_size", queueSize, queueSize);
+		if(pnh.hasParam("queue_size") && !pnh.hasParam("topic_queue_size"))
+		{
+			pnh.param("queue_size", queueSize, queueSize);
+			ROS_WARN("Parameter \"queue_size\" has been renamed "
+					"to \"topic_queue_size\" and will be removed "
+					"in future versions! The value (%d) is still copied to "
+					"\"topic_queue_size\".", queueSize);
+		}
+		else
+		{
+			pnh.param("topic_queue_size", queueSize, queueSize);
+		}
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
 		pnh.param("scan_cloud_is_2d",  scanCloudIs2d_, scanCloudIs2d_);
 		pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);