merged master->ros2

src/CoreWrapper.cpp

@@ -126,8 +126,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		mappingAltitudeDelta_(Parameters::defaultGridGlobalAltitudeDelta()),
 		alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
 		twoDMapping_(Parameters::defaultRegForce3DoF()),
-		previousStamp_(0),
-		maxNodesRepublished_(2)
+		previousStamp_(0)
 {
 	char * rosHomePath = getenv("ROS_HOME");
 	std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
@@ -145,6 +144,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
 	bool publishTf = true;
+	std::string initialPoseStr;
 	tfDelay = 0.05; // 20 Hz
 	tfTolerance = 0.1; // 100 ms
 	std::string odomFrameIdInit;
@@ -183,9 +183,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	landmarkDefaultLinVariance_ = this->declare_parameter("landmark_linear_variance", landmarkDefaultLinVariance_);
 
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
+	initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr);
 	useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
 	useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_);
-	maxNodesRepublished_ = this->declare_parameter("max_nodes_republished", maxNodesRepublished_);
 	genScan_ = this->declare_parameter("gen_scan", genScan_);
 	genScanMaxDepth_ = this->declare_parameter("gen_scan_max_depth", genScanMaxDepth_);
 	genScanMinDepth_ = this->declare_parameter("gen_scan_min_depth", genScanMinDepth_);
@@ -211,6 +211,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 				groundTruthBaseFrameId_.c_str());
 	}
 	RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id  = %s", mapFrameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose  = %s", initialPoseStr.c_str());
 	RCLCPP_INFO(this->get_logger(), "rtabmap: use_action_for_goal  = %s", useActionForGoal_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay      = %f", tfDelay);
 	RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance  = %f", tfTolerance);
@@ -765,6 +766,21 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		}
 	}
 
+	// Set initial pose if set
+	if(!initialPoseStr.empty())
+	{
+		Transform intialPose = Transform::fromString(initialPoseStr);
+		if(!intialPose.isNull())
+		{
+			RCLCPP_INFO(this->get_logger(), "Setting initial pose: \"%s\"", intialPose.prettyPrint().c_str());
+			rtabmap_.setInitialPose(intialPose);
+		}
+		else
+		{
+			RCLCPP_ERROR(this->get_logger(), "Invalid initial_pose: \"%s\"", initialPoseStr.c_str());
+		}
+	}
+
 	// Update declared parameters
 	std::vector<rclcpp::Parameter> rosParameters;
 	for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
@@ -2361,22 +2377,7 @@ void CoreWrapper::imuAsyncCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
 
 void CoreWrapper::republishNodeDataCallback(const std_msgs::msg::Int32MultiArray::ConstSharedPtr msg)
 {
-	if(maxNodesRepublished_>0)
-	{
-		nodesToRepublish_.insert(msg->data.begin(), msg->data.end());
-	}
-	else
-	{
-		static bool warned = false;
-		if(!warned)
-		{
-			RCLCPP_WARN(get_logger(), "A node is requesting some node data "
-					"to be republished after the next update, "
-					"but parameter \"max_nodes_republished\" is not over 0, "
-					"ignoring the call. This warning is only printed once.");
-			warned = true;
-		}
-	}
+	rtabmap_.addNodesToRepublish(msg->data);
 }
 
 void CoreWrapper::interOdomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
@@ -2681,7 +2682,6 @@ void CoreWrapper::resetRtabmapCallback(
 	mapToOdomMutex_.lock();
 	mapToOdom_.setIdentity();
 	mapToOdomMutex_.unlock();
-	nodesToRepublish_.clear();
 }
 
 void CoreWrapper::pauseRtabmapCallback(
@@ -2775,7 +2775,6 @@ void CoreWrapper::loadDatabaseCallback(
 	mapToOdomMutex_.lock();
 	mapToOdom_.setIdentity();
 	mapToOdomMutex_.unlock();
-	nodesToRepublish_.clear();
 
 	// Open new database
 	databasePath_ = newDatabasePath;
@@ -2893,7 +2892,6 @@ void CoreWrapper::backupDatabaseCallback(
 	globalPose_.header.stamp = rclcpp::Time(0);
 	gps_ = rtabmap::GPS();
 	tags_.clear();
-	nodesToRepublish_.clear();
 
 	RCLCPP_INFO(this->get_logger(), "Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
 	UFile::copy(databasePath_, databasePath_+".back");
@@ -3957,67 +3955,10 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 		msg->header.stamp = stamp;
 		msg->header.frame_id = mapFrameId_;
 
-		std::map<int, Signature> signatures;
-		if(stats.getLastSignatureData().id() > 0)
-		{
-			signatures.insert(std::make_pair(stats.getLastSignatureData().id(), stats.getLastSignatureData()));
-		}
-
-		if(nodesToRepublish_.size() && !rtabmap_.getLastLocalizationPose().isNull())
-		{
-			// Republish data from closest nodes of the current localization
-			std::map<int, Transform> nodesOnly(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());
-			int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose());
-			if(id>0)
-			{
-				std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, true, false, true);
-				std::multimap<int, int> missingIds;
-				for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
-				{
-					if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end())
-					{
-						missingIds.insert(std::make_pair(iter->second, iter->first));
-					}
-				}
-
-				if(nodesToRepublish_.size() != missingIds.size())
-				{
-					// remove requested nodes not anymore in the graph
-					for(std::set<int>::iterator iter=nodesToRepublish_.begin(); iter!=nodesToRepublish_.end();)
-					{
-						if(ids.find(*iter) == ids.end())
-						{
-							iter = nodesToRepublish_.erase(iter);
-						}
-						else
-						{
-							++iter;
-						}
-					}
-				}
-
-				int loaded = 0;
-				std::stringstream stream;
-				for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
-				{
-					signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true)));
-					nodesToRepublish_.erase(iter->second);
-					++loaded;
-					stream << iter->second << " ";
-				}
-				if(loaded)
-				{
-					RCLCPP_WARN(get_logger(), "Republishing data of requested node(s) %sfrom \"%s\" input topic (max_nodes_republished=%d)",
-							stream.str().c_str(),
-							republishNodeDataSub_->get_topic_name(),
-							maxNodesRepublished_);
-				}
-			}
-		}
 		rtabmap_ros::mapDataToROS(
 			stats.poses(),
 			stats.constraints(),
-			signatures,
+			stats.getSignaturesData(),
 			stats.mapCorrection(),
 			*msg);
 
@@ -4157,7 +4098,8 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 			nav_msgs::msg::Path path;
 			if(pubPath)
 			{
-				path.poses.resize(stats.poses().size());
+				// Ignore pose of current location in Localization mode
+				path.poses.resize(stats.poses().size()-(rtabmap_.getMemory()->isIncremental()?0:1));
 			}
 			int oi = 0;
 			for(std::map<int, Transform>::const_iterator poseIter=stats.poses().begin();
@@ -4225,7 +4167,7 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 
 					markers.markers.push_back(marker);
 				}
-				if(pubPath)
+				if(pubPath && (rtabmap_.getMemory()->isIncremental() || poseIter->first != stats.poses().rbegin()->first))
 				{
 					rtabmap_ros::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
 					path.poses.at(oi).header.frame_id = mapFrameId_;

src/GuiWrapper.cpp

@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QDir>
 
 #include <std_srvs/srv/empty.hpp>
-#include <std_msgs/msg/empty.hpp>
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -141,6 +140,8 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
 	UEventsManager::addHandler(this);
 	UEventsManager::addHandler(mainWindow_);
 
+	republishNodeDataPub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("republish_node_data", 1);
+
 	infoTopic_.subscribe(this, "info");
 	mapDataTopic_.subscribe(this, "mapData");
 	infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
@@ -191,10 +192,7 @@ void GuiWrapper::infoMapCallback(
 
 	stat.setMapCorrection(mapToOdom);
 	stat.setPoses(poses);
-	if(signatures.size())
-	{
-		stat.setLastSignatureData(signatures.rbegin()->second);
-	}
+	stat.setSignaturesData(signatures);
 	stat.setConstraints(links);
 
 	this->post(new RtabmapEvent(stat));
@@ -465,6 +463,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 				RCLCPP_WARN(this->get_logger(), "Service \"remove_label\" not available.");
 			}
 		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdRepublishData)
+		{
+			UASSERT(cmdEvent->value1().isIntArray());
+			std_msgs::msg::Int32MultiArray msg;
+			msg.data = cmdEvent->value1().toIntArray();
+			republishNodeDataPub_->publish(msg);
+		}
 		else
 		{
 			RCLCPP_WARN(this->get_logger(), "Not handled command (%d)...", cmd);

src/LidarDeskewingNode.cpp

@@ -0,0 +1,37 @@
+/*
+Copyright (c) 2010-2016, 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_ros/lidar_deskewing.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_ros::LidarDeskewing>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}

src/MsgConversion.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Compression.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <image_geometry/pinhole_camera_model.h>
 #include <image_geometry/stereo_camera_model.h>
@@ -1852,7 +1853,7 @@ bool convertRGBDMsgs(
 		}
 	}
 
-	if(isDepth)
+	if(isDepth && !depthMsgs.empty())
 	{
 		UASSERT_MSG(
 				imageWidth/depthWidth == imageHeight/depthHeight,
@@ -2592,4 +2593,417 @@ transformPointCloud (
   }
 }
 
+bool deskew_impl(
+		const sensor_msgs::msg::PointCloud2 & input,
+		sensor_msgs::msg::PointCloud2 & output,
+		const std::string & fixedFrameId,
+		tf2_ros::Buffer * tfBuffer,
+		double waitForTransform,
+		bool slerp,
+		const rtabmap::Transform & velocity,
+		double previousStamp)
+{
+	if(tfBuffer != 0)
+	{
+		if(input.header.frame_id.empty())
+		{
+			UERROR("Input cloud has empty frame_id!");
+			return false;
+		}
+
+		if(fixedFrameId.empty())
+		{
+			UERROR("fixedFrameId parameter should be set!");
+			return false;
+		}
+	}
+	else
+	{
+		if(!slerp)
+		{
+			UERROR("slerp should be true when constant velocity model is used!");
+			return false;
+		}
+
+		if(previousStamp <= 0.0)
+		{
+			UERROR("previousStamp should be >0 when constant velocity model is used!");
+			return false;
+		}
+
+		if(velocity.isNull())
+		{
+			UERROR("velocity should be valid when constant velocity model is used!");
+			return false;
+		}
+	}
+
+	int offsetTime = -1;
+	int offsetX = -1;
+	int offsetY = -1;
+	int offsetZ = -1;
+	int timeDatatype = 6;
+	for(size_t i=0; i<input.fields.size(); ++i)
+	{
+		if(input.fields[i].name.compare("t") == 0)
+		{
+			if(offsetTime != -1)
+			{
+				UWARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
+			}
+			offsetTime = input.fields[i].offset;
+			timeDatatype = input.fields[i].datatype;
+		}
+		else if(input.fields[i].name.compare("time") == 0)
+		{
+			if(offsetTime != -1)
+			{
+				UWARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
+			}
+			offsetTime = input.fields[i].offset;
+			timeDatatype = input.fields[i].datatype;
+		}
+		else if(input.fields[i].name.compare("stamps") == 0)
+		{
+			if(offsetTime != -1)
+			{
+				UWARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
+			}
+			offsetTime = input.fields[i].offset;
+			timeDatatype = input.fields[i].datatype;
+		}
+		else if(input.fields[i].name.compare("x") == 0)
+		{
+			UASSERT(input.fields[i].datatype==7);
+			offsetX = input.fields[i].offset;
+		}
+		else if(input.fields[i].name.compare("y") == 0)
+		{
+			UASSERT(input.fields[i].datatype==7);
+			offsetY = input.fields[i].offset;
+		}
+		else if(input.fields[i].name.compare("z") == 0)
+		{
+			UASSERT(input.fields[i].datatype==7);
+			offsetZ = input.fields[i].offset;
+		}
+	}
+
+	if(offsetTime < 0)
+	{
+		UERROR("Input cloud doesn't have \"t\" or \"time\" field!");
+		return false;
+	}
+	if(offsetX < 0)
+	{
+		UERROR("Input cloud doesn't have \"x\" field!");
+		return false;
+	}
+	if(offsetY < 0)
+	{
+		UERROR("Input cloud doesn't have \"y\" field!");
+		return false;
+	}
+	if(offsetZ < 0)
+	{
+		UERROR("Input cloud doesn't have \"z\" field!");
+		return false;
+	}
+	if(input.height == 0)
+	{
+		UERROR("Input cloud height is zero!");
+		return false;
+	}
+	if(input.width == 0)
+	{
+		UERROR("Input cloud width is zero!");
+		return false;
+	}
+
+	bool timeOnColumns = input.width > input.height;
+
+	// Get latest timestamp
+	rclcpp::Time firstStamp;
+	rclcpp::Time lastStamp;
+	if(timeDatatype == 6) // UINT32
+	{
+		unsigned int nsec = *((const unsigned int*)(&input.data[0]+offsetTime));
+		firstStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsec);
+		nsec = *((const unsigned int*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
+		lastStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsec);
+	}
+	else if(timeDatatype == 7) // FLOAT32
+	{
+		float sec = *((const float*)(&input.data[0]+offsetTime));
+		firstStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(sec);
+		sec = *((const float*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
+		lastStamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(sec);
+	}
+	else
+	{
+		UERROR("Not supported time datatype %d!", timeDatatype);
+		return false;
+	}
+
+	if(lastStamp <= firstStamp)
+	{
+		UERROR("First and last stamps in the scan are the same!");
+		return false;
+	}
+	std::string errorMsg;
+	if(tfBuffer != 0 &&
+	   waitForTransform>0.0 &&
+	   !tfBuffer->canTransform(
+			input.header.frame_id,
+			firstStamp,
+			input.header.frame_id,
+			lastStamp,
+			fixedFrameId,
+			rclcpp::Duration::from_seconds(waitForTransform),
+			&errorMsg))
+	{
+		UERROR("Could not estimate motion of %s accordingly to fixed frame %s between stamps %f and %f! (%s)",
+				input.header.frame_id.c_str(),
+				fixedFrameId.c_str(),
+				timestampFromROS(firstStamp),
+				timestampFromROS(lastStamp),
+				errorMsg.c_str());
+		return false;
+	}
+
+	rtabmap::Transform firstPose;
+	rtabmap::Transform lastPose;
+	double scanTime = 0;
+	if(slerp)
+	{
+		if(tfBuffer != 0)
+		{
+			firstPose = rtabmap_ros::getTransform(
+					input.header.frame_id,
+					fixedFrameId,
+					firstStamp,
+					input.header.stamp,
+					*tfBuffer,
+					0);
+			lastPose = rtabmap_ros::getTransform(
+					input.header.frame_id,
+					fixedFrameId,
+					lastStamp,
+					input.header.stamp,
+					*tfBuffer,
+					0);
+		}
+		else
+		{
+			float vx,vy,vz, vroll,vpitch,vyaw;
+			velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+
+			// We need three poses:
+			//  1- The pose of base frame in odom frame at first stamp
+			//  2- The pose of base frame in odom frame at msg stamp
+			//  3- The pose of base frame in odom frame at last stamp
+			UASSERT(timestampFromROS(firstStamp) >= previousStamp);
+			UASSERT(timestampFromROS(lastStamp) > previousStamp);
+			double dt1 = timestampFromROS(firstStamp) - previousStamp;
+			double dt2 = timestampFromROS(input.header.stamp) - previousStamp;
+			double dt3 = timestampFromROS(lastStamp) - previousStamp;
+
+			rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
+			rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
+			rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
+
+			// First and last poses are relative to stamp of the msg
+			firstPose = p2.inverse() * p1;
+			lastPose = p2.inverse() * p3;
+		}
+
+		if(firstPose.isNull())
+		{
+			UERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+					input.header.frame_id.c_str(),
+					fixedFrameId.empty()?"velocity":fixedFrameId.c_str(),
+					timestampFromROS(firstStamp),
+					timestampFromROS(input.header.stamp));
+			return false;
+		}
+		if(lastPose.isNull())
+		{
+			UERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+					input.header.frame_id.c_str(),
+					fixedFrameId.empty()?"velocity":fixedFrameId.c_str(),
+					timestampFromROS(lastStamp),
+					timestampFromROS(input.header.stamp));
+			return false;
+		}
+		scanTime = timestampFromROS(lastStamp) - timestampFromROS(firstStamp);
+	}
+	//else tf will be used to get more accurate transforms
+
+	output = input;
+	rclcpp::Time stamp;
+	UTimer processingTime;
+	if(timeOnColumns)
+	{
+		// ouster point cloud:
+		// t1     t2    ...
+		// ring1  ring1 ...
+		// ring2  ring2 ...
+		// ring3  ring4 ...
+		// ring4  ring3 ...
+		for(size_t u=0; u<output.width; ++u)
+		{
+			if(timeDatatype == 6) // UINT32
+			{
+				unsigned int nsec = *((const unsigned int*)(&output.data[u*output.point_step]+offsetTime));
+				stamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsec);
+			}
+			else
+			{
+				float sec = *((const float*)(&output.data[u*output.point_step]+offsetTime));
+				stamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(sec);
+			}
+
+			rtabmap::Transform transform;
+			if(slerp)
+			{
+				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+			}
+			else
+			{
+				transform = rtabmap_ros::getTransform(
+						output.header.frame_id,
+						fixedFrameId,
+						stamp,
+						output.header.stamp,
+						*tfBuffer,
+						0);
+				if(transform.isNull())
+				{
+					UERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+							output.header.frame_id.c_str(),
+							fixedFrameId.c_str(),
+							timestampFromROS(stamp),
+							timestampFromROS(output.header.stamp));
+					return false;
+				}
+			}
+
+			for(size_t v=0; v<input.height; ++v)
+			{
+				unsigned char * dataPtr = &output.data[v*output.row_step + u*output.point_step];
+				float & x = *((float*)(dataPtr+offsetX));
+				float & y = *((float*)(dataPtr+offsetY));
+				float & z = *((float*)(dataPtr+offsetZ));
+				pcl::PointXYZ pt(x,y,z);
+				pt = rtabmap::util3d::transformPoint(pt, transform);
+				x = pt.x;
+				y = pt.y;
+				z = pt.z;
+
+				// set delta stamp to zero so that on downstream they know the cloud is deskewed
+				if(timeDatatype == 6) // UINT32
+				{
+					*((unsigned int*)(dataPtr+offsetTime)) = 0;
+				}
+				else
+				{
+					*((float*)(dataPtr+offsetTime)) = 0;
+				}
+			}
+		}
+	}
+	else // time on rows
+	{
+		// velodyne point cloud:
+		// t1     ring1 ring2 ring3 ring4
+		// t2     ring1 ring2 ring3 ring4
+		// t3     ring1 ring2 ring3 ring4
+		// t4     ring1 ring2 ring3 ring4
+		// ...    ...   ...   ...   ...
+		for(size_t v=0; v<output.height; ++v)
+		{
+			if(timeDatatype == 6) // UINT32
+			{
+				unsigned int nsec = *((const unsigned int*)(&output.data[v*output.row_step]+offsetTime));
+				stamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration(0, nsec);
+			}
+			else
+			{
+				float sec = *((const float*)(&output.data[v*output.row_step]+offsetTime));
+				stamp = rclcpp::Time(input.header.stamp)+rclcpp::Duration::from_seconds(sec);
+			}
+
+			rtabmap::Transform transform;
+			if(slerp)
+			{
+				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+			}
+			else
+			{
+				transform = rtabmap_ros::getTransform(
+						output.header.frame_id,
+						fixedFrameId,
+						stamp,
+						output.header.stamp,
+						*tfBuffer,
+						0);
+				if(transform.isNull())
+				{
+					UERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+							output.header.frame_id.c_str(),
+							fixedFrameId.c_str(),
+							timestampFromROS(stamp),
+							timestampFromROS(output.header.stamp));
+					return false;
+				}
+			}
+
+			for(size_t u=0; u<input.width; ++u)
+			{
+				unsigned char * dataPtr = &output.data[v*output.row_step + u*output.point_step];
+				float & x = *((float*)(dataPtr+offsetX));
+				float & y = *((float*)(dataPtr+offsetY));
+				float & z = *((float*)(dataPtr+offsetZ));
+				pcl::PointXYZ pt(x,y,z);
+				pt = rtabmap::util3d::transformPoint(pt, transform);
+				x = pt.x;
+				y = pt.y;
+				z = pt.z;
+
+				// set delta stamp to zero so that on downstream they know the cloud is deskewed
+				if(timeDatatype == 6) // UINT32
+				{
+					*((unsigned int*)(dataPtr+offsetTime)) = 0;
+				}
+				else
+				{
+					*((float*)(dataPtr+offsetTime)) = 0;
+				}
+			}
+		}
+	}
+	UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
+	return true;
+}
+
+bool deskew(
+		const sensor_msgs::msg::PointCloud2 & input,
+		sensor_msgs::msg::PointCloud2 & output,
+		const std::string & fixedFrameId,
+		tf2_ros::Buffer & tfBuffer,
+		double waitForTransform,
+		bool slerp)
+{
+	return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0);
+}
+
+bool deskew(
+		const sensor_msgs::msg::PointCloud2 & input,
+		sensor_msgs::msg::PointCloud2 & output,
+		double previousStamp,
+		const rtabmap::Transform & velocity)
+{
+	return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp);
+}
+
 }

src/OdometryROS.cpp

@@ -383,6 +383,15 @@ void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bo
 	});
 }
 
+rtabmap::Transform OdometryROS::velocityGuess() const
+{
+	if(odometry_)
+	{
+		return odometry_->getVelocityGuess();
+	}
+	return rtabmap::Transform();
+}
+
 void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 {
 	if(!this->isPaused())

src/nodelets/icp_odometry.cpp

@@ -57,6 +57,8 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
 	scanNormalK_(0),
 	scanNormalRadius_(0.0),
 	scanNormalGroundUp_(0.0),
+	deskewing_(false),
+	deskewingSlerp_(false),
 	//plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"),
 	scanReceived_(false),
 	cloudReceived_(false)
@@ -118,6 +120,8 @@ void ICPOdometry::onOdomInit()
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k          = %d", scanNormalK_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius     = %f m", scanNormalRadius_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing              = %s", deskewing_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp        = %s", deskewingSlerp_?"true":"false");
 
 	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
 	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
@@ -303,7 +307,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 	// make sure the frame of the laser is updated too
 	Transform localScanTransform = getTransform(this->frameId(),
 			scanMsg->header.frame_id,
-			rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment),
+			scanMsg->header.stamp,
 			tfBuffer(), waitForTransform());
 	if(localScanTransform.isNull())
 	{
@@ -314,7 +318,37 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 	//transform in frameId_ frame
 	sensor_msgs::msg::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
-	projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfBuffer());
+	if(deskewing_ && !guessFrameId().empty())
+	{
+		projection.transformLaserScanToPointCloud(deskewing_&&!guessFrameId().empty()?guessFrameId():scanMsg->header.frame_id, *scanMsg, scanOut, this->tfBuffer());
+
+		rtabmap::Transform t = rtabmap_ros::getTransform(scanOut.header.frame_id, scanMsg->header.frame_id, scanMsg->header.stamp, tfBuffer(), waitForTransform());
+		if(t.isNull())
+		{
+			RCLCPP_ERROR(this->get_logger(), "Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+					scanOut.header.frame_id.c_str(), scanMsg->header.frame_id.c_str(), timestampFromROS(scanMsg->header.stamp));
+			return;
+		}
+
+		sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+		rtabmap_ros::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
+		scanOut = scanOutDeskewed;
+	}
+	else
+	{
+		projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
+
+		if(previousStamp() > 0 && !velocityGuess().isNull())
+		{
+			// deskew with constant velocity model
+			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+			if(!deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+		}
+	}
 
 	bool hasIntensity = false;
 	for(unsigned int i=0; i<scanOut.fields.size(); ++i)
@@ -507,6 +541,28 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 		cloudMsg = *pointCloudMsg;
 	}
 
+	if(deskewing_)
+	{
+		if(!guessFrameId().empty())
+		{
+			// deskew with TF
+			if(!deskew(*pointCloudMsg, cloudMsg, guessFrameId(), tfBuffer(), waitForTransform(), deskewingSlerp_))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+		}
+		else if(previousStamp() > 0 && !velocityGuess().isNull())
+		{
+			// deskew with constant velocity model
+			if(!deskew(*pointCloudMsg, cloudMsg, previousStamp(), velocityGuess()))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+		}
+	}
+
 	LaserScan scan;
 	bool hasNormals = false;
 	bool hasIntensity = false;

src/nodelets/lidar_deskewing.cpp

@@ -0,0 +1,87 @@
+
+#include <rtabmap_ros/lidar_deskewing.hpp>
+
+#include <laser_geometry/laser_geometry.hpp>
+
+#include <rtabmap_ros/MsgConversion.h>
+
+namespace rtabmap_ros
+{
+
+LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
+	Node("pointcloud_to_depthimage", options),
+	waitForTransformDuration_(0.01),
+	slerp_(false)
+{
+	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);
+	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
+
+	int queueSize = 5;
+	int qos = 0;
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
+	waitForTransformDuration_ = this->declare_parameter("wait_for_transform", waitForTransformDuration_);
+	slerp_ = this->declare_parameter("slerp", slerp_);
+
+	RCLCPP_INFO(this->get_logger(), "  fixed_frame_id:  %s", fixedFrameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "  wait_for_transform:  %fs", waitForTransformDuration_);
+	RCLCPP_INFO(this->get_logger(), "  slerp:  %s", slerp_?"true":"false");
+
+	if(fixedFrameId_.empty())
+	{
+		RCLCPP_FATAL(this->get_logger(), "fixed_frame_id parameter cannot be empty!");
+	}
+
+	subScan_ = create_subscription<sensor_msgs::msg::LaserScan>("input_scan", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackScan, this, std::placeholders::_1));
+	subCloud_ = create_subscription<sensor_msgs::msg::PointCloud2>("input_cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackCloud, this, std::placeholders::_1));
+
+	pubScan_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subScan_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	pubCloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subCloud_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+}
+
+LidarDeskewing::~LidarDeskewing()
+{
+}
+
+void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
+{
+	sensor_msgs::msg::PointCloud2 scanOut;
+	laser_geometry::LaserProjection projection;
+	projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfBuffer_);
+
+	rtabmap::Transform t = rtabmap_ros::getTransform(msg->header.frame_id, scanOut.header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransformDuration_);
+	if(t.isNull())
+	{
+		RCLCPP_ERROR(this->get_logger(), "Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+				scanOut.header.frame_id.c_str(), msg->header.frame_id.c_str(), rtabmap_ros::timestampFromROS(msg->header.stamp));
+		return;
+	}
+	sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+	rtabmap_ros::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
+	pubScan_->publish(scanOutDeskewed);
+}
+
+void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
+{
+	sensor_msgs::msg::PointCloud2 msgDeskewed;
+	if(deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_))
+	{
+		pubCloud_->publish(msgDeskewed);
+	}
+	else
+	{
+		// Just republish the msg to not breakdown downstream
+		// A warning should be already shown (see deskew() source code)
+		RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!");
+		pubCloud_->publish(*msg);
+	}
+}
+
+}
+
+

src/nodelets/point_cloud_xyz.cpp

@@ -173,7 +173,6 @@ void PointCloudXYZ::callback(
 		rclcpp::Time time = now();
 
 		cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
-		cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
 
 		rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo);
 

src/nodelets/pointcloud_to_depthimage.cpp

@@ -48,6 +48,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 		fillHolesError_(0.1),
 		fillIterations_(1),
 		decimation_(1),
+		upscale_(false),
+		upscaleDepthErrorRatio_(0.02),
 		approxSync_(0),
 		exactSync_(0)
 {
@@ -71,6 +73,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 	fillIterations_ = this->declare_parameter("fill_iterations", fillIterations_);
 	decimation_ = this->declare_parameter("decimation", decimation_);
 	approx = this->declare_parameter("approx", approx);
+	upscale_ = this->declare_parameter("upscale", upscale_);
+	upscaleDepthErrorRatio_ = this->declare_parameter("upscale_depth_error_ratio", upscaleDepthErrorRatio_);
 
 	if(fixedFrameId_.empty() && approx)
 	{
@@ -89,6 +93,7 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 	RCLCPP_INFO(this->get_logger(), "  fill_holes_error=%f", fillHolesError_);
 	RCLCPP_INFO(this->get_logger(), "  fill_iterations=%d", fillIterations_);
 	RCLCPP_INFO(this->get_logger(), "  decimation=%d", decimation_);
+	RCLCPP_INFO(this->get_logger(), "  upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
 
 	auto node = rclcpp::Node::make_shared(this->get_name());
 	image_transport::ImageTransport it(node);
@@ -217,6 +222,11 @@ void PointCloudToDepthImage::callback(
 
 		depthImage.header = cameraInfoMsg->header;
 
+		if(decimation_>1 && upscale_)
+		{
+			depthImage.image = rtabmap::util2d::interpolate(depthImage.image, decimation_, upscaleDepthErrorRatio_);
+		}
+
 		if(depthImage32Pub_.getNumSubscribers())
 		{
 			depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;