update to cbd3088eba943b434ffc0947de2be11bdc078223

log transport params on boot

include/rtabmap_odom/OdometryROS.h

@@ -109,6 +109,7 @@ class OdometryROS : public rclcpp::Node, public UThread
 
 protected:
 	rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
+	void tick(const rclcpp::Time & stamp);
 
 private:
 	rtabmap::Odometry * odometry_;
@@ -161,6 +162,7 @@ class OdometryROS : public rclcpp::Node, public UThread
 	USemaphore dataReady_;
 	rtabmap::SensorData dataToProcess_;
 	std_msgs::msg::Header dataHeaderToProcess_;
+	bool bufferedDataToProcess_;
 
 	bool paused_;
 	int resetCountdown_;
@@ -178,22 +180,28 @@ class OdometryROS : public rclcpp::Node, public UThread
 	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_;
 
 	class OdomStatusTask : public diagnostic_updater::DiagnosticTask
 	{
 	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_;

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="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

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,33 +420,36 @@ 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;
-		}
+			UScopeMutex m(imuMutex_);
 
-		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);
+			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;
+				}
+			}
 
-		UScopeMutex m(imuMutex_);
-		imus_.insert(std::make_pair(stamp, imu));
+			imus_.insert(std::make_pair(stamp, msg));
 
-		if(imus_.size() > 1000)
+			if(imus_.size() > 1000)
+			{
+				RCLCPP_WARN(this->get_logger(), "Dropping imu data!");
+				imus_.erase(imus_.begin());
+			}
+		}
+		if(dataMutex_.lockTry() == 0)
 		{
-			RCLCPP_WARN(this->get_logger(), "Dropping imu data!");
-			imus_.erase(imus_.begin());
+			if(bufferedDataToProcess_ && rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp) <= stamp)
+			{
+				bufferedDataToProcess_ = false;
+				dataReady_.release();
+			}
+			dataMutex_.unlock();
 		}
 	}
 }
@@ -454,14 +459,22 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 	//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
 	if(dataMutex_.lockTry() == 0)
 	{
+		if(bufferedDataToProcess_) {
+			RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!",
+						rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp));
+			++droppedMsgs_;
+		}
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
+		bufferedDataToProcess_ = false;
 		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 +500,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_);
 
@@ -499,26 +512,64 @@ void OdometryROS::mainLoop()
 
 		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
 		{
-			RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)",
+			RCLCPP_WARN(this->get_logger(), "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.",
 					data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
+			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(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 +695,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())
 	{
@@ -889,10 +940,9 @@ void OdometryROS::mainLoop()
 					"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
 					rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
 		}
-		else
+		else if(--resetCurrentCount_>0)
 		{
 			RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
-			--resetCurrentCount_;
 		}
 
 		if(resetCurrentCount_ == 0 || tooOldPreviousData)
@@ -920,6 +970,11 @@ void OdometryROS::mainLoop()
 					odometry_->reset(tfPose);
 				}
 			}
+			// Keep resetting if the odometry cannot initialize in next updates (e.g., lack of features).
+			// This will make sure we keep updating to latest guess pose.
+			if(resetCurrentCount_ == 0) {
+				++resetCurrentCount_;
+			}
 		}
 	}
 
@@ -1059,23 +1114,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);
@@ -1114,9 +1171,11 @@ void OdometryROS::reset(const Transform & pose)
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();
 	dataHeaderToProcess_ = std_msgs::msg::Header();
+	bufferedDataToProcess_ = false;
 	imuMutex_.lock();
 	imus_.clear();
 	imuMutex_.unlock();
+	imuLocalTransform_.setNull();
 	this->flushCallbacks();
 }
 
@@ -1188,13 +1247,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 +1274,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);
+	}
 }
 
 }